InfiniTime.git

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index d28378d5422f89b2e75e8933a4bd12817bbe4891..72507ac5cd459e193651bec44eca42507ec3469c 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -40,15 +40,15 @@   if (this->nbSteps != nbSteps && service != nullptr) {
     service->OnNewStepCountValue(nbSteps);
   }
 
-  if (service != nullptr && (this->x != x || yHistory[0] != y || zHistory[0] != z)) {
+  if (service != nullptr && (xHistory[0] != x || yHistory[0] != y || zHistory[0] != z)) {
     service->OnNewMotionValues(x, y, z);
   }
 
   lastTime = time;
   time = xTaskGetTickCount();
 
-  lastX = this->x;
-  this->x = x;
+  xHistory++;
+  xHistory[0] = x;
   yHistory++;
   yHistory[0] = y;
   zHistory++;
@@ -67,20 +67,26 @@ MotionController::AccelStats MotionController::GetAccelStats() const {
   AccelStats stats;
 
   for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    stats.xMean += xHistory[histSize - i];
     stats.yMean += yHistory[histSize - i];
     stats.zMean += zHistory[histSize - i];
+    stats.prevXMean += xHistory[1 + i];
     stats.prevYMean += yHistory[1 + i];
     stats.prevZMean += zHistory[1 + i];
   }
+  stats.xMean /= AccelStats::numHistory;
   stats.yMean /= AccelStats::numHistory;
   stats.zMean /= AccelStats::numHistory;
+  stats.prevXMean /= AccelStats::numHistory;
   stats.prevYMean /= AccelStats::numHistory;
   stats.prevZMean /= AccelStats::numHistory;
 
   for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    stats.xVariance += (xHistory[histSize - i] - stats.xMean) * (xHistory[histSize - i] - stats.xMean);
     stats.yVariance += (yHistory[histSize - i] - stats.yMean) * (yHistory[histSize - i] - stats.yMean);
     stats.zVariance += (zHistory[histSize - i] - stats.zMean) * (zHistory[histSize - i] - stats.zMean);
   }
+  stats.xVariance /= AccelStats::numHistory;
   stats.yVariance /= AccelStats::numHistory;
   stats.zVariance /= AccelStats::numHistory;
 
@@ -93,7 +99,7 @@   constexpr int16_t xThresh = 384;
   constexpr int16_t yThresh = -64;
   constexpr int16_t rollDegreesThresh = -45;
 
-  if (x < -xThresh || x > xThresh) {
+  if (std::abs(stats.xMean) > xThresh) {
     return false;
   }
 
@@ -107,8 +113,9 @@ }
 
 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 + (x - lastX) / 4) * 100 / (time - lastTime);
+  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;
 
@@ -116,6 +123,11 @@   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)) {
+    return true;
+  }
+
   if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) {
     return false;
   }




diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index b2e7e7fe648b3efab916b6b4fcd634fb59fab754..be0241d32e16ff41e0a3784da5e1f2b4e01478a7 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -21,7 +21,7 @@
       void Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps);
 
       int16_t X() const {
-        return x;
+        return xHistory[0];
       }
 
       int16_t Y() const {
@@ -76,11 +76,14 @@
       struct AccelStats {
         static constexpr uint8_t numHistory = 2;
 
+        int16_t xMean = 0;
         int16_t yMean = 0;
         int16_t zMean = 0;
+        int16_t prevXMean = 0;
         int16_t prevYMean = 0;
         int16_t prevZMean = 0;
 
+        uint32_t xVariance = 0;
         uint32_t yVariance = 0;
         uint32_t zVariance = 0;
       };
@@ -89,9 +92,8 @@       AccelStats GetAccelStats() const;
 
       AccelStats stats = {};
 
-      int16_t lastX = 0;
-      int16_t x = 0;
       static constexpr uint8_t histSize = 8;
+      Utility::CircularBuffer<int16_t, histSize> xHistory = {};
       Utility::CircularBuffer<int16_t, histSize> yHistory = {};
       Utility::CircularBuffer<int16_t, histSize> zHistory = {};
       int32_t accumulatedSpeed = 0;