From f9bcbc31aed8aa21d2a5372b50098307b0d9d8cc Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 26 Nov 2024 13:49:30 +0100 Subject: [PATCH] PID: protect from division by zero because of dt Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com> --- src/lib/pid/PID.cpp | 14 ++++++++++++-- src/lib/pid/PID.hpp | 1 + 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/lib/pid/PID.cpp b/src/lib/pid/PID.cpp index 3f0d9b4b93..6fbb346351 100644 --- a/src/lib/pid/PID.cpp +++ b/src/lib/pid/PID.cpp @@ -44,8 +44,7 @@ void PID::setGains(const float P, const float I, const float D) float PID::update(const float feedback, const float dt, const bool update_integral) { const float error = _setpoint - feedback; - const float feedback_change = std::isfinite(_last_feedback) ? (feedback - _last_feedback) / dt : 0.f; - const float output = (_gain_proportional * error) + _integral + (_gain_derivative * feedback_change); + const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt)); if (update_integral) { updateIntegral(error, dt); @@ -63,3 +62,14 @@ void PID::updateIntegral(float error, const float dt) _integral = math::constrain(integral_new, -_limit_integral, _limit_integral); } } + +float PID::updateDerivative(float feedback, const float dt) +{ + float feedback_change = 0.f; + + if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) { + feedback_change = (feedback - _last_feedback) / dt; + } + + return feedback_change; +} diff --git a/src/lib/pid/PID.hpp b/src/lib/pid/PID.hpp index cabdd3701b..615fd60b22 100644 --- a/src/lib/pid/PID.hpp +++ b/src/lib/pid/PID.hpp @@ -50,6 +50,7 @@ public: void resetDerivative() { _last_feedback = NAN; }; private: void updateIntegral(float error, const float dt); + float updateDerivative(float feedback, const float dt); float _setpoint{0.f}; ///< current setpoint to track float _integral{0.f}; ///< integral state