Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_Control: new autotune scheme 

this separately tunes D and then P, which more closely follows the
technique used for a manual tune
This commit is contained in:
Andrew Tridgell 2021-07-11 10:32:08 +10:00
parent 73952dfecb
commit 45cf726a4b
2 changed files with 154 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
libraries/APM_Control/AP_AutoTune.cpp
View File

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

28
libraries/APM_Control/AP_AutoTune.h
View File

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