diff options
| author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 10:26:43 +0100 |
|---|---|---|
| committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-20 10:26:43 +0100 |
| commit | 9c6cc7a36b3391088510d6301ed6472d487710b2 (patch) | |
| tree | 28a4a18153c6389409287184090292b112956555 /src/modules/mc_pos_control/mc_pos_control_main.cpp | |
| parent | f4edb448dd77088d12a1d37468014e1f21a97ad7 (diff) | |
| download | px4-firmware-9c6cc7a36b3391088510d6301ed6472d487710b2.tar.gz px4-firmware-9c6cc7a36b3391088510d6301ed6472d487710b2.tar.bz2 px4-firmware-9c6cc7a36b3391088510d6301ed6472d487710b2.zip | |
mc_pos_control: AWU fixed
Diffstat (limited to 'src/modules/mc_pos_control/mc_pos_control_main.cpp')
| -rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 18 |
1 files changed, 10 insertions, 8 deletions
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index eb4d1f75e..c77d25b86 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -828,16 +828,18 @@ MulticopterPositionControl::task_main() } /* update integrals */ - math::Vector<3> m; - m(0) = (_control_mode.flag_control_velocity_enabled && !saturation_xy) ? 1.0f : 0.0f; - m(1) = m(0); - m(2) = (_control_mode.flag_control_climb_rate_enabled && !saturation_z) ? 1.0f : 0.0f; + if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { + thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; + thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; + } - thrust_int += vel_err.emult(_params.vel_i) * dt; + if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { + thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; - /* protection against flipping on ground when landing */ - if (thrust_int(2) > 0.0f) - thrust_int(2) = 0.0f; + /* protection against flipping on ground when landing */ + if (thrust_int(2) > 0.0f) + thrust_int(2) = 0.0f; + } /* calculate attitude and thrust from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { |