From 9297a02ac29b66cc3628e9fa3294797e6c10fbde Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 20 Jan 2016 09:10:48 +0100 Subject: [PATCH] move code which clears the pos and alt hold flags to the correct place. this should fix the problem of the jump in the altitude setpoint when switching into altitude hold mode. --- .../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 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 e30d74fbab..262f3fa566 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1254,6 +1254,16 @@ MulticopterPositionControl::task_main() _vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); } + // reset the horizontal and vertical position hold flags for non-manual modes + // or if position / altitude is not controlled + if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { + _pos_hold_engaged = false; + } + + if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { + _alt_hold_engaged = false; + } + if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || @@ -1266,16 +1276,6 @@ MulticopterPositionControl::task_main() _run_pos_control = true; _run_alt_control = true; - // reset the horizontal and vertical position hold flags for non-manual modes - // or if position is not controlled - if (!_control_mode.flag_control_position_enabled || !_control_mode.flag_control_manual_enabled) { - _pos_hold_engaged = false; - } - - if (!_control_mode.flag_control_altitude_enabled || !_control_mode.flag_control_manual_enabled) { - _alt_hold_engaged = false; - } - /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */