InfiniTime.git

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1c6e8e6332f8f00cd7e034a9bea586dbd18f2bac..8e8e96863ba6b3e61d5baf704a51c435cdd1adaa 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -493,6 +493,8 @@         components/heartrate/Ppg.cpp
 
         buttonhandler/ButtonHandler.cpp
         touchhandler/TouchHandler.cpp
+
+        utility/Math.cpp
         )
 
 list(APPEND RECOVERY_SOURCE_FILES
@@ -558,6 +560,8 @@         components/motor/MotorController.cpp
         components/fs/FS.cpp
         buttonhandler/ButtonHandler.cpp
         touchhandler/TouchHandler.cpp
+
+        utility/Math.cpp
         )
 
 list(APPEND RECOVERYLOADER_SOURCE_FILES
@@ -677,6 +681,7 @@         libs/arduinoFFT/src/types.h
         components/motor/MotorController.h
         buttonhandler/ButtonHandler.h
         touchhandler/TouchHandler.h
+        utility/Math.h
         )
 
 include_directories(




diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp
index b2643a7c33ca71611cf09264dcc4fff221fe4d53..69e418ce58c6479bff99c84ef26460bac4043667 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -2,8 +2,39 @@ #include "components/motion/MotionController.h"
 
 #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);
@@ -23,11 +54,37 @@   yHistory[0] = y;
   zHistory++;
   zHistory[0] = z;
 
+  stats = GetAccelStats();
+
   int32_t deltaSteps = nbSteps - this->nbSteps;
   if (deltaSteps > 0) {
     currentTripSteps += deltaSteps;
   }
   this->nbSteps = nbSteps;
+}
+
+MotionController::AccelStats MotionController::GetAccelStats() const {
+  AccelStats stats;
+
+  for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    stats.yMean += yHistory[histSize - i];
+    stats.zMean += zHistory[histSize - i];
+    stats.prevYMean += yHistory[1 + i];
+    stats.prevZMean += zHistory[1 + i];
+  }
+  stats.yMean /= AccelStats::numHistory;
+  stats.zMean /= AccelStats::numHistory;
+  stats.prevYMean /= AccelStats::numHistory;
+  stats.prevZMean /= AccelStats::numHistory;
+
+  for (uint8_t i = 0; i < AccelStats::numHistory; i++) {
+    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.yVariance /= AccelStats::numHistory;
+  stats.zVariance /= AccelStats::numHistory;
+
+  return stats;
 }
 
 bool MotionController::ShouldRaiseWake(bool isSleeping) {




diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index c967530b5158e93fbd3b74b818644245bf63d7fc..de86d44c3ee7745a788ec8fb37fb2ecb0a2d2415 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -68,6 +68,22 @@
       TickType_t lastTime = 0;
       TickType_t time = 0;
 
+      struct AccelStats {
+        static constexpr uint8_t numHistory = 2;
+
+        int16_t yMean = 0;
+        int16_t zMean = 0;
+        int16_t prevYMean = 0;
+        int16_t prevZMean = 0;
+
+        uint32_t yVariance = 0;
+        uint32_t zVariance = 0;
+      };
+
+      AccelStats GetAccelStats() const;
+
+      AccelStats stats = {};
+
       int16_t lastX = 0;
       int16_t x = 0;
       int16_t lastYForRaiseWake = 0;




diff --git a/src/utility/Math.cpp b/src/utility/Math.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fee4f64af3ad3c9199f603ab7fe9d480f6fed647
--- /dev/null
+++ b/src/utility/Math.cpp
@@ -0,0 +1,49 @@
+#include "utility/Math.h"
+
+#include <lvgl/src/lv_misc/lv_math.h>
+
+using namespace Pinetime::Utility;
+
+#ifndef PINETIME_IS_RECOVERY
+
+int16_t Pinetime::Utility::Asin(int16_t arg) {
+  int16_t a = arg < 0 ? -arg : arg;
+
+  int16_t angle = 45;
+  int16_t low = 0;
+  int16_t high = 90;
+  while (low <= high) {
+    int16_t sinAngle = _lv_trigo_sin(angle);
+    int16_t sinAngleSub = _lv_trigo_sin(angle - 1);
+    int16_t sinAngleAdd = _lv_trigo_sin(angle + 1);
+
+    if (a >= sinAngleSub && a <= sinAngleAdd) {
+      if (a <= (sinAngleSub + sinAngle) / 2) {
+        angle--;
+      } else if (a > (sinAngle + sinAngleAdd) / 2) {
+        angle++;
+      }
+      break;
+    }
+
+    if (a < sinAngle) {
+      high = angle - 1;
+    }
+
+    else {
+      low = angle + 1;
+    }
+
+    angle = (low + high) / 2;
+  }
+
+  return arg < 0 ? -angle : angle;
+}
+
+#else
+
+int16_t Pinetime::Utility::Asin(int16_t /*arg*/) {
+  return 0;
+}
+
+#endif




diff --git a/src/utility/Math.h b/src/utility/Math.h
new file mode 100644
index 0000000000000000000000000000000000000000..e8d190c72dcd31df104d791353f02deb86a1f33f
--- /dev/null
+++ b/src/utility/Math.h
@@ -0,0 +1,10 @@
+#pragma once
+
+#include <cstdint>
+
+namespace Pinetime {
+  namespace Utility {
+    // returns the arcsin of `arg`. asin(-32767) = -90, asin(32767) = 90
+    int16_t Asin(int16_t arg);
+  }
+}