fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 11:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

PID: protect from division by zero because of dt

Browse Source 
Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>
This commit is contained in:
Matthias Grob 2024-11-26 13:49:30 +01:00
parent b89c53d28c
commit f9bcbc31ae
2 changed files with 13 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/pid/PID.cpp
View File

@ -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;
}

1
src/lib/pid/PID.hpp
View File

@ -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