From 9c6cc7a36b3391088510d6301ed6472d487710b2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 Jan 2014 10:26:43 +0100 Subject: mc_pos_control: AWU fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 18 ++++++++++-------- 1 file 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) { -- cgit v1.2.3