diff --git a/src/components/motion/MotionController.cpp b/src/components/motion/MotionController.cpp index 6cfff61f9d..4053f401ea 100644 --- a/src/components/motion/MotionController.cpp +++ b/src/components/motion/MotionController.cpp @@ -56,9 +56,12 @@ void MotionController::Update(int16_t x, int16_t y, int16_t z, uint32_t nbSteps) // 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); + int32_t speed = 0; + if (time != lastTime) { + 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;