From 548bab5d244bbb91ef4ad21578ef2e696d87486f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 10 May 2020 17:11:11 +1000 Subject: [PATCH] APM_Control: Add gyro feedback limit cycle protection --- libraries/APM_Control/AP_PitchController.cpp | 50 +++++++++++++++++--- libraries/APM_Control/AP_PitchController.h | 12 ++++- libraries/APM_Control/AP_RollController.cpp | 47 ++++++++++++++++-- libraries/APM_Control/AP_RollController.h | 10 ++++ 4 files changed, 107 insertions(+), 12 deletions(-) diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index 154415ecdd..4170d34d40 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -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 diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 3f064b606d..5dee18b1d9 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -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) + }; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 18fdc6e455..020b91ae6d 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -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 diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index a32928ed9d..9463754d1d 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -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) + };