aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_Control: Add gyro feedback limit cycle protection

Browse Source
This commit is contained in:
Paul Riseborough 2020-05-10 17:11:11 +10:00 committed by Andrew Tridgell
parent 7292ee8ec6
commit 548bab5d24
4 changed files with 107 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/APM_Control/AP_PitchController.cpp
View File

@ -98,7 +98,25 @@ const AP_Param::GroupInfo AP_PitchController::var_info[] = {
// @User: User
AP_GROUPINFO("FF", 8, AP_PitchController, gains.FF, 0.0f),
AP_GROUPEND
// @Param: SRMAX
// @DisplayName: Servo slew rate limit
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (pitch rate feedback). If the amplitude of the control action produced by the pitch rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The limit should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A value of zero will disable this feature.
// @Units: deg/s
// @Range: 0 500
// @Increment: 10.0
// @User: Advanced
AP_GROUPINFO("SRMAX", 9, AP_PitchController, _slew_rate_max, 150.0f),
// @Param: SRTAU
// @DisplayName: Servo slew rate decay time constant
// @Description: This sets the time constant used to recover the D gain after it has been reduced due to excessive servo slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SRTAU", 10, AP_PitchController, _slew_rate_tau, 1.0f),
AP_GROUPEND
};
/*
@ -128,7 +146,10 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the pitch rate error (deg/sec) and scale
float achieved_rate = ToDeg(omega_y);
float rate_error = (desired_rate - achieved_rate) * scaler;
_pid_info.error = desired_rate - achieved_rate;
float rate_error = _pid_info.error * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
// Multiply pitch rate error by _ki_rate and integrate
// Scaler is applied before integrator so that integrator state relates directly to elevator deflection
@ -177,7 +198,7 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
float eas2tas = _ahrs.get_EAS2TAS();
float kp_ff = MAX((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / eas2tas;
float k_ff = gains.FF / eas2tas;
// Calculate the demanded control surface deflection
// Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward
// path, but want a 1/speed^2 scaler applied to the rate error path.
@ -185,9 +206,26 @@ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.D = rate_error * gains.D * scaler;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
if (dt > 0 && _slew_rate_max > 0) {
// Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply(((_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
_slew_rate_amplitude = fmaxf(fabsf(Dterm_slew_rate), alpha * _slew_rate_amplitude);
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _D_gain_modifier = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _D_gain_modifier;
}
_last_pid_info_D = _pid_info.D;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values

12
libraries/APM_Control/AP_PitchController.h
View File

@ -15,6 +15,8 @@ public:
, _ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
_slew_rate_filter.set_cutoff_frequency(10.0f);
_slew_rate_filter.reset(0.0f);
}
/* Do not allow copies */
@ -61,5 +63,13 @@ private:
float _get_coordination_rate_offset(float &aspeed, bool &inverted) const;
AP_AHRS &_ahrs;
// D gain limit cycle control
float _last_pid_info_D; // value of the D term (angular rate control feedback) from the previous time step (deg)
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
float _D_gain_modifier = 1.0f; // Gain modifier applied to the angular rate feedback to prevent excessive slew rate
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
};

47
libraries/APM_Control/AP_RollController.cpp
View File

@ -81,7 +81,25 @@ const AP_Param::GroupInfo AP_RollController::var_info[] = {
// @User: User
AP_GROUPINFO("FF", 6, AP_RollController, gains.FF, 0.0f),
AP_GROUPEND
// @Param: SRMAX
// @DisplayName: Servo slew rate limit
// @Description: Sets an upper limit on the servo slew rate produced by the D-gain (roll rate feedback). If the amplitude of the control action produced by the roll rate feedback exceeds this value, then the D-gain is reduced to respect the limit. This limits the amplitude of high frequency oscillations caused by an excessive D-gain. The parameter should be set to no more than 25% of the servo's specified slew rate to allow for inertia and aerodynamic load effects. Note: The D-gain will not be reduced to less than 10% of the nominal value. A valule of zero will disable this feature.
// @Units: deg/s
// @Range: 0 500
// @Increment: 10.0
// @User: Advanced
AP_GROUPINFO("SRMAX", 7, AP_RollController, _slew_rate_max, 150.0f),
// @Param: SRTAU
// @DisplayName: Servo slew rate decay time constant
// @Description: This sets the time constant used to recover the D-gain after it has been reduced due to excessive servo slew rate.
// @Units: s
// @Range: 0.5 5.0
// @Increment: 0.1
// @User: Advanced
AP_GROUPINFO("SRTAU", 8, AP_RollController, _slew_rate_tau, 1.0f),
AP_GROUPEND
};
@ -110,7 +128,10 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
// Calculate the roll rate error (deg/sec) and apply gain scaler
float achieved_rate = ToDeg(omega_x);
float rate_error = (desired_rate - achieved_rate) * scaler;
_pid_info.error = desired_rate - achieved_rate;
float rate_error = _pid_info.error * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
// Get an airspeed estimate - default to zero if none available
float aspeed;
@ -152,10 +173,26 @@ int32_t AP_RollController::_get_rate_out(float desired_rate, float scaler, bool
_pid_info.D = rate_error * gains.D * scaler;
_pid_info.P = desired_rate * kp_ff * scaler;
_pid_info.FF = desired_rate * k_ff * scaler;
_pid_info.target = desired_rate;
_pid_info.actual = achieved_rate;
_last_out = _pid_info.FF + _pid_info.P + _pid_info.D;
if (dt > 0 && _slew_rate_max > 0) {
// Calculate the slew rate amplitude produced by the unmodified D term
// calculate a low pass filtered slew rate
float Dterm_slew_rate = _slew_rate_filter.apply(((_pid_info.D - _last_pid_info_D)/ delta_time), delta_time);
// rectify and apply a decaying envelope filter
float alpha = 1.0f - constrain_float(delta_time/_slew_rate_tau, 0.0f , 1.0f);
_slew_rate_amplitude = fmaxf(fabsf(Dterm_slew_rate), alpha * _slew_rate_amplitude);
_slew_rate_amplitude = fminf(_slew_rate_amplitude, 10.0f*_slew_rate_max);
// Calculate and apply the D gain adjustment
_pid_info.Dmod = _D_gain_modifier = _slew_rate_max / fmaxf(_slew_rate_amplitude, _slew_rate_max);
_pid_info.D *= _D_gain_modifier;
}
_last_pid_info_D = _pid_info.D;
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P;
if (autotune.running && aspeed > aparm.airspeed_min) {
// let autotune have a go at the values

10
libraries/APM_Control/AP_RollController.h
View File

@ -15,6 +15,8 @@ public:
, _ahrs(ahrs)
{
AP_Param::setup_object_defaults(this, var_info);
_slew_rate_filter.set_cutoff_frequency(10.0f);
_slew_rate_filter.reset(0.0f);
}
/* Do not allow copies */
@ -66,4 +68,12 @@ private:
AP_AHRS &_ahrs;
// D gain limit cycle control
float _last_pid_info_D; // value of the D term (angular rate control feedback) from the previous time step (deg)
LowPassFilterFloat _slew_rate_filter; // LPF applied to the derivative of the control action generated by the angular rate feedback
float _slew_rate_amplitude; // Amplitude of the servo slew rate produced by the angular rate feedback (deg/sec)
float _D_gain_modifier = 1.0f; // Gain modifier applied to the angular rate feedback to prevent excessive slew rate
AP_Float _slew_rate_max; // Maximum permitted angular rate control feedback servo slew rate (deg/sec)
AP_Float _slew_rate_tau; // Time constant used to recover gain after a slew rate exceedance (sec)
};