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

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

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;

libraries/AC_PID/AC_PID.h

@@ -8,6 +8,7 @@
 #include <stdlib.h>
 #include <cmath>
 #include <AP_Logger/AP_Logger.h>
+#include <Filter/SlewLimiter.h>
 
 #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 {