From 2b1eae7f597ea6c210b4c15a73ab5ba116fc5d2a Mon Sep 17 00:00:00 2001
From: Finlay Davidson <finlay.davidson@coderclass.nl>
Date: Sat, 20 May 2023 01:27:00 +0200
Subject: [PATCH] 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.
---
 src/components/motion/MotionController.cpp | 32 ++++++++++------------
 src/components/motion/MotionController.h   |  3 +-
 src/systemtask/SystemTask.cpp              |  2 +-
 3 files changed, 16 insertions(+), 21 deletions(-)

diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index 69e418ce..feb9ead0 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -87,26 +87,22 @@ MotionController::AccelStats MotionController::GetAccelStats() const {
   return stats;
 }
 
-bool MotionController::ShouldRaiseWake(bool isSleeping) {
-  if ((x + 335) <= 670 && zHistory[0] < 0) {
-    if (!isSleeping) {
-      if (yHistory[0] <= 0) {
-        return false;
-      }
-      lastYForRaiseWake = 0;
-      return false;
-    }
+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 (yHistory[0] >= 0) {
-      lastYForRaiseWake = 0;
-      return false;
-    }
-    if (yHistory[0] + 230 < lastYForRaiseWake) {
-      lastYForRaiseWake = yHistory[0];
-      return true;
-    }
+  if (x < -xThresh || x > xThresh) {
+    return false;
   }
-  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) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index de86d44c..1ad032b8 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -45,7 +45,7 @@ namespace Pinetime {
       }
 
       bool ShouldShakeWake(uint16_t thresh);
-      bool ShouldRaiseWake(bool isSleeping);
+      bool ShouldRaiseWake() const;
 
       int32_t CurrentShakeSpeed() const {
         return accumulatedSpeed;
@@ -86,7 +86,6 @@ namespace Pinetime {
 
       int16_t lastX = 0;
       int16_t x = 0;
-      int16_t lastYForRaiseWake = 0;
       static constexpr uint8_t histSize = 8;
       Utility::CircularBuffer<int16_t, histSize> yHistory = {};
       Utility::CircularBuffer<int16_t, histSize> zHistory = {};
diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp
index b199d53f..4719bb50 100644
--- a/src/systemtask/SystemTask.cpp
+++ b/src/systemtask/SystemTask.cpp
@@ -430,7 +430,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();