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;