aboutsummaryrefslogtreecommitdiffstats
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/CMakeLists.txt5
-rw-r--r--src/components/motion/MotionController.cpp57
-rw-r--r--src/components/motion/MotionController.h16
-rw-r--r--src/utility/Math.cpp49
-rw-r--r--src/utility/Math.h10
5 files changed, 137 insertions, 0 deletions
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1c6e8e63..8e8e9686 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -493,6 +493,8 @@ list(APPEND SOURCE_FILES
buttonhandler/ButtonHandler.cpp
touchhandler/TouchHandler.cpp
+
+ utility/Math.cpp
)
list(APPEND RECOVERY_SOURCE_FILES
@@ -558,6 +560,8 @@ list(APPEND RECOVERY_SOURCE_FILES
components/fs/FS.cpp
buttonhandler/ButtonHandler.cpp
touchhandler/TouchHandler.cpp
+
+ utility/Math.cpp
)
list(APPEND RECOVERYLOADER_SOURCE_FILES
@@ -677,6 +681,7 @@ set(INCLUDE_FILES
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 b2643a7c..69e418ce 100644
--- a/src/components/motion/MotionController.cpp
+++ b/src/components/motion/MotionController.cpp
@@ -2,8 +2,39 @@
#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,6 +54,8 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
zHistory++;
zHistory[0] = z;
+ stats = GetAccelStats();
+
int32_t deltaSteps = nbSteps - this->nbSteps;
if (deltaSteps > 0) {
currentTripSteps += deltaSteps;
@@ -30,6 +63,30 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps)
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) {
if ((x + 335) <= 670 && zHistory[0] < 0) {
if (!isSleeping) {
diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h
index c967530b..de86d44c 100644
--- a/src/components/motion/MotionController.h
+++ b/src/components/motion/MotionController.h
@@ -68,6 +68,22 @@ namespace Pinetime {
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 00000000..fee4f64a
--- /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 00000000..e8d190c7
--- /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);
+ }
+}
generated by cgit v1.2.3-70-g09d2 (git 2.51.2) at 2026-01-12 09:05:32 +0000