From 739410953b630a36b3ff1b0c2a3f3baf52235063 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 24 Sep 2020 13:28:24 +1000 Subject: [PATCH] AC_PID: added slew limiter AC_PID this adds the fixed wing slew limiter as an optional part of AC_PID. It allows the user to configure a maximum slew limit to prevent oscillations in PIDs when gains are too high --- libraries/AC_PID/AC_HELI_PID.cpp | 8 ++++++++ libraries/AC_PID/AC_PID.cpp | 25 ++++++++++++++++++++++++- libraries/AC_PID/AC_PID.h | 13 ++++++++++++- 3 files changed, 44 insertions(+), 2 deletions(-) diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 4a5db6ef34..0101186b13 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -61,6 +61,14 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = { // @Units: Hz AP_GROUPINFO("FLTD", 11, AC_HELI_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT), + // @Param: SMAX + // @DisplayName: Slew rate limit + // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature. + // @Range: 0 200 + // @Increment: 0.5 + // @User: Advanced + AP_GROUPINFO("SMAX", 12, AC_HELI_PID, _slew_rate_max, 0), + AP_GROUPEND }; diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 5d4cb202db..1805c12bd3 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -56,11 +56,20 @@ const AP_Param::GroupInfo AC_PID::var_info[] = { // @Units: Hz AP_GROUPINFO("FLTD", 11, AC_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT), + // @Param: SMAX + // @DisplayName: Slew rate limit + // @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature. + // @Range: 0 200 + // @Increment: 0.5 + // @User: Advanced + AP_GROUPINFO("SMAX", 12, AC_PID, _slew_rate_max, 0), + AP_GROUPEND }; // Constructor -AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) : +AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, + float dt, float initial_srmax, float initial_srtau): _dt(dt), _integrator(0.0f), _error(0.0f), @@ -77,6 +86,8 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ filt_T_hz(initial_filt_T_hz); filt_E_hz(initial_filt_E_hz); filt_D_hz(initial_filt_D_hz); + _slew_rate_max.set(initial_srmax); + _slew_rate_tau.set(initial_srtau); // reset input filter to first value received _flags._reset_filter = true; @@ -144,6 +155,12 @@ float AC_PID::update_all(float target, float measurement, bool limit) float P_out = (_error * _kp); float D_out = (_derivative * _kd); + // calculate slew limit modifier for P+D + _pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); + + P_out *= _pid_info.Dmod; + D_out *= _pid_info.Dmod; + _pid_info.target = _target; _pid_info.actual = measurement; _pid_info.error = _error; @@ -190,6 +207,12 @@ float AC_PID::update_error(float error, bool limit) float P_out = (_error * _kp); float D_out = (_derivative * _kd); + // calculate slew limit modifier for P+D + _pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt); + + P_out *= _pid_info.Dmod; + D_out *= _pid_info.Dmod; + _pid_info.target = 0.0f; _pid_info.actual = 0.0f; _pid_info.error = _error; diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index b83df07aad..400e9cde6b 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -8,6 +8,7 @@ #include #include #include +#include #define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency #define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency @@ -19,7 +20,8 @@ class AC_PID { public: // Constructor for PID - AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt); + AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, + float dt, float initial_srmax=0, float initial_srtau=1.0); // set_dt - set time step in seconds void set_dt(float dt); @@ -105,6 +107,12 @@ public: // parameter var table static const struct AP_Param::GroupInfo var_info[]; + // the time constant tau is not currently configurable, but is set + // as an AP_Float to make it easy to make it configurable for a + // single user of AC_PID by adding the parameter in the param + // table of the parent class. It is made public for this reason + AP_Float _slew_rate_tau; + protected: // parameters @@ -116,6 +124,9 @@ protected: AP_Float _filt_T_hz; // PID target filter frequency in Hz AP_Float _filt_E_hz; // PID error filter frequency in Hz AP_Float _filt_D_hz; // PID derivative filter frequency in Hz + AP_Float _slew_rate_max; + + SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau}; // flags struct ac_pid_flags {