Unconditionally calculate shake speed

1 files changed, 8 insertions, 11 deletions

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 72507ac5..6cfff61f 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -54,6 +54,14 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
   zHistory++;
   zHistory[0] = z;
 
+  // Update accumulated speed
+  // Currently polling at 10Hz, if this ever goes faster scalar and EMA might need adjusting
+  int32_t speed = std::abs(zHistory[0] - zHistory[histSize - 1] + ((yHistory[0] - yHistory[histSize - 1]) / 2) +
+                           ((xHistory[0] - xHistory[histSize - 1]) / 4)) *
+                  100 / (time - lastTime);
+  // integer version of (.2 * speed) + ((1 - .2) * accumulatedSpeed);
+  accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
+
   stats = GetAccelStats();
 
   int32_t deltaSteps = nbSteps - this->nbSteps;
@@ -111,17 +119,6 @@ bool MotionController::ShouldRaiseWake() const {
   return DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < rollDegreesThresh;
 }
 
-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(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 +
-                           (xHistory[0] - xHistory[histSize - 1]) / 4) *
-                  100 / (time - lastTime);
-  // (.2 * speed) + ((1 - .2) * accumulatedSpeed);
-  accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5;
-
-  return accumulatedSpeed > thresh;
-}
-
 bool MotionController::ShouldLowerSleep() const {
   if ((stats.xMean > 887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) > 30) ||
       (stats.xMean < -887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) < -30)) {