dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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:
Beat Küng 2019-04-17 11:52:08 +02:00 committed by Daniel Agar
parent 011f4990ff
commit 6956d85e10
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
1 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -644,8 +644,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;