From bea54d7c9385f42807e9ce8e6c7caf06c223d84f 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 1/2] 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 4db941683377..ec6472afdba6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -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; From 848783041ece61129b4110e898b238afbeabb28c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 17 Jun 2019 10:52:41 +0200 Subject: [PATCH 2/2] mc airframes: increase I gains a bit Previous commit allows to increase them again. Partially reverts commit 6c3e79f3614f0060d5998c39be358512101252ff. --- ROMFS/px4fmu_common/init.d/airframes/4041_beta75x | 2 +- ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 | 2 +- ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x index be3c9afba5a4..2f4949b49ad3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x +++ b/ROMFS/px4fmu_common/init.d/airframes/4041_beta75x @@ -31,7 +31,7 @@ then param set MC_AIRMODE 2 param set MC_PITCHRATE_D 0.0010 - param set MC_PITCHRATE_I 0.4 + param set MC_PITCHRATE_I 0.5 param set MC_PITCHRATE_MAX 600 param set MC_PITCHRATE_P 0.0750 param set MC_PITCH_P 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 index 324d6f7f794f..2b465501fb4a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 @@ -17,7 +17,7 @@ if [ $AUTOCNF = yes ] then param set MC_ROLL_P 8 param set MC_ROLLRATE_P 0.08 - param set MC_ROLLRATE_I 0.2 + param set MC_ROLLRATE_I 0.25 param set MC_ROLLRATE_D 0.001 param set MC_PITCH_P 8 param set MC_PITCHRATE_P 0.08 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 index d720fb0c74c0..07ce82eea839 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 +++ b/ROMFS/px4fmu_common/init.d/airframes/4052_holybro_qav250 @@ -26,12 +26,12 @@ then param set MC_AIRMODE 1 param set MC_PITCHRATE_D 0.0012 - param set MC_PITCHRATE_I 0.25 + param set MC_PITCHRATE_I 0.35 param set MC_PITCHRATE_MAX 1200 param set MC_PITCHRATE_P 0.082 param set MC_PITCH_P 8 param set MC_ROLLRATE_D 0.0012 - param set MC_ROLLRATE_I 0.2 + param set MC_ROLLRATE_I 0.3 param set MC_ROLLRATE_MAX 1200 param set MC_ROLLRATE_P 0.076 param set MC_ROLL_P 8