From 47ca403857a06e6432f36b7e1f722dd9acbfd5af Mon Sep 17 00:00:00 2001 From: Finlay Davidson Date: Wed, 5 Apr 2023 10:32:27 +0200 Subject: [PATCH] shakewake: Slightly improve accuracy The accumulated speed was calculated by dividing first and multiplying after, which results in more rounding errors than if you multiply first and then divide. The values aren't big enough to overflow. --- src/components/motion/MotionController.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp index ef3cf811..3572195b 100644 --- a/src/components/motion/MotionController.cpp +++ b/src/components/motion/MotionController.cpp @@ -54,10 +54,9 @@ bool MotionController::ShouldRaiseWake(bool isSleeping) { bool MotionController::ShouldShakeWake(uint16_t thresh) { /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */ - int32_t speed = std::abs(z - lastZ + (y / 2) - (lastY / 2) + (x / 4) - (lastX / 4)) / (time - lastTime) * 100; - //(.2 * speed) + ((1 - .2) * accumulatedSpeed); - // implemented without floats as .25Alpha - accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4); + int32_t speed = std::abs(z - lastZ + (y - lastY) / 2 + (x - lastX) / 4) * 100 / (time - lastTime); + // (.2 * speed) + ((1 - .2) * accumulatedSpeed); + accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; return accumulatedSpeed > thresh; }