From b17b78e3299a03181f4109d824e8c5d03d4b1962 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 27 Jul 2023 15:58:50 +0200 Subject: [PATCH] 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 | 54 ++++++++++++- libraries/AC_PID/AC_HELI_PID.h | 2 +- libraries/AC_PID/AC_PID.cpp | 133 +++++++++++++++++++++++++++++-- libraries/AC_PID/AC_PID.h | 33 +++++++- 4 files changed, 209 insertions(+), 13 deletions(-) diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 3525ae26c6..14dbd97b60 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/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_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, 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, dff_val) { _last_requested_rate = 0; } diff --git a/libraries/AC_PID/AC_HELI_PID.h b/libraries/AC_PID/AC_HELI_PID.h index 0ed7737f18..d9f323d34a 100644 --- a/libraries/AC_PID/AC_HELI_PID.h +++ b/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); diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 8d3349252c..fcb2406cfe 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -4,6 +4,8 @@ #include #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; - return _target * _kff; + _pid_info.FF = _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 diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 7ae4592473..30300d071d 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -8,12 +8,17 @@ #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 #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;