diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt
index f8b495e29f..bee40bbc66 100644
--- a/src/modules/flight_mode_manager/CMakeLists.txt
+++ b/src/modules/flight_mode_manager/CMakeLists.txt
@@ -71,13 +71,6 @@ set(python_args
 	-f ${files_to_generate}
 )
 
-# add the additional tasks for the python script (if there are any)
-if(flight_tasks_to_add)
-	list(APPEND python_args
-		-s ${flight_tasks_to_add}
-	)
-endif()
-
 # generate the files using the python script and template
 add_custom_command(
 	OUTPUT
diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index 8c70e09f12..5dbe29b726 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -114,6 +114,8 @@ void FlightModeManager::Run()
 			handleCommand();
 		}
 
+		tryApplyCommandIfAny();
+
 		if (isAnyTaskActive()) {
 			generateTrajectorySetpoint(dt, vehicle_local_position);
 		}
@@ -231,6 +233,19 @@ void FlightModeManager::start_flight_task()
 
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
 		should_disable_task = false;
+
+		if (!_command_failed) {
+			FlightTaskError error = FlightTaskError::InvalidTask;
+
+#if !defined(CONSTRAINED_FLASH)
+			error = switchTask(FlightTaskIndex::Orbit);
+#endif // !CONSTRAINED_FLASH
+
+			if (error != FlightTaskError::NoError) {
+				task_failure = true;
+			}
+
+		}
 	}
 
 	// check task failure
@@ -265,39 +280,39 @@ void FlightModeManager::start_flight_task()
 
 }
 
+void FlightModeManager::tryApplyCommandIfAny()
+{
+	if (isAnyTaskActive() && _current_command.command != 0 && hrt_absolute_time() < _current_command.timestamp + 200_ms) {
+		bool success = false;
+
+		if (_current_task.task->applyCommandParameters(_current_command, success)) {
+			_current_command.command = 0;
+
+			if (!success) {
+				switchTask(FlightTaskIndex::Failsafe);
+				_command_failed = true;
+			}
+		}
+	}
+}
+
 void FlightModeManager::handleCommand()
 {
 	// get command
 	vehicle_command_s command;
 
 	while (_vehicle_command_sub.update(&command)) {
-		// check what command it is
-		FlightTaskIndex desired_task = switchVehicleCommand(command.command);
 
-		// ignore all unknown commands
-		if (desired_task != FlightTaskIndex::None
-		    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			// switch to the commanded task
-			bool switch_succeeded = (switchTask(desired_task) == FlightTaskError::NoError);
-			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
+		switch (command.command) {
+		case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
+			// The command might trigger a mode switch, and the mode switch can happen before or
+			// after we receive the command here, so we store it for later.
+			memcpy(&_current_command, &command, sizeof(vehicle_command_s));
+			_command_failed = false;
+			break;
+		}
 
-			// if we are in/switched to the desired task
-			if (switch_succeeded) {
-				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_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_CMD_RESULT_DENIED;
-
-					// if we just switched and parameters are not accepted, go to failsafe
-					if (switch_succeeded) {
-						switchTask(FlightTaskIndex::Failsafe);
-					}
-				}
-			}
-
-
-		} else if (_current_task.task) {
+		if (_current_task.task) {
 			// check for other commands not related to task switching
 			if ((command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED)
 			    && (static_cast<uint8_t>(command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_GROUNDSPEED)
@@ -411,6 +426,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	_current_task.task->setResetCounters(last_reset_counters);
+	_command_failed = false;
 
 	return FlightTaskError::NoError;
 }
diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp
index 7f7aea1171..db5fb0bf56 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.hpp
+++ b/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)};
diff --git a/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em
index 16cedf4056..a1f38d14d1 100644
--- a/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em
+++ b/src/modules/flight_mode_manager/Templates/FlightTasks_generated.cpp.em
@@ -75,23 +75,3 @@ int FlightModeManager::_initTask(FlightTaskIndex task_index)
 	_current_task.index = task_index;
 	return 0;
 }
-
-FlightTaskIndex FlightModeManager::switchVehicleCommand(const int command)
-{
-    switch (command) {
-@# loop through all additional tasks
-@[if tasks_add]@
-@[for task in tasks_add]@
-@{
-upperCase = lambda s: s[:].upper() if s else ''
-}@
-	case vehicle_command_s::VEHICLE_CMD_DO_@(upperCase(task)) :
-		return FlightTaskIndex::@(task);
-		break;
-
-@[end for]@
-@[end if]@
-	// ignore all unknown commands
-	default : return FlightTaskIndex::None;
-	}
-}
diff --git a/src/modules/flight_mode_manager/generate_flight_tasks.py b/src/modules/flight_mode_manager/generate_flight_tasks.py
index 9bbb192f0e..99f6266b3a 100644
--- a/src/modules/flight_mode_manager/generate_flight_tasks.py
+++ b/src/modules/flight_mode_manager/generate_flight_tasks.py
@@ -6,7 +6,6 @@ import argparse
 
 parser = argparse.ArgumentParser()
 parser.add_argument("-t", "--tasks", dest='tasks_all', nargs='+', required=True, help="All tasks to be generated")
-parser.add_argument("-s", "--tasks_additional", dest='tasks_add', nargs='+', help="Additional tasks to be generated (on top of the core)")
 parser.add_argument("-i", "--input_directory", dest='directory_in', required=True, help="Output directory")
 parser.add_argument("-o", "--output_directory", dest='directory_out', required=True, help="Input directory")
 parser.add_argument("-f", "--files", dest='gen_files', nargs='+', required=True, help="Files to generate")
@@ -20,7 +19,6 @@ for gen_file in args.gen_files:
     # need to specify the em_globals inside the loop -> em.Error: interpreter globals collision
     em_globals = {
         "tasks": args.tasks_all,
-        "tasks_add": args.tasks_add,
     }
     interpreter = em.Interpreter(output=output_file, globals=em_globals)
     interpreter.file(open(args.directory_in + "/" + gen_file + ".em"))
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
index c76a83206a..1df5351fb4 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+++ b/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()
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index b1a0e70ffb..ea68b452fc 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/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()
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 900c8e469a..187a47de41 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/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;