diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-31 16:46:54 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-31 16:46:54 +0100 |
| commit | d933d523eb74ee2290c56afcd11fe8e85c6e702b (patch) | |
| tree | b3b3dacae9d351efdaa0b75e521072e42a9f7060 | |
| parent | 5a7a356c2038544efbde562398aa5a50cb7e19d9 (diff) | |
| download | px4-firmware-d933d523eb74ee2290c56afcd11fe8e85c6e702b.tar.gz px4-firmware-d933d523eb74ee2290c56afcd11fe8e85c6e702b.tar.bz2 px4-firmware-d933d523eb74ee2290c56afcd11fe8e85c6e702b.zip | |
mc_att_control: att rate integral fix
| -rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 12 |
1 files changed, 7 insertions, 5 deletions
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 4086a227a..a0accb855 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -654,13 +654,15 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f && _att_control.length() < _thrust_sp) { + if (_thrust_sp > 0.1f) { for (int i = 0; i < 3; i++) { - float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; + if (fabsf(_att_control(i)) < _thrust_sp) { + float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; - if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { - _rates_int(i) = rate_i; + if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _rates_int(i) = rate_i; + } } } } |