From 6609faff9e0e60bf78bd7d089a743ebb42b7778c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 Jul 2021 10:32:08 +1000 Subject: [PATCH] APM_Control: new autotune scheme this separately tunes D and then P, which more closely follows the technique used for a manual tune --- libraries/APM_Control/AP_AutoTune.cpp | 202 +++++++++++++++++--------- libraries/APM_Control/AP_AutoTune.h | 28 +++- 2 files changed, 154 insertions(+), 76 deletions(-) diff --git a/libraries/APM_Control/AP_AutoTune.cpp b/libraries/APM_Control/AP_AutoTune.cpp index 03540abbf5..c9ae435df0 100644 --- a/libraries/APM_Control/AP_AutoTune.cpp +++ b/libraries/APM_Control/AP_AutoTune.cpp @@ -43,13 +43,9 @@ extern const AP_HAL::HAL& hal; // time in milliseconds between autotune saves #define AUTOTUNE_SAVE_PERIOD 10000U -// step size for increasing gains, percentage +// step size for changing FF gains, percentage #define AUTOTUNE_INCREASE_FF_STEP 12 -#define AUTOTUNE_INCREASE_PD_STEP 10 - -// step size for decreasing gains, percentage #define AUTOTUNE_DECREASE_FF_STEP 15 -#define AUTOTUNE_DECREASE_PD_STEP 20 // limits on IMAX #define AUTOTUNE_MIN_IMAX 0.4 @@ -61,9 +57,6 @@ extern const AP_HAL::HAL& hal; // time constant of rate trim loop #define TRIM_TCONST 1.0f -// overshoot threshold -#define AUTOTUNE_OVERSHOOT 1.1 - // constructor AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type, const AP_Vehicle::FixedWing &parms, @@ -122,6 +115,8 @@ void AP_AutoTune::start(void) // do first update of rmax and tau now update_rmax(); + dt = AP::scheduler().get_loop_period_s(); + rpid.kIMAX().set(constrain_float(rpid.kIMAX(), AUTOTUNE_MIN_IMAX, AUTOTUNE_MAX_IMAX)); next_save = current; @@ -136,6 +131,12 @@ void AP_AutoTune::start(void) ff_filter.reset(); actuator_filter.reset(); rate_filter.reset(); + D_limit = 0; + P_limit = 0; + ff_count = 0; + D_set_ms = 0; + P_set_ms = 0; + done_count = 0; if (!is_positive(rpid.slew_limit())) { // we must have a slew limit, default to 150 deg/s @@ -158,6 +159,12 @@ void AP_AutoTune::stop(void) { if (running) { running = false; + if (is_positive(D_limit)) { + restore.D = MIN(restore.D, D_limit); + } + if (is_positive(P_limit)) { + restore.P = MIN(restore.P, P_limit); + } save_gains(restore); current = restore; } @@ -193,7 +200,17 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e max_D = MAX(max_D, fabsf(pinfo.D)); min_Dmod = MIN(min_Dmod, pinfo.Dmod); max_Dmod = MAX(max_Dmod, pinfo.Dmod); - max_SRate = MAX(max_SRate, pinfo.slew_rate); + + // update the P and D slew rates, using P and D values from before Dmod was applied + const float slew_limit_scale = 45.0 / degrees(1); + slew_limit_max = rpid.slew_limit(); + slew_limit_tau = 1.0; + slew_limiter_P.modifier((pinfo.P/pinfo.Dmod)*slew_limit_scale, dt); + slew_limiter_D.modifier((pinfo.D/pinfo.Dmod)*slew_limit_scale, dt); + + // remember maximum slew rates for this cycle + max_SRate_P = MAX(max_SRate_P, slew_limiter_P.get_slew_rate()); + max_SRate_D = MAX(max_SRate_D, slew_limiter_D.get_slew_rate()); float att_limit_deg; if (type == AUTOTUNE_ROLL) { @@ -237,8 +254,8 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e type : uint8_t(type), state: uint8_t(new_state), actuator : actuator, - desired_rate : desired_rate, - actual_rate : actual_rate, + P_slew : max_SRate_P, + D_slew : max_SRate_D, FF_single: FF_single, FF: current.FF, P: current.P, @@ -257,15 +274,19 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e now - state_enter_ms > 500 && max_Dmod < 0.9) { // we've been oscillating while idle, reduce P or D - const float gain_mul = (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01; - if (max_P < max_D) { - current.D *= gain_mul; - } else { - current.P *= gain_mul; - } + const float slew_sum = max_SRate_P + max_SRate_D; + const float gain_mul = 0.5; + current.P *= linear_interpolate(gain_mul, 1.0, + max_SRate_P, + slew_sum, 0); + current.D *= linear_interpolate(gain_mul, 1.0, + max_SRate_D, + slew_sum, 0); rpid.kP().set(current.P); rpid.kD().set(current.D); action = Action::IDLE_LOWER_PD; + P_limit = MIN(P_limit, current.P); + D_limit = MIN(D_limit, current.D); state_change(state); } return; @@ -303,6 +324,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e // apply median filter float FF = ff_filter.apply(FF_single); + ff_count++; const float old_FF = rpid.ff(); @@ -311,81 +333,102 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler, float angle_e old_FF*(1-AUTOTUNE_DECREASE_FF_STEP*0.01), old_FF*(1+AUTOTUNE_INCREASE_FF_STEP*0.01)); - // did the P or D components go over 30% of total actuator? - const float abs_actuator = MAX(max_actuator, fabsf(min_actuator)); - const float PD_high = 0.3 * abs_actuator; - bool PD_significant = (max_P > PD_high || max_D > PD_high); - - // see if we overshot - const float dem_ratio = (state == ATState::DEMAND_POS)? - constrain_float(max_rate / max_target, 0.1, 2): - constrain_float(min_rate / min_target, 0.1, 2); - bool overshot = dem_ratio > AUTOTUNE_OVERSHOOT; - // adjust P and D float D = rpid.kD(); float P = rpid.kP(); - D = MAX(D, 0.0005); - P = MAX(P, 0.01); - - // if the slew limiter kicked in or - if (min_Dmod < 1.0 || (overshot && PD_significant)) { - // we apply a gain reduction in proportion to the overshoot and dmod - const float gain_mul = (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01; - const float dmod_mul = linear_interpolate(gain_mul, 1, - min_Dmod, - 0.6, 1.0); - const float overshoot_mul = linear_interpolate(1, gain_mul, - dem_ratio, - AUTOTUNE_OVERSHOOT, 1.3 * AUTOTUNE_OVERSHOOT); - - // we're overshooting or oscillating, decrease gains. We - // assume the gain that needs to be reduced is the one that - // peaked at a higher value - if (max_P < max_D) { - D *= dmod_mul * overshoot_mul; - } else { - P *= dmod_mul * overshoot_mul; - } - action = Action::LOWER_PD; - } else { - /* not oscillating or overshooting, increase the gains - - The increase is based on how far we are below the slew - limit. At 80% of the limit we stop increasing gains, to - give some margin. Below 25% of the limit we apply max - increase - */ - const float slew_limit = rpid.slew_limit(); - const float gain_mul = (100+AUTOTUNE_INCREASE_PD_STEP)*0.01; - const float PD_mul = linear_interpolate(gain_mul, 1, - max_SRate, - 0.2*slew_limit, 0.6*slew_limit); - P *= PD_mul; - D *= PD_mul; - action = Action::RAISE_PD; + if (ff_count == 1) { + // apply minimum D and P values + D = MAX(D, 0.0005); + P = MAX(P, 0.01); + } else if (ff_count == 4) { + // we got a good ff estimate, halve P ready to start raising D + P *= 0.5; } + // see if the slew limiter kicked in + if (min_Dmod < 1.0 && !is_positive(D_limit)) { + // oscillation, without D_limit set + if (max_P > 0.5 * max_D) { + // lower P and D to get us to a non-oscillating state + P *= 0.35; + D *= 0.75; + action = Action::LOWER_PD; + } else { + // set D limit to 30% of current D, remember D limit and start to work on P + D *= 0.3; + D_limit = D; + D_set_ms = now; + action = Action::LOWER_D; + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit); + } + } else if (min_Dmod < 1.0) { + // oscillation, with D_limit set + if (now - D_set_ms > 2000) { + // leave 2s for Dmod to settle after lowering D + if (max_D > 0.8 * max_P) { + // lower D limit some more + D *= 0.35; + D_limit = D; + D_set_ms = now; + action = Action::LOWER_D; + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sD: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", D_limit); + done_count = 0; + } else if (now - P_set_ms > 2000) { + P *= 0.35; + P_limit = P; + P_set_ms = now; + action = Action::LOWER_P; + done_count = 0; + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%sP: %.4f", type==AUTOTUNE_ROLL?"Roll":"Pitch", P_limit); + } + } + } else if (ff_count < 4) { + // we don't have a good FF estimate yet, keep going + + } else if (!is_positive(D_limit)) { + /* we haven't detected D oscillation yet, keep raising D */ + D *= 1.3; + action = Action::RAISE_D; + } else if (!is_positive(P_limit)) { + /* not oscillating, increase P gain */ + P *= 1.3; + action = Action::RAISE_PD; + } else { + // after getting P_limit we consider the tune done when we + // have done 3 cycles without reducing P + if (done_count < 3) { + if (++done_count == 3) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "%s: Finished", type==AUTOTUNE_ROLL?"Roll":"Pitch"); + next_save.P = P; + next_save.D = D; + restore.P = P; + restore.D = D; + } + } + } rpid.ff().set(FF); rpid.kP().set(P); rpid.kD().set(D); rpid.kI().set(MAX(P*AUTOTUNE_I_RATIO, (FF / TRIM_TCONST))); - // setup target filter to be suitable for time constant + // setup filters to be suitable for time constant and gyro filter rpid.filt_T_hz().set(10.0/(current.tau * 2 * M_PI)); + rpid.filt_E_hz().set(0); + rpid.filt_D_hz().set(AP::ins().get_gyro_filter_hz()*0.5); current.FF = FF; current.P = P; current.I = rpid.kI().get(); current.D = D; - Debug("FPID=(%.3f, %.3f, %.3f, %.3f)\n", + Debug("FPID=(%.3f, %.3f, %.3f, %.3f) Dmod=%.2f\n", rpid.ff().get(), rpid.kP().get(), rpid.kI().get(), - rpid.kD().get()); + rpid.kD().get(), + min_Dmod); // move rmax and tau towards target update_rmax(); @@ -400,7 +443,8 @@ void AP_AutoTune::state_change(ATState new_state) { min_Dmod = 1; max_Dmod = 0; - max_SRate = 0; + max_SRate_P = 1; + max_SRate_D = 1; max_P = max_D = 0; state = new_state; state_enter_ms = AP_HAL::millis(); @@ -432,6 +476,12 @@ void AP_AutoTune::check_save(void) // the next values to save will be the ones we are flying now next_save = tmp; + if (is_positive(D_limit)) { + next_save.D = MIN(next_save.D, D_limit); + } + if (is_positive(P_limit)) { + next_save.P = MIN(next_save.P, P_limit); + } last_save_ms = AP_HAL::millis(); } @@ -477,6 +527,8 @@ void AP_AutoTune::save_gains(const ATGains &v) save_float_if_changed(rpid.kD(), v.D); save_float_if_changed(rpid.kIMAX(), v.IMAX); save_float_if_changed(rpid.filt_T_hz(), v.flt_T); + save_float_if_changed(rpid.filt_E_hz(), v.flt_E); + save_float_if_changed(rpid.filt_D_hz(), v.flt_D); last_save = get_gains(current); current = tmp; } @@ -493,6 +545,8 @@ AP_AutoTune::ATGains AP_AutoTune::get_gains(const ATGains &v) ret.D = rpid.kD(); ret.IMAX = rpid.kIMAX(); ret.flt_T = rpid.filt_T_hz(); + ret.flt_E = rpid.filt_E_hz(); + ret.flt_D = rpid.filt_D_hz(); return ret; } @@ -508,6 +562,8 @@ void AP_AutoTune::set_gains(const ATGains &v) rpid.kD().set(v.D); rpid.kIMAX().set(v.IMAX); rpid.filt_T_hz().set(v.flt_T); + rpid.filt_E_hz().set(v.flt_E); + rpid.filt_D_hz().set(v.flt_D); } /* @@ -529,6 +585,10 @@ void AP_AutoTune::update_rmax(void) } else { target_rmax = tuning_table[level-1].rmax; target_tau = tuning_table[level-1].tau; + if (type == AUTOTUNE_PITCH) { + // 50% longer time constant on pitch + target_tau *= 1.5; + } } if (level > 0 && is_positive(current.FF)) { diff --git a/libraries/APM_Control/AP_AutoTune.h b/libraries/APM_Control/AP_AutoTune.h index 9cb46253c2..eba38484bb 100644 --- a/libraries/APM_Control/AP_AutoTune.h +++ b/libraries/APM_Control/AP_AutoTune.h @@ -12,7 +12,8 @@ public: AP_Float tau; AP_Int16 rmax_pos; AP_Int16 rmax_neg; - float FF, P, I, D, IMAX, flt_T; + float FF, P, I, D, IMAX; + float flt_T, flt_E, flt_D; }; enum ATType { @@ -26,8 +27,8 @@ public: uint8_t type; uint8_t state; float actuator; - float desired_rate; - float actual_rate; + float P_slew; + float D_slew; float FF_single; float FF; float P; @@ -93,7 +94,11 @@ private: SHORT, RAISE_PD, LOWER_PD, - IDLE_LOWER_PD}; + IDLE_LOWER_PD, + RAISE_D, + RAISE_P, + LOWER_D, + LOWER_P}; Action action; // when we entered the current state @@ -121,6 +126,11 @@ private: LowPassFilterFloat rate_filter; LowPassFilterFloat target_filter; + // separate slew limiters for P and D + float slew_limit_max, slew_limit_tau; + SlewLimiter slew_limiter_P{slew_limit_max, slew_limit_tau}; + SlewLimiter slew_limiter_D{slew_limit_max, slew_limit_tau}; + float max_actuator; float min_actuator; float max_rate; @@ -131,6 +141,14 @@ private: float max_D; float min_Dmod; float max_Dmod; - float max_SRate; + float max_SRate_P; + float max_SRate_D; float FF_single; + uint16_t ff_count; + float dt; + float D_limit; + float P_limit; + uint32_t D_set_ms; + uint32_t P_set_ms; + uint8_t done_count; };