diff options
| -rw-r--r-- | src/components/motion/MotionController.cpp | 19 | ||||
| -rw-r--r-- | src/components/motion/MotionController.h | 1 | ||||
| -rw-r--r-- | src/systemtask/SystemTask.cpp | 2 |
3 files changed, 9 insertions, 13 deletions
diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp index 72507ac5..6cfff61f 100644 --- a/src/components/motion/MotionController.cpp +++ b/src/components/motion/MotionController.cpp @@ -54,6 +54,14 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) zHistory++; zHistory[0] = z; + // Update accumulated speed + // Currently polling at 10Hz, if this ever goes faster scalar and EMA might need adjusting + int32_t speed = std::abs(zHistory[0] - zHistory[histSize - 1] + ((yHistory[0] - yHistory[histSize - 1]) / 2) + + ((xHistory[0] - xHistory[histSize - 1]) / 4)) * + 100 / (time - lastTime); + // integer version of (.2 * speed) + ((1 - .2) * accumulatedSpeed); + accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; + stats = GetAccelStats(); int32_t deltaSteps = nbSteps - this->nbSteps; @@ -111,17 +119,6 @@ bool MotionController::ShouldRaiseWake() const { return DegreesRolled(stats.yMean, stats.zMean, stats.prevYMean, stats.prevZMean) < rollDegreesThresh; } -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(zHistory[0] - zHistory[histSize - 1] + (yHistory[0] - yHistory[histSize - 1]) / 2 + - (xHistory[0] - xHistory[histSize - 1]) / 4) * - 100 / (time - lastTime); - // (.2 * speed) + ((1 - .2) * accumulatedSpeed); - accumulatedSpeed = speed / 5 + accumulatedSpeed * 4 / 5; - - return accumulatedSpeed > thresh; -} - bool MotionController::ShouldLowerSleep() const { if ((stats.xMean > 887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) > 30) || (stats.xMean < -887 && DegreesRolled(stats.xMean, stats.zMean, stats.prevXMean, stats.prevZMean) < -30)) { diff --git a/src/components/motion/MotionController.h b/src/components/motion/MotionController.h index be0241d3..ad95f31f 100644 --- a/src/components/motion/MotionController.h +++ b/src/components/motion/MotionController.h @@ -44,7 +44,6 @@ namespace Pinetime { return currentTripSteps; } - bool ShouldShakeWake(uint16_t thresh); bool ShouldRaiseWake() const; bool ShouldLowerSleep() const; diff --git a/src/systemtask/SystemTask.cpp b/src/systemtask/SystemTask.cpp index 0a38f03f..8c979f39 100644 --- a/src/systemtask/SystemTask.cpp +++ b/src/systemtask/SystemTask.cpp @@ -437,7 +437,7 @@ void SystemTask::UpdateMotion() { if ((settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::RaiseWrist) && motionController.ShouldRaiseWake()) || (settingsController.isWakeUpModeOn(Pinetime::Controllers::Settings::WakeUpMode::Shake) && - motionController.ShouldShakeWake(settingsController.GetShakeThreshold()))) { + motionController.CurrentShakeSpeed() > settingsController.GetShakeThreshold())) { GoToRunning(); } } |