2 files changed, 141 insertions, 47 deletions

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 9d16e00d..72507ac5 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -2,25 +2,59 @@
 
 #include <task.h>
 
+#include "utility/Math.h"
+
 using namespace Pinetime::Controllers;
 
+namespace {
+  constexpr inline int32_t Clamp(int32_t val, int32_t min, int32_t max) {
+    return val < min ? min : (val > max ? max : val);
+  }
+
+  // only returns meaningful values if inputs are acceleration due to gravity
+  int16_t DegreesRolled(int16_t y, int16_t z, int16_t prevY, int16_t prevZ) {
+    int16_t prevYAngle = Pinetime::Utility::Asin(Clamp(prevY * 32, -32767, 32767));
+    int16_t yAngle = Pinetime::Utility::Asin(Clamp(y * 32, -32767, 32767));
+
+    if (z < 0 && prevZ < 0) {
+      return yAngle - prevYAngle;
+    }
+    if (prevZ < 0) {
+      if (y < 0) {
+        return -prevYAngle - yAngle - 180;
+      }
+      return -prevYAngle - yAngle + 180;
+    }
+    if (z < 0) {
+      if (y < 0) {
+        return prevYAngle + yAngle + 180;
+      }
+      return prevYAngle + yAngle - 180;
+    }
+    return prevYAngle - yAngle;
+  }
+}
+
 void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) {
   if (this->nbSteps != nbSteps && service != nullptr) {
     service->OnNewStepCountValue(nbSteps);
   }
 
-  if (service != nullptr && (this->x != x || this->y != y || this->z != z)) {
+  if (service != nullptr && (xHistory[0] != x || yHistory[0] != y || zHistory[0] != z)) {
     service->OnNewMotionValues(x, y, z);
   }
 
   lastTime = time;
   time = xTaskGetTickCount();
 
-  this->x = x;
-  lastY = this->y;
-  this->y = y;
-  lastZ = this->z;
-  this->z = z;
+  xHistory++;
+  xHistory[0] = x;
+  yHistory++;
+  yHistory[0] = y;
+  zHistory++;
+  zHistory[0] = z;
+
+  stats = GetAccelStats();
 
   int32_t deltaSteps = nbSteps - this->nbSteps;
   if (deltaSteps > 0) {
@@ -29,38 +63,84 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
   this->nbSteps = nbSteps;
 }
 
-bool MotionController::ShouldRaiseWake(bool isSleeping) {
-  if ((x + 335) <= 670 && z < 0) {
-    if (!isSleeping) {
-      if (y <= 0) {
-        return false;
-      }
-      lastYForRaiseWake = 0;
-      return false;
-    }
+MotionController::AccelStats MotionController::GetAccelStats() const {
+  AccelStats stats;
 
-    if (y >= 0) {
-      lastYForRaiseWake = 0;
-      return false;
-    }
-    if (y + 230 < lastYForRaiseWake) {
-      lastYForRaiseWake = y;
-      return true;
-    }
+  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];
   }
-  return false;
+  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;
+
+  return stats;
+}
+
+bool MotionController::ShouldRaiseWake() const {
+  constexpr uint32_t varianceThresh = 56 * 56;
+  constexpr int16_t xThresh = 384;
+  constexpr int16_t yThresh = -64;
+  constexpr int16_t rollDegreesThresh = -45;
+
+  if (std::abs(stats.xMean) > xThresh) {
+    return false;
+  }
+
+  // if the variance is below the threshold, the accelerometer values can be considered to be from acceleration due to gravity
+  if (stats.yVariance > varianceThresh || (stats.yMean < -724 && stats.zVariance > varianceThresh) || stats.yMean > yThresh) {
+    return false;
+  }
+
+  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(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);
+  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)) {
+    return true;
+  }
+
+  if (stats.yMean < 724 || DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < 30) {
+    return false;
+  }
+
+  for (uint8_t i = AccelStats::numHistory + 1; i < yHistory.Size(); i++) {
+    if (yHistory[i] < 265) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
 void MotionController::Init(Pinetime::Drivers::Bma421::DeviceTypes types) {
   switch (types) {
     case Drivers::Bma421::DeviceTypes::BMA421:
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index eb8d04aa..be0241d3 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -6,6 +6,7 @@
 
 #include "drivers/Bma421.h"
 #include "components/ble/MotionService.h"
+#include "utility/CircularBuffer.h"
 
 namespace Pinetime {
   namespace Controllers {
@@ -20,15 +21,15 @@ namespace Pinetime {
       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 {
-        return y;
+        return yHistory[0];
       }
 
       int16_t Z() const {
-        return z;
+        return zHistory[0];
       }
 
       uint32_t NbSteps() const {
@@ -44,20 +45,13 @@ namespace Pinetime {
       }
 
       bool ShouldShakeWake(uint16_t thresh);
-      bool ShouldRaiseWake(bool isSleeping);
+      bool ShouldRaiseWake() const;
+      bool ShouldLowerSleep() const;
 
       int32_t CurrentShakeSpeed() const {
         return accumulatedSpeed;
       }
 
-      void IsSensorOk(bool isOk) {
-        isSensorOk = isOk;
-      }
-
-      bool IsSensorOk() const {
-        return isSensorOk;
-      }
-
       DeviceTypes DeviceType() const {
         return deviceType;
       }
@@ -68,6 +62,10 @@ namespace Pinetime {
         this->service = service;
       }
 
+      Pinetime::Controllers::MotionService* GetService() const {
+        return service;
+      }
+
     private:
       uint32_t nbSteps = 0;
       uint32_t currentTripSteps = 0;
@@ -75,15 +73,31 @@ namespace Pinetime {
       TickType_t lastTime = 0;
       TickType_t time = 0;
 
-      int16_t x = 0;
-      int16_t lastYForRaiseWake = 0;
-      int16_t lastY = 0;
-      int16_t y = 0;
-      int16_t lastZ = 0;
-      int16_t z = 0;
+      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;
+      };
+
+      AccelStats GetAccelStats() const;
+
+      AccelStats stats = {};
+
+      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;
 
-      bool isSensorOk = false;
       DeviceTypes deviceType = DeviceTypes::Unknown;
       Pinetime::Controllers::MotionService* service = nullptr;
     };