From 3910552a7bf29d62e51ab681a4f9ff6ca55d2cc7 Mon Sep 17 00:00:00 2001 From: Finlay Davidson Date: Sun, 20 Aug 2023 18:29:41 +0200 Subject: [PATCH] raisewake: Fix for new raise to wake impl --- CMakeLists.txt | 2 + InfiniTime | 2 +- sim/components/motion/MotionController.cpp | 107 ++++++++++++++++----- sim/components/motion/MotionController.h | 43 +++++---- 4 files changed, 109 insertions(+), 45 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b039219..11797f3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -211,6 +211,8 @@ target_sources(infinisim PUBLIC ${InfiniTime_DIR}/src/systemtask/SystemTask.cpp ${InfiniTime_DIR}/src/systemtask/SystemMonitor.h ${InfiniTime_DIR}/src/systemtask/SystemMonitor.cpp + ${InfiniTime_DIR}/src/utility/Math.h + ${InfiniTime_DIR}/src/utility/Math.cpp ) target_sources(infinisim PUBLIC diff --git a/InfiniTime b/InfiniTime index 6d0d8c7..2b1eae7 160000 --- a/InfiniTime +++ b/InfiniTime @@ -1 +1 @@ -Subproject commit 6d0d8c7d6373062cd084c170d2e50e1535c9e95d +Subproject commit 2b1eae7f597ea6c210b4c15a73ab5ba116fc5d2a diff --git a/sim/components/motion/MotionController.cpp b/sim/components/motion/MotionController.cpp index da90eae..4309c51 100644 --- a/sim/components/motion/MotionController.cpp +++ b/sim/components/motion/MotionController.cpp @@ -2,25 +2,59 @@ //#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); // } // -// 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(); + lastX = this->x; this->x = x; - lastY = this->y; - this->y = y; - lastZ = this->z; - this->z = z; + yHistory++; + yHistory[0] = y; + zHistory++; + zHistory[0] = z; + + stats = GetAccelStats(); int32_t deltaSteps = nbSteps - this->nbSteps; if (deltaSteps > 0) { @@ -29,35 +63,56 @@ 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; - } +MotionController::AccelStats MotionController::GetAccelStats() const { + AccelStats stats; - if (y >= 0) { - lastYForRaiseWake = 0; - return false; - } - if (y + 230 < lastYForRaiseWake) { - lastYForRaiseWake = y; - return true; - } + 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() const { return false; +// constexpr uint32_t varianceThresh = 56 * 56; +// constexpr int16_t xThresh = 384; +// constexpr int16_t yThresh = -64; +// constexpr int16_t rollDegreesThresh = -45; +// +// if (x < -xThresh || x > xThresh) { +// 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) { return false; // /* 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; -// //(.2 * speed) + ((1 - .2) * accumulatedSpeed); -// // implemented without floats as .25Alpha -// accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4); +// int32_t speed = +// std::abs(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + (x - lastX) / 4) * 100 / (time - lastTime); +// // (.2 * speed) + ((1 - .2) * accumulatedSpeed); +// accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; // // return accumulatedSpeed > thresh; } diff --git a/sim/components/motion/MotionController.h b/sim/components/motion/MotionController.h index fd8610a..00b7474 100644 --- a/sim/components/motion/MotionController.h +++ b/sim/components/motion/MotionController.h @@ -6,12 +6,13 @@ #include "drivers/Bma421.h" //#include "components/ble/MotionService.h" +#include "utility/CircularBuffer.h" namespace Pinetime { namespace Controllers { class MotionController { public: - enum class DeviceTypes{ + enum class DeviceTypes { Unknown, BMA421, BMA425, @@ -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,20 +45,12 @@ namespace Pinetime { } bool ShouldShakeWake(uint16_t thresh); - bool ShouldRaiseWake(bool isSleeping); + bool ShouldRaiseWake() const; int32_t CurrentShakeSpeed() const { return accumulatedSpeed; } - void IsSensorOk(bool isOk) { - isSensorOk = isOk; - } - - bool IsSensorOk() const { - return isSensorOk; - } - DeviceTypes DeviceType() const { return deviceType; } @@ -75,15 +68,29 @@ 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; - int16_t lastY = 0; - int16_t y = 0; - int16_t lastZ = 0; - int16_t z = 0; + static constexpr uint8_t histSize = 8; + Utility::CircularBuffer yHistory = {}; + Utility::CircularBuffer zHistory = {}; int32_t accumulatedSpeed = 0; - bool isSensorOk = false; DeviceTypes deviceType = DeviceTypes::Unknown; // Pinetime::Controllers::MotionService* service = nullptr; };