2 files changed, 10 insertions, 7 deletions
diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index aab8b9d4..5fc548be 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -1,6 +1,5 @@
 #include "components/motion/MotionController.h"
 
-#include <FreeRTOS.h>
 #include <task.h>
 
 using namespace Pinetime::Controllers;
@@ -14,6 +13,9 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
     service->OnNewMotionValues(x, y, z);
   }
 
+  lastTime = time;
+  time = xTaskGetTickCount();
+
   this->x = x;
   lastY = this->y;
   this->y = y;
@@ -50,10 +52,8 @@ bool MotionController::Should_RaiseWake(bool isSleeping) {
 }
 
 bool MotionController::ShouldShakeWake(uint16_t thresh) {
-  auto diff = xTaskGetTickCount() - lastShakeTime;
-  lastShakeTime = xTaskGetTickCount();
   /* Currently Polling at 10hz, If this ever goes faster scalar and EMA might need adjusting */
-  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / diff * 100;
+  int32_t speed = std::abs(z + (y / 2) + (x / 4) - lastY / 2 - lastZ) / (time - lastTime) * 100;
   //(.2 * speed) + ((1 - .2) * accumulatedSpeed);
   // implemented without floats as .25Alpha
   accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4);
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index 8fb03d81..126504a3 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -2,6 +2,8 @@
 
 #include <cstdint>
 
+#include <FreeRTOS.h>
+
 #include "drivers/Bma421.h"
 #include "components/ble/MotionService.h"
 
@@ -65,19 +67,20 @@ namespace Pinetime {
       uint32_t nbSteps;
       uint32_t currentTripSteps = 0;
 
+      TickType_t lastTime = 0;
+      TickType_t time = 0;
+
       int16_t x;
       int16_t lastYForWakeUp = 0;
       int16_t lastY = 0;
       int16_t y;
       int16_t lastZ = 0;
       int16_t z;
+      int32_t accumulatedSpeed = 0;
 
       bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
       Pinetime::Controllers::MotionService* service = nullptr;
-
-      int32_t accumulatedSpeed = 0;
-      uint32_t lastShakeTime = 0;
     };
   }
 }