diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
index fcd5ce5539..830d89b98c 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp
@@ -687,6 +687,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 
 	_skipping_takeoff_detection = false;
 
+	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
+		_control_mode_current = FW_POSCTRL_MODE_AUTO_VELOCITY;
+		return;
+	}
+
 	if (_control_mode.flag_control_offboard_enabled && _position_setpoint_current_valid
 	    && _control_mode.flag_control_position_enabled) {
 		if (PX4_ISFINITE(_pos_sp_triplet.current.vx) && PX4_ISFINITE(_pos_sp_triplet.current.vy)
@@ -892,10 +897,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
 		break;
 
-	case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
-		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
-		break;
-
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
 #ifdef CONFIG_FIGURE_OF_EIGHT
 		if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
@@ -1003,10 +1004,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
 {
 	uint8_t position_sp_type = pos_sp_curr.type;
 
-	if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
-		return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
-	}
-
 	Vector2d curr_wp{0, 0};
 
 	/* current waypoint (the one currently heading for) */
@@ -2594,6 +2591,11 @@ FixedwingPositionControl::Run()
 				break;
 			}
 
+		case FW_POSCTRL_MODE_AUTO_VELOCITY: {
+				control_auto_velocity(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
+				break;
+			}
+
 		case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
 				control_auto_fixed_bank_alt_hold(control_interval);
 				break;
diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
index 84300ae1b4..148b94457f 100644
--- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp
@@ -255,6 +255,7 @@ private:
 		FW_POSCTRL_MODE_AUTO_TAKEOFF,
 		FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
 		FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
+		FW_POSCTRL_MODE_AUTO_VELOCITY,
 		FW_POSCTRL_MODE_AUTO_PATH,
 		FW_POSCTRL_MODE_MANUAL_POSITION,
 		FW_POSCTRL_MODE_MANUAL_ALTITUDE,