mirror of https://github.com/ArduPilot/ardupilot
APM_Control: use slew rate to move gains more smoothly
This commit is contained in:
parent
59a805a740
commit
b079da33c5
|
@ -45,10 +45,7 @@ extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// step size for increasing gains, percentage
|
// step size for increasing gains, percentage
|
||||||
#define AUTOTUNE_INCREASE_FF_STEP 12
|
#define AUTOTUNE_INCREASE_FF_STEP 12
|
||||||
#define AUTOTUNE_INCREASE_PD_STEP 5
|
#define AUTOTUNE_INCREASE_PD_STEP 10
|
||||||
|
|
||||||
// step size for increasing gains when low impact, percentage
|
|
||||||
#define AUTOTUNE_INCREASE_PD_LOW_STEP 30
|
|
||||||
|
|
||||||
// step size for decreasing gains, percentage
|
// step size for decreasing gains, percentage
|
||||||
#define AUTOTUNE_DECREASE_FF_STEP 15
|
#define AUTOTUNE_DECREASE_FF_STEP 15
|
||||||
|
@ -87,6 +84,7 @@ AP_AutoTune::AP_AutoTune(ATGains &_gains, ATType _type,
|
||||||
tuning parameters based on a user chosen AUTOTUNE_LEVEL parameter
|
tuning parameters based on a user chosen AUTOTUNE_LEVEL parameter
|
||||||
from 1 to 10. Level 1 is a very soft tune. Level 10 is a very
|
from 1 to 10. Level 1 is a very soft tune. Level 10 is a very
|
||||||
aggressive tune.
|
aggressive tune.
|
||||||
|
Level 0 means use the existing RMAX and TCONST parameters
|
||||||
*/
|
*/
|
||||||
static const struct {
|
static const struct {
|
||||||
float tau;
|
float tau;
|
||||||
|
@ -129,13 +127,15 @@ void AP_AutoTune::start(void)
|
||||||
actuator_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
actuator_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
||||||
rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), 2);
|
||||||
|
|
||||||
// scale slew limit to more agressively find oscillations during autotune
|
|
||||||
rpid.set_slew_limit_scale(1.5*45);
|
|
||||||
|
|
||||||
ff_filter.reset();
|
ff_filter.reset();
|
||||||
actuator_filter.reset();
|
actuator_filter.reset();
|
||||||
rate_filter.reset();
|
rate_filter.reset();
|
||||||
|
|
||||||
|
if (!is_positive(rpid.slew_limit())) {
|
||||||
|
// we must have a slew limit, default to 150 deg/s
|
||||||
|
rpid.slew_limit().set_and_save(150);
|
||||||
|
}
|
||||||
|
|
||||||
Debug("START FF -> %.3f\n", rpid.ff().get());
|
Debug("START FF -> %.3f\n", rpid.ff().get());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -199,6 +199,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
max_P = MAX(max_P, fabsf(pinfo.P));
|
max_P = MAX(max_P, fabsf(pinfo.P));
|
||||||
max_D = MAX(max_D, fabsf(pinfo.D));
|
max_D = MAX(max_D, fabsf(pinfo.D));
|
||||||
min_Dmod = MIN(min_Dmod, pinfo.Dmod);
|
min_Dmod = MIN(min_Dmod, pinfo.Dmod);
|
||||||
|
max_SRate = MAX(max_SRate, pinfo.slew_rate);
|
||||||
|
|
||||||
int16_t att_limit_cd;
|
int16_t att_limit_cd;
|
||||||
if (type == AUTOTUNE_ROLL) {
|
if (type == AUTOTUNE_ROLL) {
|
||||||
|
@ -273,6 +274,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
state = ATState::IDLE;
|
state = ATState::IDLE;
|
||||||
action = Action::LOW_RATE;
|
action = Action::LOW_RATE;
|
||||||
min_Dmod = 1;
|
min_Dmod = 1;
|
||||||
|
max_SRate = 0;
|
||||||
max_P = max_D = 0;
|
max_P = max_D = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -282,6 +284,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
state = ATState::IDLE;
|
state = ATState::IDLE;
|
||||||
action = Action::SHORT;
|
action = Action::SHORT;
|
||||||
min_Dmod = 1;
|
min_Dmod = 1;
|
||||||
|
max_SRate = 0;
|
||||||
max_P = max_D = 0;
|
max_P = max_D = 0;
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -325,13 +328,12 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
if (min_Dmod < 1.0 || (overshot && PD_significant)) {
|
if (min_Dmod < 1.0 || (overshot && PD_significant)) {
|
||||||
// we apply a gain reduction in proportion to the overshoot and dmod
|
// 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 gain_mul = (100 - AUTOTUNE_DECREASE_PD_STEP)*0.01;
|
||||||
const float dmod_mul = linear_interpolate(1, gain_mul,
|
const float dmod_mul = linear_interpolate(gain_mul, 1,
|
||||||
min_Dmod,
|
min_Dmod,
|
||||||
1.0, 0.6);
|
0.6, 1.0);
|
||||||
const float overshoot_mul = linear_interpolate(1, gain_mul,
|
const float overshoot_mul = linear_interpolate(1, gain_mul,
|
||||||
dem_ratio,
|
dem_ratio,
|
||||||
AUTOTUNE_OVERSHOOT,
|
AUTOTUNE_OVERSHOOT, 1.3 * AUTOTUNE_OVERSHOOT);
|
||||||
1.3 * AUTOTUNE_OVERSHOOT);
|
|
||||||
|
|
||||||
// we're overshooting or oscillating, decrease gains. We
|
// we're overshooting or oscillating, decrease gains. We
|
||||||
// assume the gain that needs to be reduced is the one that
|
// assume the gain that needs to be reduced is the one that
|
||||||
|
@ -343,20 +345,20 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
}
|
}
|
||||||
action = Action::LOWER_PD;
|
action = Action::LOWER_PD;
|
||||||
} else {
|
} else {
|
||||||
const float low_PD = 0.05 * MAX(max_actuator, fabsf(min_actuator));
|
/* not oscillating or overshooting, increase the gains
|
||||||
// not oscillating or overshooting, increase the gains
|
|
||||||
if (max_P < low_PD) {
|
The increase is based on how far we are below the slew
|
||||||
// P is very small, increase rapidly
|
limit. At 80% of the limit we stop increasing gains, to
|
||||||
P *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
|
give some margin. Below 25% of the limit we apply max
|
||||||
} else {
|
increase
|
||||||
P *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
|
*/
|
||||||
}
|
const float slew_limit = rpid.slew_limit();
|
||||||
if (max_D < low_PD) {
|
const float gain_mul = (100+AUTOTUNE_INCREASE_PD_STEP)*0.01;
|
||||||
// D is very small, increase rapidly
|
const float PD_mul = linear_interpolate(gain_mul, 1,
|
||||||
D *= (100 + AUTOTUNE_INCREASE_PD_LOW_STEP)*0.01;
|
max_SRate,
|
||||||
} else {
|
0.2*slew_limit, 0.6*slew_limit);
|
||||||
D *= (100 + AUTOTUNE_INCREASE_PD_STEP)*0.01;
|
P *= PD_mul;
|
||||||
}
|
D *= PD_mul;
|
||||||
action = Action::RAISE_PD;
|
action = Action::RAISE_PD;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -380,6 +382,7 @@ void AP_AutoTune::update(AP_Logger::PID_Info &pinfo, float scaler)
|
||||||
update_rmax();
|
update_rmax();
|
||||||
|
|
||||||
min_Dmod = 1;
|
min_Dmod = 1;
|
||||||
|
max_SRate = 0;
|
||||||
max_P = max_D = 0;
|
max_P = max_D = 0;
|
||||||
state = new_state;
|
state = new_state;
|
||||||
state_enter_ms = now;
|
state_enter_ms = now;
|
||||||
|
|
|
@ -117,5 +117,6 @@ private:
|
||||||
float max_P;
|
float max_P;
|
||||||
float max_D;
|
float max_D;
|
||||||
float min_Dmod;
|
float min_Dmod;
|
||||||
|
float max_SRate;
|
||||||
float FF_single;
|
float FF_single;
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue