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 */