feedc0de/bobbycar-boardcomputer-firmware
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #294 from bobbycar-graz/mick-mode-safety

Browse Source
This commit is contained in:
CommanderRedYT
2022-04-03 23:22:47 +02:00
committed by GitHub
parent 255d09b06e 3bd0a4b010
commit 7be54d20c8
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
main/modes/mickmode.cpp
View File

@@ -36,19 +36,35 @@ void MickMode::update()
else else
{ {
auto now = espchrono::millis_clock::now(); auto now = espchrono::millis_clock::now();
float timeDelta = std::chrono::floor<std::chrono::milliseconds>(now - lastUpdate_).count(); auto timeDelta = std::chrono::floor<std::chrono::milliseconds>(now - lastUpdate_).count();
// Make sure timeDelta can never be negative, otherwise the exponential below may explode
timeDelta = std::max(timeDelta, 0ll);
lastUpdate_ = now; lastUpdate_ = now;
float alpha = 1.f - expf(-timeDelta / profileSettings.mickMode.smoothing); 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 gasOf1500 = *gas * 1500.f / 1000.f;
float brakeOf1500 = *brems * 1500.f / 1000.f; float brakeOf1500 = *brems * 1500.f / 1000.f;
float controlInput = gasOf1500 - brakeOf1500; float controlInput = gasOf1500 - brakeOf1500;
if (!isfinite(controlInput) || controlInput > 1500.f || controlInput < -1500.f)
controlInput = 0.f;
pwm_ = pwm_ * (1.f-alpha) + controlInput * alpha; 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); 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()) for (bobbycar::protocol::serial::MotorState &motor : motors())
{ {
const auto pair = split(profileSettings.mickMode.modelMode); const auto pair = split(profileSettings.mickMode.modelMode);