Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andy Piper 2023-10-01 18:21:00 +01:00 committed by Andrew Tridgell
parent 560030c9ba
commit fc76312fc3
4 changed files with 75 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
libraries/AC_PID/AC_HELI_PID.cpp
View File

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

146
libraries/AC_PID/AC_PID.cpp
View File

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

22
libraries/AC_PID/AC_PID.h
View File

@ -9,16 +9,13 @@
#include <cmath>
#include <Filter/SlewLimiter.h>
#include <Filter/NotchFilter.h>
#include <Filter/AP_Filter.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
@ -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;

1
libraries/AC_PID/AP_PIDInfo.h
View File

@ -14,6 +14,7 @@ struct AP_PIDInfo {
float I;
float D;
float FF;
float DFF;
float Dmod;
float slew_rate;
bool limit;