forked from Archive/PX4-Autopilot
mc rate controller: add I term reduction factor
Reduce the I gain for high rate errors to reduce bounce-back effects after flips. Up to 200 degrees the gain is almost not reduced (<25%), so this will only take noticeable effects for large errors (setpoint changes), where we actually want to have an effect. This allows to increase the MC_*RATE_I parameters w/o negative effects when doing flips (i.e. bounce-back after flips). The 400 degrees limit and the x^2 are empirical. The better the rate tracking in general (high P gain), the less this is required (because of the lower tracking error). At the same time it also does not harm, as the i_factor will always be close to 1.
This commit is contained in:
parent
4c4bcc5fdb
commit
ea31f34d09
|
@ -481,8 +481,17 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
|
|||
|
||||
}
|
||||
|
||||
// I term factor: reduce the I gain with increasing rate error.
|
||||
// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint
|
||||
// change (noticeable in a bounce-back effect after a flip).
|
||||
// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:
|
||||
// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),
|
||||
// and up to 200 deg error leads to <25% reduction of I.
|
||||
float i_factor = rates_err(i) / math::radians(400.f);
|
||||
i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
|
||||
|
||||
// Perform the integration using a first order method and do not propagate the result if out of range or invalid
|
||||
float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt;
|
||||
float rate_i = _rates_int(i) + i_factor * rates_i_scaled(i) * rates_err(i) * dt;
|
||||
|
||||
if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) {
|
||||
_rates_int(i) = rate_i;
|
||||
|
|
Loading…
Reference in New Issue