diff --git a/main/modes/mickmode.cpp b/main/modes/mickmode.cpp index 9133804..c1b9b64 100644 --- a/main/modes/mickmode.cpp +++ b/main/modes/mickmode.cpp @@ -36,19 +36,35 @@ void MickMode::update() else { auto now = espchrono::millis_clock::now(); - float timeDelta = std::chrono::floor(now - lastUpdate_).count(); + auto timeDelta = std::chrono::floor(now - lastUpdate_).count(); + // Make sure timeDelta can never be negative, otherwise the exponential below may explode + timeDelta = std::max(timeDelta, 0ll); lastUpdate_ = now; float alpha = 1.f - expf(-timeDelta / profileSettings.mickMode.smoothing); + // Fall back to fixed smoothing if this calculation fails + if (!isfinite(alpha) || alpha < 0.f || alpha > 1.f) + alpha = 0.02f; + // Limit time constant to below 3 s at update rate of 20 ms to make stopping possible + // if misconfiguration or corruption happens + else if (alpha < 0.0066f) + alpha = 0.0066f; float gasOf1500 = *gas * 1500.f / 1000.f; float brakeOf1500 = *brems * 1500.f / 1000.f; float controlInput = gasOf1500 - brakeOf1500; + if (!isfinite(controlInput) || controlInput > 1500.f || controlInput < -1500.f) + controlInput = 0.f; pwm_ = pwm_ * (1.f-alpha) + controlInput * alpha; - // Should be in this range anyway + // Should be in this range anyway, but clamp to make sure pwm_ = std::clamp(pwm_, -1500.f, 1500.f); + // This should not happen either, but make sure our state can not get persistently broken + // in case of corruption. Set to 0 as a last resort (may lead to a hard stop). + if (!isfinite(pwm_)) + pwm_ = 0.f; + for (bobbycar::protocol::serial::MotorState &motor : motors()) { const auto pair = split(profileSettings.mickMode.modelMode);