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

AC_PID: added slew limiter AC_PID

Browse Source 
this adds the fixed wing slew limiter as an optional part of
AC_PID. It allows the user to configure a maximum slew limit to
prevent oscillations in PIDs when gains are too high
This commit is contained in:
Andrew Tridgell 2020-09-24 13:28:24 +10:00
parent 709e1f8f93
commit 739410953b
3 changed files with 44 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -61,6 +61,14 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
// @Units: Hz
AP_GROUPINFO("FLTD", 11, AC_HELI_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT),
// @Param: SMAX
// @DisplayName: Slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO("SMAX", 12, AC_HELI_PID, _slew_rate_max, 0),
AP_GROUPEND
};

25
libraries/AC_PID/AC_PID.cpp
View File

@ -56,11 +56,20 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
// @Units: Hz
AP_GROUPINFO("FLTD", 11, AC_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT),
// @Param: SMAX
// @DisplayName: Slew rate limit
// @Description: Sets an upper limit on the slew rate produced by the combined P and D gains. If the amplitude of the control action produced by the rate feedback exceeds this value, then the D+P gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive gain. The limit should be set to no more than 25% of the actuators maximum slew rate to allow for load effects. Note: The gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Range: 0 200
// @Increment: 0.5
// @User: Advanced
AP_GROUPINFO("SMAX", 12, AC_PID, _slew_rate_max, 0),
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 dt) :
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 dt, float initial_srmax, float initial_srtau):
_dt(dt),
_integrator(0.0f),
_error(0.0f),
@ -77,6 +86,8 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
filt_T_hz(initial_filt_T_hz);
filt_E_hz(initial_filt_E_hz);
filt_D_hz(initial_filt_D_hz);
_slew_rate_max.set(initial_srmax);
_slew_rate_tau.set(initial_srtau);
// reset input filter to first value received
_flags._reset_filter = true;
@ -144,6 +155,12 @@ float AC_PID::update_all(float target, float measurement, bool limit)
float P_out = (_error * _kp);
float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt);
P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod;
_pid_info.target = _target;
_pid_info.actual = measurement;
_pid_info.error = _error;
@ -190,6 +207,12 @@ float AC_PID::update_error(float error, bool limit)
float P_out = (_error * _kp);
float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier(_pid_info.P + _pid_info.D, _dt);
P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod;
_pid_info.target = 0.0f;
_pid_info.actual = 0.0f;
_pid_info.error = _error;

13
libraries/AC_PID/AC_PID.h
View File

@ -8,6 +8,7 @@
#include <stdlib.h>
#include <cmath>
#include <AP_Logger/AP_Logger.h>
#include <Filter/SlewLimiter.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
@ -19,7 +20,8 @@ class AC_PID {
public:
// 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 dt);
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 dt, float initial_srmax=0, float initial_srtau=1.0);
// set_dt - set time step in seconds
void set_dt(float dt);
@ -105,6 +107,12 @@ public:
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
// the time constant tau is not currently configurable, but is set
// as an AP_Float to make it easy to make it configurable for a
// single user of AC_PID by adding the parameter in the param
// table of the parent class. It is made public for this reason
AP_Float _slew_rate_tau;
protected:
// parameters
@ -116,6 +124,9 @@ protected:
AP_Float _filt_T_hz; // PID target filter frequency in Hz
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;
SlewLimiter _slew_limiter{_slew_rate_max, _slew_rate_tau};
// flags
struct ac_pid_flags {