FixedwingPositionControl combine TECS resets

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