From 7ea36e8cacaab1406809851c6234cae61287fc17 Mon Sep 17 00:00:00 2001
From: mark9064 <30447455+mark9064@users.noreply.github.com>
Date: Sat, 28 Jun 2025 20:07:25 +0100
Subject: Unconditionally calculate shake speed

---
 src/components/motion/MotionController.cpp | 19 ++++++++-----------
 src/components/motion/MotionController.h   |  1 -
 src/systemtask/SystemTask.cpp              |  2 +-
 3 files changed, 9 insertions(+), 13 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)) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index be0241d3..ad95f31f 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -44,7 +44,6 @@ namespace Pinetime {
         return currentTripSteps;
       }
 
-      bool ShouldShakeWake(uint16_t thresh);
       bool ShouldRaiseWake() const;
       bool ShouldLowerSleep() const;
 
diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index 0a38f03f..8c979f39 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/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();
     }
   }
-- 
cgit v1.2.3-70-g09d2