From fc76312fc39e18322cf38d161a84cb51ab4d8e25 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sun, 1 Oct 2023 18:21:00 +0100 Subject: [PATCH] AC_PID: use AP_Filter for notch configuration enable filters with AP_FILTER_ENABLED dynamically allocate notches remove load/save for notches, update docs move feedfoward update to update_all() restrict load_gains() and save_gains() to just what autotune needs add D_FF logging --- libraries/AC_PID/AC_HELI_PID.cpp | 48 +++------- libraries/AC_PID/AC_PID.cpp | 146 +++++++++++-------------------- libraries/AC_PID/AC_PID.h | 22 ++--- libraries/AC_PID/AP_PIDInfo.h | 1 + 4 files changed, 75 insertions(+), 142 deletions(-) diff --git a/libraries/AC_PID/AC_HELI_PID.cpp b/libraries/AC_PID/AC_HELI_PID.cpp index 14dbd97b60..0e599a160c 100644 --- a/libraries/AC_PID/AC_HELI_PID.cpp +++ b/libraries/AC_PID/AC_HELI_PID.cpp @@ -69,13 +69,11 @@ 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 + // @Param: PDMX + // @DisplayName: PD sum maximum + // @Description: The maximum/minimum value that the sum of the P and D term can output // @User: Advanced - AP_GROUPINFO_FLAGS("ADV", 32, AC_HELI_PID, _adv_enable, 0, AP_PARAM_FLAG_ENABLE), + AP_GROUPINFO("PDMX", 13, AC_HELI_PID, _kpdmax, 0), // @Param: D_FF // @DisplayName: PID Derivative FeedForward Gain @@ -83,40 +81,22 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = { // @Range: 0 0.02 // @Increment: 0.0001 // @User: Advanced - AP_GROUPINFO("D_FF", 33, AC_HELI_PID, _kdff, 0), + AP_GROUPINFO("D_FF", 14, AC_HELI_PID, _kdff, 0), +#if AP_FILTER_ENABLED // @Param: NTF - // @DisplayName: PID Target notch Filter center frequency - // @Description: PID Target notch Filter center frequency in Hz. - // @Range: 10 495 - // @Units: Hz + // @DisplayName: PID Target notch filter index + // @Description: PID Target notch filter index + // @Range: 1 8 // @User: Advanced - AP_GROUPINFO("NTF", 34, AC_HELI_PID, _notch_T_center_freq_hz, 0), + AP_GROUPINFO("NTF", 15, AC_HELI_PID, _notch_T_filter, 0), // @Param: NEF - // @DisplayName: PID Error notch Filter center frequency - // @Description: PID Error notch Filter center frequency in Hz. - // @Range: 10 495 - // @Units: Hz + // @DisplayName: PID Error notch filter index + // @Description: PID Error notch filter index + // @Range: 1 8 // @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), - + AP_GROUPINFO("NEF", 16, AC_HELI_PID, _notch_E_filter, 0), #endif AP_GROUPEND diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index fcb2406cfe..557b6b5bfb 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -72,54 +72,28 @@ 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), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D_FF", 14, AC_PID, _kdff, default_kdff), +#if AP_FILTER_ENABLED // @Param: NTF - // @DisplayName: PID Target notch Filter center frequency - // @Description: PID Target notch Filter center frequency in Hz. - // @Range: 10 495 - // @Units: Hz + // @DisplayName: PID Target notch filter index + // @Description: PID Target notch filter index + // @Range: 1 8 // @User: Advanced - AP_GROUPINFO("NTF", 34, AC_PID, _notch_T_center_freq_hz, 0), + AP_GROUPINFO("NTF", 15, AC_PID, _notch_T_filter, 0), // @Param: NEF - // @DisplayName: PID Error notch Filter center frequency - // @Description: PID Error notch Filter center frequency in Hz. - // @Range: 10 495 - // @Units: Hz + // @DisplayName: PID Error notch filter index + // @Description: PID Error notch filter index + // @Range: 1 8 // @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), - + AP_GROUPINFO("NEF", 16, AC_PID, _notch_E_filter, 0), #endif AP_GROUPEND @@ -181,20 +155,33 @@ void AC_PID::slew_limit(float smax) void AC_PID::set_notch_sample_rate(float sample_rate) { -#if AC_PID_ADVANCED_ENABLED - if (!_adv_enable) { +#if AP_FILTER_ENABLED + if (_notch_T_filter == 0 && _notch_E_filter == 0) { 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 (_notch_T_filter != 0) { + if (_target_notch == nullptr) { + _target_notch = new NotchFilterFloat(); + } + AP_Filter* filter = AP::filters().get_filter(_notch_T_filter); + if (filter != nullptr && !filter->setup_notch_filter(*_target_notch, sample_rate)) { + delete _target_notch; + _target_notch = nullptr; + _notch_T_filter.set(0); + } } - 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); + + if (_notch_E_filter != 0) { + if (_error_notch == nullptr) { + _error_notch = new NotchFilterFloat(); + } + AP_Filter* filter = AP::filters().get_filter(_notch_E_filter); + if (filter != nullptr && !filter->setup_notch_filter(*_error_notch, sample_rate)) { + delete _error_notch; + _error_notch = nullptr; + _notch_E_filter.set(0); + } } #endif } @@ -217,31 +204,27 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit, _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); - } +#if AP_FILTER_ENABLED + if (_target_notch != nullptr) { + _target_notch->reset(); + _target = _target_notch->apply(_target); + } + if (_error_notch != nullptr) { + _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); - } +#if AP_FILTER_ENABLED + // apply notch filters before FTLD/FLTE to avoid shot noise + if (_target_notch != nullptr) { + target = _target_notch->apply(target); + } + if (_error_notch != nullptr) { + error = _error_notch->apply(error); } #endif _target += get_filt_T_alpha(dt) * (target - _target); @@ -289,6 +272,8 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit, _pid_info.error = _error; _pid_info.P = P_out; _pid_info.D = D_out; + _pid_info.FF = _target * _kff; + _pid_info.DFF = _target_derivative * _kdff; return P_out + D_out + _integrator; } @@ -353,12 +338,7 @@ float AC_PID::get_d() const float AC_PID::get_ff() { - _pid_info.FF = _target * _kff -#if AC_PID_ADVANCED_ENABLED - + _target_derivative * _kdff -#endif - ; - return _pid_info.FF; + return _pid_info.FF + _pid_info.DFF; } void AC_PID::reset_I() @@ -366,46 +346,28 @@ void AC_PID::reset_I() _integrator = 0.0; } +// load original gains from eeprom, used by autotune to restore gains after tuning void AC_PID::load_gains() { _kp.load(); _ki.load(); _kd.load(); _kff.load(); - _kimax.load(); - _kimax.set(fabsf(_kimax)); - _kpdmax.load(); - _kpdmax.set(fabsf(_kpdmax)); _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 +// save original gains to eeprom, used by autotune to save gains before tuning void AC_PID::save_gains() { _kp.save(); _ki.save(); _kd.save(); _kff.save(); - _kimax.save(); _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 @@ -419,9 +381,7 @@ 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 30300d071d..0aa7e35e2c 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -9,16 +9,13 @@ #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 @@ -153,10 +150,8 @@ 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[]; @@ -180,13 +175,10 @@ 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; +#if AP_FILTER_ENABLED + AP_Int8 _notch_T_filter; + AP_Int8 _notch_E_filter; #endif SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau}; @@ -202,9 +194,9 @@ protected: 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; +#if AP_FILTER_ENABLED + NotchFilterFloat* _target_notch; + NotchFilterFloat* _error_notch; #endif AP_PIDInfo _pid_info; diff --git a/libraries/AC_PID/AP_PIDInfo.h b/libraries/AC_PID/AP_PIDInfo.h index 4ba52fe37d..70afb0974e 100644 --- a/libraries/AC_PID/AP_PIDInfo.h +++ b/libraries/AC_PID/AP_PIDInfo.h @@ -14,6 +14,7 @@ struct AP_PIDInfo { float I; float D; float FF; + float DFF; float Dmod; float slew_rate; bool limit;