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/tasks/FlightTask/FlightTask.hpp

@@ -91,9 +91,10 @@ public:
 	/**
 	 * To be called to adopt parameters from an arrived vehicle command
 	 * @param command received command message containing the parameters
-	 * @return true if accepted, false if declined
+	 * @param success set to true if it was successfully applied, false on error
+	 * @return true if handled
 	 */
-	virtual bool applyCommandParameters(const vehicle_command_s &command) { return false; }
+	virtual bool applyCommandParameters(const vehicle_command_s &command, bool &success) { return false; }
 
 	/**
 	 * Call before activate() or update()

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp

@@ -49,9 +49,13 @@ FlightTaskOrbit::FlightTaskOrbit()
 	_sticks_data_required = false;
 }
 
-bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
+bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, bool &success)
 {
-	bool ret = true;
+	if (command.command != vehicle_command_s::VEHICLE_CMD_DO_ORBIT) {
+		return false;
+	}
+
+	success = true;
 	// save previous velocity and rotation direction
 	bool new_is_clockwise = _orbit_velocity > 0;
 	float new_radius = _orbit_radius;
@@ -81,7 +85,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 	} else {
 		mavlink_log_critical(&_mavlink_log_pub, "Orbit radius limit exceeded\t");
 		events::send(events::ID("orbit_radius_exceeded"), events::Log::Alert, "Orbit radius limit exceeded");
-		ret = false;
+		success = false;
 	}
 
 	// commanded heading behaviour
@@ -98,7 +102,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 			_center.xy() = _geo_projection.project(command.param5, command.param6);
 
 		} else {
-			ret = false;
+			success = false;
 		}
 	}
 
@@ -108,7 +112,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 			_center(2) = _global_local_alt0 - command.param7;
 
 		} else {
-			ret = false;
+			success = false;
 		}
 	}
 
@@ -118,7 +122,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 		_position_smoothing.reset(_acceleration_setpoint, _velocity_setpoint, _position);
 	}
 
-	return ret;
+	return true;
 }
 
 bool FlightTaskOrbit::sendTelemetry()

src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp

@@ -56,7 +56,7 @@ public:
 	FlightTaskOrbit();
 	virtual ~FlightTaskOrbit() = default;
 
-	bool applyCommandParameters(const vehicle_command_s &command) override;
+	bool applyCommandParameters(const vehicle_command_s &command, bool &success) override;
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool update() override;