AC_PID: add notch filters to target and error terms

calculate D feed-forward and notch applications correctly
only update notches when enabled
add notches and D feedforward to heli PIDs
add advanced flag an selectively compile advanced options

libraries/AC_PID/AC_HELI_PID.cpp

@@ -69,12 +69,62 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("SMAX", 12, AC_HELI_PID, _slew_rate_max, 0),
 
+#if AC_PID_ADVANCED_ENABLED
+    // @Param: ADV
+    // @DisplayName: Advanced parameters enable
+    // @Description: Advanced parameters enable
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ADV", 32, AC_HELI_PID, _adv_enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: D_FF
+    // @DisplayName: PID Derivative FeedForward Gain
+    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
+    // @Range: 0 0.02
+    // @Increment: 0.0001
+    // @User: Advanced
+    AP_GROUPINFO("D_FF", 33, AC_HELI_PID, _kdff, 0),
+
+    // @Param: NTF
+    // @DisplayName: PID Target notch Filter center frequency
+    // @Description: PID Target notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NTF", 34, AC_HELI_PID, _notch_T_center_freq_hz, 0),
+
+    // @Param: NEF
+    // @DisplayName: PID Error notch Filter center frequency
+    // @Description: PID Error notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NEF", 35, AC_HELI_PID, _notch_E_center_freq_hz, 0),
+
+    // @Param: NBW
+    // @DisplayName: PID notch Filter bandwidth
+    // @Description: PID notch Filter bandwidth in Hz.
+    // @Range: 5 250
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NBW", 36, AC_HELI_PID, _notch_bandwidth_hz, 0),
+
+    // @Param: NATT
+    // @DisplayName: PID notch Filter attenuation
+    // @Description: PID notch Filter attenuation in dB.
+    // @Range: 5 50
+    // @Units: dB
+    // @User: Advanced
+    AP_GROUPINFO("NATT", 37, AC_HELI_PID, _notch_attenuation_dB, 40),
+
+#endif
+
     AP_GROUPEND
 };
 
 /// Constructor for PID
-AC_HELI_PID::AC_HELI_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) :
+AC_HELI_PID::AC_HELI_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 dff_val) :
-    AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz)
+    AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz, dff_val)
 {
     _last_requested_rate = 0;
 }

libraries/AC_PID/AC_HELI_PID.h

@@ -17,7 +17,7 @@ class AC_HELI_PID : public AC_PID {
 public:
 
     /// Constructor for PID
-    AC_HELI_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);
+    AC_HELI_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 dff_val=0);
 
     CLASS_NO_COPY(AC_HELI_PID);
 

libraries/AC_PID/AC_PID.cpp

@@ -4,6 +4,8 @@
 #include <AP_Math/AP_Math.h>
 #include "AC_PID.h"
 
+#define AC_PID_DEFAULT_NOTCH_ATTENUATION 40
+
 const AP_Param::GroupInfo AC_PID::var_info[] = {
     // @Param: P
     // @DisplayName: PID Proportional Gain
@@ -70,12 +72,62 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("PDMX", 13, AC_PID, _kpdmax, 0),
 
+#if AC_PID_ADVANCED_ENABLED
+    // @Param: ADV
+    // @DisplayName: Advanced parameters enable
+    // @Description: Advanced parameters enable
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ADV", 32, AC_PID, _adv_enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: D_FF
+    // @DisplayName: PID Derivative FeedForward Gain
+    // @Description: FF D Gain which produces an output that is proportional to the rate of change of the target
+    // @Range: 0 0.02
+    // @Increment: 0.0001
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D_FF", 33, AC_PID, _kdff, default_kdff),
+
+    // @Param: NTF
+    // @DisplayName: PID Target notch Filter center frequency
+    // @Description: PID Target notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NTF", 34, AC_PID, _notch_T_center_freq_hz, 0),
+
+    // @Param: NEF
+    // @DisplayName: PID Error notch Filter center frequency
+    // @Description: PID Error notch Filter center frequency in Hz.
+    // @Range: 10 495
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NEF", 35, AC_PID, _notch_E_center_freq_hz, 0),
+
+    // @Param: NBW
+    // @DisplayName: PID notch Filter bandwidth
+    // @Description: PID notch Filter bandwidth in Hz.
+    // @Range: 5 250
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("NBW", 36, AC_PID, _notch_bandwidth_hz, 0),
+
+    // @Param: NATT
+    // @DisplayName: PID notch Filter attenuation
+    // @Description: PID notch Filter attenuation in dB.
+    // @Range: 5 50
+    // @Units: dB
+    // @User: Advanced
+    AP_GROUPINFO("NATT", 37, AC_PID, _notch_attenuation_dB, 40),
+
+#endif
+
     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 initial_srmax, float initial_srtau) :
+               float initial_srmax, float initial_srtau, float initial_dff) :
     default_kp(initial_p),
     default_ki(initial_i),
     default_kd(initial_d),
@@ -84,7 +136,8 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
     default_filt_T_hz(initial_filt_T_hz),
     default_filt_E_hz(initial_filt_E_hz),
     default_filt_D_hz(initial_filt_D_hz),
-    default_slew_rate_max(initial_srmax)
+    default_slew_rate_max(initial_srmax),
+    default_kdff(initial_dff)
 {
     // load parameter values from eeprom
     AP_Param::setup_object_defaults(this, var_info);
@@ -126,6 +179,26 @@ void AC_PID::slew_limit(float smax)
     _slew_rate_max.set(fabsf(smax));
 }
 
+void AC_PID::set_notch_sample_rate(float sample_rate)
+{
+#if AC_PID_ADVANCED_ENABLED
+    if (!_adv_enable) {
+        return;
+    }
+
+    if (!is_zero(_notch_T_center_freq_hz.get()) &&
+        (!is_equal(sample_rate, _target_notch.sample_freq_hz())
+         || !is_equal(_notch_T_center_freq_hz.get(), _target_notch.center_freq_hz()))) {
+        _target_notch.init(sample_rate, _notch_T_center_freq_hz, _notch_bandwidth_hz, _notch_attenuation_dB);
+    }
+    if (!is_zero(_notch_E_center_freq_hz.get()) &&
+        (!is_equal(sample_rate, _error_notch.sample_freq_hz())
+         || !is_equal(_notch_E_center_freq_hz.get(), _error_notch.center_freq_hz()))) {
+        _error_notch.init(sample_rate, _notch_E_center_freq_hz, _notch_bandwidth_hz, _notch_attenuation_dB);
+    }
+#endif
+}
+
 //  update_all - set target and measured inputs to PID controller and calculate outputs
 //  target and error are filtered
 //  the derivative is then calculated and filtered
@@ -143,15 +216,42 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
         _target = target;
         _error = _target - measurement;
         _derivative = 0.0f;
+        _target_derivative = 0.0f;
+#if AC_PID_ADVANCED_ENABLED
+        if (_adv_enable) {
+            if (!is_zero(_notch_T_center_freq_hz.get())) {
+                _target_notch.reset();
+                _target = _target_notch.apply(_target);
+            }
+            if (!is_zero(_notch_E_center_freq_hz.get())) {
+                _error_notch.reset();
+                _error = _error_notch.apply(_error);
+            }
+        }
+#endif
     } else {
         float error_last = _error;
+        float target_last = _target;
+        float error = _target - measurement;
+#if AC_PID_ADVANCED_ENABLED
+        if (_adv_enable) {
+            // apply notch filters before FTLD/FLTE to avoid shot noise
+            if (!is_zero(_notch_T_center_freq_hz.get())) {
+                target = _target_notch.apply(target);
+            }
+            if (!is_zero(_notch_E_center_freq_hz.get())) {
+                error = _error_notch.apply(error);
+            }
+        }
+#endif
         _target += get_filt_T_alpha(dt) * (target - _target);
-        _error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
+        _error += get_filt_E_alpha(dt) * (error - _error);
 
         // calculate and filter derivative
         if (is_positive(dt)) {
             float derivative = (_error - error_last) / dt;
             _derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
+            _target_derivative = (_target - target_last) / dt;
         }
     }
 
@@ -253,8 +353,12 @@ float AC_PID::get_d() const
 
 float AC_PID::get_ff()
 {
-    _pid_info.FF = _target * _kff;
+    _pid_info.FF = _target * _kff
-    return _target * _kff;
+#if AC_PID_ADVANCED_ENABLED
+        + _target_derivative * _kdff
+#endif
+        ;
+    return  _pid_info.FF;
 }
 
 void AC_PID::reset_I()
@@ -275,6 +379,13 @@ void AC_PID::load_gains()
     _filt_T_hz.load();
     _filt_E_hz.load();
     _filt_D_hz.load();
+#if AC_PID_ADVANCED_ENABLED
+    _kdff.load();
+    _notch_T_center_freq_hz.load();
+    _notch_E_center_freq_hz.load();
+    _notch_bandwidth_hz.load();
+    _notch_attenuation_dB.load();
+#endif
 }
 
 // save_gains - save gains to eeprom
@@ -288,10 +399,17 @@ void AC_PID::save_gains()
     _filt_T_hz.save();
     _filt_E_hz.save();
     _filt_D_hz.save();
+#if AC_PID_ADVANCED_ENABLED
+    _kdff.save();
+    _notch_T_center_freq_hz.save();
+    _notch_E_center_freq_hz.save();
+    _notch_bandwidth_hz.save();
+    _notch_attenuation_dB.save();
+#endif
 }
 
 /// Overload the function call operator to permit easy initialisation
-void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz)
+void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dff_val)
 {
     _kp.set(p_val);
     _ki.set(i_val);
@@ -301,6 +419,9 @@ void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, flo
     _filt_T_hz.set(input_filt_T_hz);
     _filt_E_hz.set(input_filt_E_hz);
     _filt_D_hz.set(input_filt_D_hz);
+#if AC_PID_ADVANCED_ENABLED
+    _kdff.set(dff_val);
+#endif
 }
 
 // get_filt_T_alpha - get the target filter alpha

libraries/AC_PID/AC_PID.h

@@ -8,12 +8,17 @@
 #include <stdlib.h>
 #include <cmath>
 #include <Filter/SlewLimiter.h>
+#include <Filter/NotchFilter.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
 #define AC_PID_DFILT_HZ_DEFAULT  20.0f   // default input filter frequency
 #define AC_PID_RESET_TC          0.16f   // Time constant for integrator reset decay to zero
 
+#ifndef AC_PID_ADVANCED_ENABLED
+#define AC_PID_ADVANCED_ENABLED 0
+#endif
+
 #include "AP_PIDInfo.h"
 
 /// @class	AC_PID
@@ -32,11 +37,12 @@ public:
         float filt_D_hz;
         float srmax;
         float srtau;
+        float dff;
     };
 
     // 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 initial_srmax=0, float initial_srtau=1.0);
+           float initial_srmax=0, float initial_srtau=1.0, float initial_dff=0);
     AC_PID(const AC_PID::Defaults &defaults) :
         AC_PID(
             defaults.p,
@@ -48,7 +54,8 @@ public:
             defaults.filt_E_hz,
             defaults.filt_D_hz,
             defaults.srmax,
-            defaults.srtau
+            defaults.srtau,
+            defaults.dff
             )
         { }
 
@@ -95,7 +102,7 @@ public:
     void save_gains();
 
     /// operator function call for easy initialisation
-    void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz);
+    void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dff_val=0);
 
     // get accessors
     const AP_Float &kP() const { return _kp; }
@@ -146,6 +153,11 @@ public:
 
     const AP_PIDInfo& get_pid_info(void) const { return _pid_info; }
 
+#if AC_PID_ADVANCED_ENABLED
+    AP_Float &kDff() { return _kdff; }
+    void kDff(const float v) { _kdff.set(v); }
+#endif
+    void set_notch_sample_rate(float);
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -168,7 +180,14 @@ protected:
     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;
-
+#if AC_PID_ADVANCED_ENABLED
+    AP_Int8  _adv_enable;
+    AP_Float _kdff;
+    AP_Float _notch_T_center_freq_hz;
+    AP_Float _notch_E_center_freq_hz;
+    AP_Float _notch_bandwidth_hz;
+    AP_Float _notch_attenuation_dB;
+#endif
     SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau};
 
     // flags
@@ -182,6 +201,11 @@ protected:
     float _error;             // error value to enable filtering
     float _derivative;        // derivative value to enable filtering
     int8_t _slew_limit_scale;
+    float _target_derivative; // target derivative value to enable dff
+#if AC_PID_ADVANCED_ENABLED
+    NotchFilterFloat _target_notch;
+    NotchFilterFloat _error_notch;
+#endif
 
     AP_PIDInfo _pid_info;
 
@@ -190,6 +214,7 @@ private:
     const float default_ki;
     const float default_kd;
     const float default_kff;
+    const float default_kdff;
     const float default_kimax;
     const float default_filt_T_hz;
     const float default_filt_E_hz;