diff options
Diffstat (limited to 'src/components/motion/MotionController.cpp')
| -rw-r--r-- | src/components/motion/MotionController.cpp | 19 |
1 files changed, 8 insertions, 11 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)) { |