fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 14:47:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_att_control: att rate integral fix

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-31 16:46:54 +01:00
parent 5a7a356c20
commit d933d523eb
1 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+7 -5
View File
@@ -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;
}
}
}
}