diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 8cd404ba8f..49537e5eb5 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -37,6 +37,13 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] PROGMEM = { // @Description: AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT), + // @Param: I_L_MIN + // @DisplayName: I-term Leak Minimum + // @Description: Point below which I-term will not leak down + // @Range: 0 4500 + // @User: Advanced + AP_GROUPINFO("I_L_MIN", 7, AC_HELI_PID, _leak_min, AC_PID_LEAK_MIN), + AP_GROUPEND }; @@ -59,7 +66,14 @@ float AC_HELI_PID::get_vff(float requested_rate) float AC_HELI_PID::get_leaky_i(float leak_rate) { if(!is_zero(_ki) && !is_zero(_dt)){ - _integrator -= (float)_integrator * leak_rate; + + // integrator does not leak down below Leak Min + if (_integrator > _leak_min){ + _integrator -= (float)(_integrator - _leak_min) * leak_rate; + } else if (_integrator < -_leak_min) { + _integrator -= (float)(_integrator + _leak_min) * leak_rate; + } + _integrator += ((float)_input * _ki) * _dt; if (_integrator < -_imax) { _integrator = -_imax; diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h index 38dc5ff35f..d9da4de464 100644 --- a/libraries/AC_PID/AC_HELI_PID.h +++ b/libraries/AC_PID/AC_HELI_PID.h @@ -12,6 +12,8 @@ #include #include "AC_PID.h" +#define AC_PID_LEAK_MIN 500.0 // Default I-term Leak Minimum + /// @class AC_HELI_PID /// @brief Heli PID control class class AC_HELI_PID : public AC_PID { @@ -34,6 +36,7 @@ public: private: AP_Float _vff; + AP_Float _leak_min; };