Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: add notch filters to target and error terms

Browse Source 
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
This commit is contained in:
Andy Piper 2023-07-27 15:58:50 +02:00 committed by Andrew Tridgell
parent 61e91dde35
commit b17b78e329
4 changed files with 209 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AC_PID/AC_HELI_PID.cpp
View File

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

2
libraries/AC_PID/AC_HELI_PID.h
View File

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

133
libraries/AC_PID/AC_PID.cpp
View File

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

33
libraries/AC_PID/AC_PID.h
View File

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