diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index b7ed48db67..dce75b15ad 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -141,6 +141,10 @@ void FlightModeManager::Run()
 
 		start_flight_task();
 
+		if (_vehicle_command_sub.updated()) {
+			handleCommand();
+		}
+
 		if (isAnyTaskActive()) {
 			generateTrajectorySetpoint(dt, vehicle_local_position);
 		}
@@ -433,53 +437,52 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 	_vehicle_command_pub.publish(command);
 }
 
+void FlightModeManager::handleCommand()
+{
+	// get command
+	vehicle_command_s command{};
+	_vehicle_command_sub.copy(&command);
+
+	// check what command it is
+	FlightTaskIndex desired_task = switchVehicleCommand(command.command);
+
+	// ignore all unkown commands
+	if (desired_task != FlightTaskIndex::None) {
+		// switch to the commanded task
+		FlightTaskError switch_result = switchTask(desired_task);
+		uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+
+		// if we are in/switched to the desired task
+		if (switch_result >= FlightTaskError::NoError) {
+			cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+
+			// if the task is running apply parameters to it and see if it rejects
+			if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+
+				// if we just switched and parameters are not accepted, go to failsafe
+				if (switch_result >= FlightTaskError::NoError) {
+					switchTask(FlightTaskIndex::ManualPosition);
+					cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+				}
+			}
+		}
+
+		// send back acknowledgment
+		vehicle_command_ack_s command_ack{};
+		command_ack.command = command.command;
+		command_ack.result = cmd_result;
+		command_ack.result_param1 = static_cast<int>(switch_result);
+		command_ack.target_system = command.source_system;
+		command_ack.target_component = command.source_component;
+		command_ack.timestamp = hrt_absolute_time();
+		_vehicle_command_ack_pub.publish(command_ack);
+	}
+}
+
 void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		const vehicle_local_position_s &vehicle_local_position)
 {
-	// In case flight task was not able to update correctly we send the empty setpoint which makes the position controller failsafe.
-	if (_vehicle_command_sub.updated()) {
-		// get command
-		vehicle_command_s command{};
-		_vehicle_command_sub.copy(&command);
-
-		// check what command it is
-		FlightTaskIndex desired_task = switchVehicleCommand(command.command);
-
-		// ignore all unkown commands
-		if (desired_task != FlightTaskIndex::None) {
-			// switch to the commanded task
-			FlightTaskError switch_result = switchTask(desired_task);
-			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-
-			// if we are in/switched to the desired task
-			if (switch_result >= FlightTaskError::NoError) {
-				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
-
-				// if the task is running apply parameters to it and see if it rejects
-				if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
-					cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
-
-					// if we just switched and parameters are not accepted, go to failsafe
-					if (switch_result >= FlightTaskError::NoError) {
-						switchTask(FlightTaskIndex::ManualPosition);
-						cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
-					}
-				}
-			}
-
-			// send back acknowledgment
-			vehicle_command_ack_s command_ack{};
-			command_ack.command = command.command;
-			command_ack.result = cmd_result;
-			command_ack.result_param1 = static_cast<int>(switch_result);
-			command_ack.target_system = command.source_system;
-			command_ack.target_component = command.source_component;
-			command_ack.timestamp = hrt_absolute_time();
-			_vehicle_command_ack_pub.publish(command_ack);
-		}
-	}
-
-
 	_current_task.task->setYawHandler(_wv_controller);
 
 	// Inform FlightTask about the input and output of the velocity controller
@@ -494,7 +497,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 		}
 	}
 
-	// initialize with empty NAN setpoints, if the task fails they get sent out and controller's emergency failsafe kicks in
+	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
 	vehicle_local_position_setpoint_s setpoint = FlightTask::empty_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;
 
diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp
index 17c640756f..04b67278e4 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.hpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.hpp
@@ -90,6 +90,7 @@ private:
 	void start_flight_task();
 	void check_failure(bool task_failure, uint8_t nav_state);
 	void send_vehicle_cmd_do(uint8_t nav_state);
+	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
 	void limitAltitude(vehicle_local_position_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
 	void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);