Unconditionally calculate shake speed

Author: mark9064

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)) {

src/components/motion/MotionController.h

@@ -44,7 +44,6 @@ namespace Pinetime {
         return currentTripSteps;
       }
 
-      bool ShouldShakeWake(uint16_t thresh);
       bool ShouldRaiseWake() const;
       bool ShouldLowerSleep() const;
 

src/systemtask/SystemTask.cpp

@@ -437,7 +437,7 @@ void SystemTask::UpdateMotion() {
     if ((settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist) &&
          motionController.ShouldRaiseWake()) ||
         (settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::Shake) &&
-         motionController.ShouldShakeWake(settingsController.GetShakeThreshold()))) {
+         motionController.CurrentShakeSpeed() > settingsController.GetShakeThreshold())) {
       GoToRunning();
     }
   }