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 +#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 + +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 + +namespace Pinetime { + namespace Utility { + // returns the arcsin of `arg`. asin(-32767) = -90, asin(32767) = 90 + int16_t Asin(int16_t arg); + } +}