diff --git a/.gitmodules b/.gitmodules index e6518b3d3d..58a8db2773 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,6 @@ [submodule "src/libs/lvgl"] path = src/libs/lvgl - url = https://github.com/InfiniTimeOrg/lvgl.git + url = https://github.com/FintasticMan/lvgl.git [submodule "src/libs/littlefs"] path = src/libs/littlefs url = https://github.com/littlefs-project/littlefs.git diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp index 9d16e00d2e..a62c399858 100644 --- a/src/components/motion/MotionController.cpp +++ b/src/components/motion/MotionController.cpp @@ -1,15 +1,44 @@ #include "components/motion/MotionController.h" +#include #include using namespace Pinetime::Controllers; +namespace { + inline int16_t Clamp(int16_t in, int16_t min, int16_t max) { + return in < min ? min : (in > max ? max : in); + } + + int16_t DegreesRolled(int16_t y, int16_t z, int16_t lastY, int16_t lastZ) { + int16_t lastYAngle = _lv_trigo_asin(Clamp(lastY, -1023, 1023) * 32); + int16_t yAngle = _lv_trigo_asin(Clamp(y, -1023, 1023) * 32); + + if (z < 0 && lastZ < 0) { + return lastYAngle - yAngle; + } + if (lastZ < 0) { + if (y < 0) { + return lastYAngle + yAngle + 180; + } + return lastYAngle + yAngle - 180; + } + if (z < 0) { + if (y < 0) { + return -lastYAngle - yAngle - 180; + } + return -lastYAngle - yAngle + 180; + } + return yAngle - lastYAngle; + } +} + 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); } @@ -17,10 +46,10 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) time = xTaskGetTickCount(); this->x = x; - lastY = this->y; - this->y = y; - lastZ = this->z; - this->z = z; + yHistory++; + yHistory[0] = y; + zHistory++; + zHistory[0] = z; int32_t deltaSteps = nbSteps - this->nbSteps; if (deltaSteps > 0) { @@ -29,31 +58,55 @@ 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; - } +bool MotionController::ShouldRaiseWake() const { + constexpr uint8_t numHistory = 2; + constexpr int32_t varianceThresh = 48 * 48; - if (y >= 0) { - lastYForRaiseWake = 0; - return false; - } - if (y + 230 < lastYForRaiseWake) { - lastYForRaiseWake = y; - return true; - } + if (x < -384 || x > 384) { + return false; + } + + int16_t currentYMean = 0; + int16_t currentZMean = 0; + int16_t prevYMean = 0; + int16_t prevZMean = 0; + for (uint8_t i = 0; i < numHistory; i++) { + currentYMean += yHistory[yHistory.Size() - i]; + currentZMean += zHistory[zHistory.Size() - i]; + prevYMean += yHistory[1 + i]; + prevZMean += zHistory[1 + i]; } - return false; + currentYMean /= numHistory; + currentZMean /= numHistory; + prevYMean /= numHistory; + prevZMean /= numHistory; + + int32_t currentYVariance = 0; + int32_t currentZVariance = 0; + // int32_t prevYVariance = 0; + // int32_t prevZVariance = 0; + for (uint8_t i = 0; i < numHistory; i++) { + currentYVariance += (yHistory[yHistory.Size() - i] - currentYMean) * (yHistory[yHistory.Size() - i] - currentYMean); + currentZVariance += (zHistory[zHistory.Size() - i] - currentZMean) * (zHistory[zHistory.Size() - i] - currentZMean); + // prevYVariance += (yHistory[1 + i] - currentYMean) * (yHistory[1 + i] - currentYMean); + // prevZVariance += (zHistory[1 + i] - currentZMean) * (zHistory[1 + i] - currentZMean); + } + currentYVariance /= numHistory; + currentZVariance /= numHistory; + // prevYVariance /= numHistory; + // prevZVariance /= numHistory; + + if (currentYVariance > varianceThresh || (currentYMean < -724 && currentZVariance > varianceThresh) || currentYMean > 0) { + return false; + } + + return DegreesRolled(currentYMean, currentZMean, prevYMean, prevZMean) > 45; } bool MotionController::ShouldShakeWake(uint16_t thresh) { /* 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; + int32_t speed = std::abs(zHistory[0] + (yHistory[0] / 2) + (x / 4) - yHistory[yHistory.Size() - 1] / 2 - zHistory[zHistory.Size() - 1]) / + (time - lastTime) * 100; //(.2 * speed) + ((1 - .2) * accumulatedSpeed); // implemented without floats as .25Alpha accumulatedSpeed = (speed / 5) + ((accumulatedSpeed / 5) * 4); diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h index eb8d04aa59..b21eb0e5b4 100644 --- a/src/components/motion/MotionController.h +++ b/src/components/motion/MotionController.h @@ -6,6 +6,7 @@ #include "drivers/Bma421.h" #include "components/ble/MotionService.h" +#include "utility/CircularBuffer.h" namespace Pinetime { namespace Controllers { @@ -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,7 +45,7 @@ namespace Pinetime { } bool ShouldShakeWake(uint16_t thresh); - bool ShouldRaiseWake(bool isSleeping); + bool ShouldRaiseWake() const; int32_t CurrentShakeSpeed() const { return accumulatedSpeed; @@ -76,11 +77,8 @@ namespace Pinetime { TickType_t time = 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; + Utility::CircularBuffer yHistory = {}; + Utility::CircularBuffer zHistory = {}; int32_t accumulatedSpeed = 0; bool isSensorOk = false; diff --git a/src/libs/lvgl b/src/libs/lvgl index 7c96fb87e3..6411a35627 160000 --- a/src/libs/lvgl +++ b/src/libs/lvgl @@ -1 +1 @@ -Subproject commit 7c96fb87e33733a7a67b33bf8057e598c6843e3a +Subproject commit 6411a35627e2d87eda0677dea11c4a248870d6ec diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp index 539ff89568..c4df8e6214 100644 --- a/src/systemtask/SystemTask.cpp +++ b/src/systemtask/SystemTask.cpp @@ -440,7 +440,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();