raisewake: Improve raise to wake algorithm

This new algorithm calculates the number of degrees that the wrist has
rolled, and checks if that is above a threshold.

First it checks if the wrist is still enough for the acceleration values
to be considered mostly from gravity. It does this by calculating the
mean over the past 2 SystemTask loops, and checking that the variance
from that mean is below a threshold.
Then it calculates the angle the wrist is being held at, and calculates
the difference from the angle some time ago. If this difference is above
the threshold, it wakes the watch.

.gitmodules

@@ -1,6 +1,6 @@
 [submodule "src/libs/lvgl"]
 	path = src/libs/lvgl
-	url = https://github.com/InfiniTimeOrg/lvgl.git
+	url = https://github.com/FintasticMan/lvgl.git
 [submodule "src/libs/littlefs"]
 	path = src/libs/littlefs
 	url = https://github.com/littlefs-project/littlefs.git

src/components/motion/MotionController.cpp

@@ -1,26 +1,55 @@
 #include "components/motion/MotionController.h"
 
+#include <lvgl/src/lv_misc/lv_math.h>
 #include <task.h>
 
 using namespace Pinetime::Controllers;
 
+namespace {
+  inline int16_t Clamp(int16_t in, int16_t min, int16_t max) {
+    return in < min ? min : (in > max ? max : in);
+  }
+
+  int16_t DegreesRolled(int16_t y, int16_t z, int16_t lastY, int16_t lastZ) {
+    int16_t lastYAngle = _lv_trigo_asin(Clamp(lastY, -1023, 1023) * 32);
+    int16_t yAngle = _lv_trigo_asin(Clamp(y, -1023, 1023) * 32);
+
+    if (z < 0 && lastZ < 0) {
+      return lastYAngle - yAngle;
+    }
+    if (lastZ < 0) {
+      if (y < 0) {
+        return lastYAngle + yAngle + 180;
+      }
+      return lastYAngle + yAngle - 180;
+    }
+    if (z < 0) {
+      if (y < 0) {
+        return -lastYAngle - yAngle - 180;
+      }
+      return -lastYAngle - yAngle + 180;
+    }
+    return yAngle - lastYAngle;
+  }
+}
+
 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 && (this->x != 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;
+  yHistory++;
+  yHistory[0] = y;
+  zHistory++;
+  zHistory[0] = z;
 
   int32_t deltaSteps = nbSteps - this->nbSteps;
   if (deltaSteps > 0) {
@@ -29,31 +58,55 @@ 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;
-    }
+bool MotionController::ShouldRaiseWake() const {
+  constexpr uint8_t numHistory = 2;
+  constexpr int32_t varianceThresh = 48 * 48;
 
-    if (y >= 0) {
-      lastYForRaiseWake = 0;
-      return false;
-    }
-    if (y + 230 < lastYForRaiseWake) {
-      lastYForRaiseWake = y;
-      return true;
-    }
+  if (x < -384 || x > 384) {
+    return false;
+  }
+
+  int16_t currentYMean = 0;
+  int16_t currentZMean = 0;
+  int16_t prevYMean = 0;
+  int16_t prevZMean = 0;
+  for (uint8_t i = 0; i < numHistory; i++) {
+    currentYMean += yHistory[yHistory.Size() - i];
+    currentZMean += zHistory[zHistory.Size() - i];
+    prevYMean += yHistory[1 + i];
+    prevZMean += zHistory[1 + i];
   }
-  return false;
+  currentYMean /= numHistory;
+  currentZMean /= numHistory;
+  prevYMean /= numHistory;
+  prevZMean /= numHistory;
+
+  int32_t currentYVariance = 0;
+  int32_t currentZVariance = 0;
+  // int32_t prevYVariance = 0;
+  // int32_t prevZVariance = 0;
+  for (uint8_t i = 0; i < numHistory; i++) {
+    currentYVariance += (yHistory[yHistory.Size() - i] - currentYMean) * (yHistory[yHistory.Size() - i] - currentYMean);
+    currentZVariance += (zHistory[zHistory.Size() - i] - currentZMean) * (zHistory[zHistory.Size() - i] - currentZMean);
+    // prevYVariance += (yHistory[1 + i] - currentYMean) * (yHistory[1 + i] - currentYMean);
+    // prevZVariance += (zHistory[1 + i] - currentZMean) * (zHistory[1 + i] - currentZMean);
+  }
+  currentYVariance /= numHistory;
+  currentZVariance /= numHistory;
+  // prevYVariance /= numHistory;
+  // prevZVariance /= numHistory;
+
+  if (currentYVariance > varianceThresh || (currentYMean < -724 && currentZVariance > varianceThresh) || currentYMean > 0) {
+    return false;
+  }
+
+  return DegreesRolled(currentYMean, currentZMean, prevYMean, prevZMean) > 45;
 }
 
 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;
+  int32_t speed = std::abs(zHistory[0] + (yHistory[0] / 2) + (x / 4) - yHistory[yHistory.Size() - 1] / 2 - zHistory[zHistory.Size() - 1]) /
+                  (time - lastTime) * 100;
   //(.2 * speed) + ((1 - .2) * accumulatedSpeed);
   // implemented without floats as .25Alpha
   accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4);

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 {
@@ -24,11 +25,11 @@ namespace Pinetime {
       }
 
       int16_t Y() const {
-        return y;
+        return yHistory[0];
       }
 
       int16_t Z() const {
-        return z;
+        return zHistory[0];
       }
 
       uint32_t NbSteps() const {
@@ -44,7 +45,7 @@ namespace Pinetime {
       }
 
       bool ShouldShakeWake(uint16_t thresh);
-      bool ShouldRaiseWake(bool isSleeping);
+      bool ShouldRaiseWake() const;
 
       int32_t CurrentShakeSpeed() const {
         return accumulatedSpeed;
@@ -76,11 +77,8 @@ namespace Pinetime {
       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;
+      Utility::CircularBuffer<int16_t, 8> yHistory = {};
+      Utility::CircularBuffer<int16_t, 8> zHistory = {};
       int32_t accumulatedSpeed = 0;
 
       bool isSensorOk = false;

src/systemtask/SystemTask.cpp

@@ -440,7 +440,7 @@ void SystemTask::UpdateMotion() {
 
   if (settingsController.GetNotificationStatus() != Controllers::Settings::Notification::Sleep) {
     if ((settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist) &&
-         motionController.ShouldRaiseWake(state == SystemTaskState::Sleeping)) ||
+         motionController.ShouldRaiseWake()) ||
         (settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::Shake) &&
          motionController.ShouldShakeWake(settingsController.GetShakeThreshold()))) {
       GoToRunning();