FlightModeManager: handle commands all the time

Moving the command handling to a separate function that gets called
whenever a vehicle command is available to always react on commands
and not just when already a task is running.

This solves e.g. commanding an Orbit when in Staibilized.

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,11 +437,8 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
 	_vehicle_command_pub.publish(command);
 }
 
-void FlightModeManager::generateTrajectorySetpoint(const float dt,
+void FlightModeManager::handleCommand()
-		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);
@@ -479,7 +480,9 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	}
 }
 
-
+void FlightModeManager::generateTrajectorySetpoint(const float dt,
+		const vehicle_local_position_s &vehicle_local_position)
+{
 	_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;
 

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);