diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 2ee7977e3a..8082abdcff 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -682,15 +682,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _was_in_air = false; } + /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ + if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { + /* reset integrators */ + _tecs.reset_state(); + } + if (_control_mode.flag_control_auto_enabled && pos_sp_curr.valid) { /* AUTONOMOUS FLIGHT */ - /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ - if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { - /* reset integrators */ - _tecs.reset_state(); - } - _control_mode_current = FW_POSCTRL_MODE_AUTO; /* reset hold altitude */ @@ -1222,12 +1222,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _att_sp.yaw_body = 0; } - /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ - if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { - /* reset integrators */ - _tecs.reset_state(); - } - _control_mode_current = FW_POSCTRL_MODE_POSITION; float altctrl_airspeed = get_demanded_airspeed(); @@ -1329,12 +1323,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons _hold_alt = _global_pos.alt; } - /* Reset integrators if switching to this mode from a other mode in which posctl was not active */ - if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { - /* reset integrators */ - _tecs.reset_state(); - } - _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; /* Get demanded airspeed */