From 6956d85e104397b711e02e780331ba6f5d6c9547 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 17 Apr 2019 11:52:08 +0200 Subject: [PATCH] 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. --- src/modules/mc_att_control/mc_att_control_main.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 2f268eb455..06a256ce1f 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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;