fix orbit for mc: handle VEHICLE_CMD_DO_ORBIT command and avoid race condition

Fixes a regression from 8bae4e5c0e, where
the orbit flight task wasn't an extra task (flight_tasks_to_add) anymore
and therefore the command handling wasn't generated.

There was a race condition that could cause several outcomes. The most severe
was that flight_mode_manager gets the command, switches to orbit and then
in the next iteration switches back because commander did not change
nav_state yet. When commander then switches, flight_mode_manager would still
be in the old mode.
This is prevented by storing the command (allowing it to arrive before or
after mode switch), and then apply it after the switch happens.

src/modules/flight_mode_manager/FlightModeManager.hpp

@@ -111,9 +111,10 @@ private:
 	 */
 	bool isAnyTaskActive() const { return _current_task.task; }
 
+	void tryApplyCommandIfAny();
+
 	// generated
 	int _initTask(FlightTaskIndex task_index);
-	FlightTaskIndex switchVehicleCommand(const int command);
 
 	/**
 	 * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
@@ -135,6 +136,9 @@ private:
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter
 	hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
 
+	vehicle_command_s _current_command{};
+	bool _command_failed{false};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};