commander: implement external modes and mode executors

2022-11-07 15:57:51 +01:00 · 2022-11-07 15:57:51 +01:00 · 9cb8245851
parent 1ad5a9de08
commit 9cb8245851
32 changed files with 1629 additions and 51 deletions
--- a/msg/ArmingCheckReply.msg
+++ b/msg/ArmingCheckReply.msg
@ -0,0 +1,34 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint8 request_id
+uint8 registration_id
+
+uint8 HEALTH_COMPONENT_INDEX_NONE = 0
+uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19
+
+uint8 health_component_index      # HEALTH_COMPONENT_INDEX_*
+bool health_component_is_present
+bool health_component_warning
+bool health_component_error
+
+bool can_arm_and_run              # whether arming is possible, and if it's a navigation mode, if it can run
+
+uint8 num_events
+
+Event[5] events
+
+# Mode requirements
+bool mode_req_angular_velocity
+bool mode_req_attitude
+bool mode_req_local_alt
+bool mode_req_local_position
+bool mode_req_local_position_relaxed
+bool mode_req_global_position
+bool mode_req_mission
+bool mode_req_home_position
+bool mode_req_prevent_arming
+bool mode_req_manual_control
+
+
+uint8 ORB_QUEUE_LENGTH = 4
+
--- a/msg/ArmingCheckRequest.msg
+++ b/msg/ArmingCheckRequest.msg
@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+# broadcast message to request all registered arming checks to be reported
+
+uint8 request_id
+
+
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@ -49,6 +49,8 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
+	ArmingCheckReply.msg
+	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg
@ -161,6 +163,8 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
+	RegisterExtComponentReply.msg
+	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@ -198,6 +202,7 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UnregisterExtComponent.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
--- a/msg/RegisterExtComponentReply.msg
+++ b/msg/RegisterExtComponentReply.msg
@ -0,0 +1,14 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id          # ID from the request
+char[25] name              # name from the request
+
+uint16 px4_ros2_api_version
+
+bool success
+int8 arming_check_id      # arming check registration ID (-1 if invalid)
+int8 mode_id              # assigned mode ID (-1 if invalid)
+int8 mode_executor_id     # assigned mode executor ID (-1 if invalid)
+
+uint8 ORB_QUEUE_LENGTH = 2
+
--- a/msg/RegisterExtComponentRequest.msg
+++ b/msg/RegisterExtComponentRequest.msg
@ -0,0 +1,21 @@
+# Request to register an external component
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id                  # ID, set this to a random value
+char[25] name                      # either the requested mode name, or component name
+
+uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change.
+
+uint16 px4_ros2_api_version   # Set to LATEST_PX4_ROS2_API_VERSION
+
+# Components to be registered
+bool register_arming_check
+bool register_mode                 # registering a mode also requires arming_check to be set
+bool register_mode_executor        # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
+
+bool enable_replace_internal_mode  # set to true if an internal mode should be replaced
+uint8 replace_internal_mode        # vehicle_status::NAVIGATION_STATE_*
+bool activate_mode_immediately     # switch to the registered mode (can only be set in combination with an executor)
+
+
+uint8 ORB_QUEUE_LENGTH = 2
--- a/msg/UnregisterExtComponent.msg
+++ b/msg/UnregisterExtComponent.msg
@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+char[25] name                      # either the mode name, or component name
+
+int8 arming_check_id      # arming check registration ID (-1 if not registered)
+int8 mode_id              # assigned mode ID (-1 if not registered)
+int8 mode_executor_id     # assigned mode executor ID (-1 if not registered)
+
+
+
--- a/msg/VehicleCommand.msg
+++ b/msg/VehicleCommand.msg
@ -174,8 +174,10 @@ uint32 command			# Command ID
 uint8 target_system		# System which should execute the command
 uint8 target_component		# Component which should execute the command, 0 for all components
 uint8 source_system		# System sending the command
-uint8 source_component		# Component sending the command
+uint16 source_component	# Component / mode executor sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external

-# TOPICS vehicle_command gimbal_v1_command
+uint16 COMPONENT_MODE_EXECUTOR_START = 1000
+
+# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
--- a/msg/VehicleCommandAck.msg
+++ b/msg/VehicleCommandAck.msg
@ -28,6 +28,6 @@ uint8 result						# Command result
 uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
 int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
-uint8 target_component
+uint16 target_component 				# Target component / mode executor

 bool from_external					# Indicates if the command came from an external source
--- a/msg/VehicleControlMode.msg
+++ b/msg/VehicleControlMode.msg
@ -15,3 +15,8 @@ bool flag_control_altitude_enabled		# true if altitude is controlled
 bool flag_control_climb_rate_enabled		# true if climb rate is controlled
 bool flag_control_termination_enabled		# true if flighttermination is enabled
 bool flag_control_allocation_enabled		# true if control allocation is enabled
+
+# TODO: use dedicated topic for external requests
+uint8 source_id                  # Mode ID (nav_state)
+
+# TOPICS vehicle_control_mode config_control_setpoints
--- a/msg/VehicleStatus.msg
+++ b/msg/VehicleStatus.msg
@ -52,7 +52,17 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
 uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
+uint8 NAVIGATION_STATE_EXTERNAL1 = 23
+uint8 NAVIGATION_STATE_EXTERNAL2 = 24
+uint8 NAVIGATION_STATE_EXTERNAL3 = 25
+uint8 NAVIGATION_STATE_EXTERNAL4 = 26
+uint8 NAVIGATION_STATE_EXTERNAL5 = 27
+uint8 NAVIGATION_STATE_EXTERNAL6 = 28
+uint8 NAVIGATION_STATE_EXTERNAL7 = 29
+uint8 NAVIGATION_STATE_EXTERNAL8 = 30
+uint8 NAVIGATION_STATE_MAX = 31
+
+uint8 executor_in_charge                        # Current mode executor in charge (0=Autopilot)

 # Bitmask of detected failures
 uint16 failure_detector_status
--- a/src/lib/events/enums.json
+++ b/src/lib/events/enums.json
@ -110,6 +110,38 @@
                        "4194304": {
                            "name": "vtol_takeoff",
                            "description": "VTOL Takeoff"
+                        },
+                        "8388608": {
+                            "name": "external1",
+                            "description": "External 1"
+                        },
+                        "16777216": {
+                            "name": "external2",
+                            "description": "External 2"
+                        },
+                        "33554432": {
+                            "name": "external3",
+                            "description": "External 3"
+                        },
+                        "67108864": {
+                            "name": "external4",
+                            "description": "External 4"
+                        },
+                        "134217728": {
+                            "name": "external5",
+                            "description": "External 5"
+                        },
+                        "268435456": {
+                            "name": "external6",
+                            "description": "External 6"
+                        },
+                        "536870912": {
+                            "name": "external7",
+                            "description": "External 7"
+                        },
+                        "1073741824": {
+                            "name": "external8",
+                            "description": "External 8"
                        }
                    }
                },
@ -532,6 +564,38 @@
                            "name": "auto_vtol_takeoff",
                            "description": "Vtol Takeoff"
                        },
+                        "16": {
+                            "name": "external1",
+                            "description": "External 1"
+                        },
+                        "17": {
+                            "name": "external2",
+                            "description": "External 2"
+                        },
+                        "18": {
+                            "name": "external3",
+                            "description": "External 3"
+                        },
+                        "19": {
+                            "name": "external4",
+                            "description": "External 4"
+                        },
+                        "20": {
+                            "name": "external5",
+                            "description": "External 5"
+                        },
+                        "21": {
+                            "name": "external6",
+                            "description": "External 6"
+                        },
+                        "22": {
+                            "name": "external7",
+                            "description": "External 7"
+                        },
+                        "23": {
+                            "name": "external8",
+                            "description": "External 8"
+                        },
                        "255": {
                            "name": "unknown",
                            "description": "[Unknown]"
@ -705,7 +769,15 @@
                    "19": [134479872],
                    "20": [151257088],
                    "21": [16973824],
-                    "22": [168034304]
+                    "22": [168034304],
+                    "23": [184811520],
+                    "24": [201588736],
+                    "25": [218365952],
+                    "26": [235143168],
+                    "27": [251920384],
+                    "28": [268697600],
+                    "29": [285474816],
+                    "30": [302252032]
                }
            },
            "supported_protocols": [ "health_and_arming_check" ]
--- a/src/modules/commander/CMakeLists.txt
+++ b/src/modules/commander/CMakeLists.txt
@ -52,6 +52,7 @@ px4_add_module(
 		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		HomePosition.cpp
+		ModeManagement.cpp
 		UserModeIntention.cpp
 		level_calibration.cpp
 		lm_fit.cpp
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@ -407,6 +407,10 @@ int Commander::custom_command(int argc, char *argv[])
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);

+			} else if (!strcmp(argv[1], "ext1")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
+						     PX4_CUSTOM_SUB_MODE_EXTERNAL1);
+
 			} else {
 				PX4_ERR("argument %s unsupported.", argv[1]);
 			}
@ -475,8 +479,9 @@ int Commander::print_status()
 {
 	PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
 	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
-	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
 	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
+	_mode_management.printStatus();
 	perf_print_counter(_loop_perf);
 	perf_print_counter(_preflight_check_perf);
 	return 0;
@ -727,7 +732,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);

 			} else {
-				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 				} else {
@ -807,6 +812,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 							break;

+						case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
+							break;
+
 						default:
 							main_ret = TRANSITION_DENIED;
 							mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
@ -848,7 +857,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}

 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-				if (_user_mode_intention.change(desired_nav_state)) {
+				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@ -969,7 +978,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 	case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
 			/* switch to RTL which ends the mission */
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
 				events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -983,7 +992,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
 			/* ok, home set, use it to take off */
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 			} else {
@ -997,7 +1006,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 #if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF

 		/* ok, home set, use it to take off */
-		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) {
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) {
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 		} else {
@ -1011,7 +1020,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;

 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
@ -1025,7 +1034,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;

 	case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
 				events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
 					     "Landing using precision landing");
@ -1049,7 +1058,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {

 					// switch to AUTO_MISSION and ARM
-					if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
+					if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd))
 					    && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {

 						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -1088,7 +1097,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 			} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 				// for fixed wings the behavior of orbit is the same as loiter
-				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@ -1097,7 +1106,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 			} else {
 				// Switch to orbit state and let the orbit task handle the command further
-				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@ -1445,6 +1454,43 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	return true;
 }

+ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd)
+{
+	return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor :
+	       ModeChangeSource::User;
+}
+
+void Commander::handleCommandsFromModeExecutors()
+{
+	if (_vehicle_command_mode_executor_sub.updated()) {
+		const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation();
+		vehicle_command_s cmd;
+
+		if (_vehicle_command_mode_executor_sub.copy(&cmd)) {
+			if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation,
+					_vehicle_command_mode_executor_sub.get_last_generation());
+			}
+
+			// For commands from mode executors, we check if it is in charge and then publish it on the official
+			// command topic
+			const int mode_executor_in_charge = _mode_management.modeExecutorInCharge();
+
+			// source_system is set to the mode executor
+			if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) {
+				cmd.source_system = _vehicle_status.system_id;
+				cmd.timestamp = hrt_absolute_time();
+				_vehicle_command_pub.publish(cmd);
+
+			} else {
+				cmd.source_system = _vehicle_status.system_id;
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge);
+			}
+		}
+	}
+}
+
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@ -1569,7 +1615,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)

 	case action_request_s::ACTION_SWITCH_MODE:

-		if (!_user_mode_intention.change(action_request.mode, true)) {
+		if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) {
 			printRejectMode(action_request.mode);
 		}

@ -1717,6 +1763,8 @@ void Commander::run()
 			_status_changed = true;
 		}

+		modeManagementUpdate();
+
 		const hrt_abstime now = hrt_absolute_time();

 		const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
@ -1739,6 +1787,8 @@ void Commander::run()
 		}

 		// handle commands last, as the system needs to be updated to handle them
+		handleCommandsFromModeExecutors();
+
 		if (_vehicle_command_sub.updated()) {
 			// got command
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@ -1880,7 +1930,8 @@ void Commander::checkForMissionUpdate()

 		if (isArmed() && !_vehicle_land_detected.landed
 		    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
-		    && mission_result.finished) {
+		    && mission_result.finished
+		    && _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) {

 			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
 			    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
@ -2200,13 +2251,16 @@ bool Commander::handleModeIntentionAndFailsafe()

 	// Force intended mode if changed by the failsafe state machine
 	if (state.user_intended_mode != updated_user_intented_mode) {
-		_user_mode_intention.change(updated_user_intented_mode, false, true);
+		_user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true);
 		_user_mode_intention.getHadModeChangeAndClear();
 	}

 	// Handle failsafe action
-	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
-	_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get());
+	_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
+			false);
+	_vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(
+					    _failsafe.selectedAction(), _user_mode_intention.get()));
+	_vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge(); // Set this in sync with nav_state

 	switch (_failsafe.selectedAction()) {
 	case FailsafeBase::Action::Disarm:
@ -2248,6 +2302,21 @@ void Commander::checkAndInformReadyForTakeoff()
 #endif // CONFIG_ARCH_BOARD_PX4_SITL
 }

+void Commander::modeManagementUpdate()
+{
+	ModeManagement::UpdateRequest mode_management_update{};
+	_mode_management.update(isArmed(), _vehicle_status.nav_state_user_intention,
+				_failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update);
+
+	if (!isArmed() && mode_management_update.change_user_intended_nav_state) {
+		_user_mode_intention.change(mode_management_update.user_intended_nav_state);
+	}
+
+	if (mode_management_update.control_setpoint_update) {
+		_status_changed = true;
+	}
+}
+
 void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 {
 	switch (blink_msg_state()) {
@ -2393,8 +2462,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 void Commander::updateControlMode()
 {
 	_vehicle_control_mode = {};
-	mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state,
+
+	mode_util::getVehicleControlMode(_vehicle_status.nav_state,
 					 _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
+	_mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode);
+
+	_vehicle_control_mode.flag_armed = isArmed();
+	_vehicle_control_mode.flag_multicopter_position_control_enabled =
+		(_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
+		&& (_vehicle_control_mode.flag_control_altitude_enabled
+		    || _vehicle_control_mode.flag_control_climb_rate_enabled
+		    || _vehicle_control_mode.flag_control_position_enabled
+		    || _vehicle_control_mode.flag_control_velocity_enabled
+		    || _vehicle_control_mode.flag_control_acceleration_enabled);
 	_vehicle_control_mode.timestamp = hrt_absolute_time();
 	_vehicle_control_mode_pub.publish(_vehicle_control_mode);
 }
@ -2744,7 +2824,7 @@ void Commander::manualControlCheck()
 				if (override_enabled) {
 					// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
 					if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
-						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) {
+						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
 							tune_positive(true);
 							mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
 							events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
@ -2761,7 +2841,7 @@ void Commander::manualControlCheck()

 			// if there's never been a mode change force position control as initial state
 			if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) {
-				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true);
+				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true);
 			}
 		}
 	}
@ -2823,7 +2903,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@ -40,6 +40,7 @@
 #include "worker_thread.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
+#include "ModeManagement.hpp"
 #include "UserModeIntention.hpp"

 #include <lib/controllib/blocks.hpp>
@ -124,6 +125,7 @@ public:

 private:
 	bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); }
+	static ModeChangeSource getSourceFromCommand(const vehicle_command_s &cmd);

 	void answer_command(const vehicle_command_s &cmd, uint8_t result);

@ -190,6 +192,10 @@ private:

 	void checkAndInformReadyForTakeoff();

+	void handleCommandsFromModeExecutors();
+
+	void modeManagementUpdate();
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@ -211,8 +217,13 @@ private:
 	FailureDetector		_failure_detector{this};
 	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
 	Safety			_safety{};
-	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
 	WorkerThread 		_worker_thread{};
+	ModeManagement  	_mode_management{
+#ifndef CONSTRAINED_FLASH
+		_health_and_arming_checks.externalChecks()
+#endif
+	};
+	UserModeIntention	_user_mode_intention {this, _vehicle_status, _health_and_arming_checks, &_mode_management};

 	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
 	HomePosition 		_home_position{_failsafe_flags};
@ -276,6 +287,7 @@ private:
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription					_vehicle_command_mode_executor_sub{ORB_ID(vehicle_command_mode_executor)};
 	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};

@ -295,6 +307,7 @@ private:
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
 	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_s>			_vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};

--- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
@ -65,6 +65,7 @@ px4_add_library(health_and_arming_checks
 	checks/vtolCheck.cpp
 	checks/windCheck.cpp

+	checks/externalChecks.cpp
 )
 add_dependencies(health_and_arming_checks mode_util)

--- a/src/modules/commander/HealthAndArmingChecks/Common.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp
@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve
 	return header;
 }

+bool Report::addExternalEvent(const event_s &event, NavModes modes)
+{
+	unsigned args_size = sizeof(event.arguments);
+
+	// trim 0's
+	while (args_size > 0 && event.arguments[args_size - 1] == '\0') {
+		--args_size;
+	}
+
+	unsigned total_size = sizeof(EventBufferHeader) + args_size;
+
+	if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
+		_buffer_overflowed = true;
+		return false;
+	}
+
+	events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))};
+	memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size);
+	addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size);
+	return true;
+}
+
+
+
 NavModes Report::reportedModes(NavModes required_modes)
 {
 	// Make sure optional checks are still shown in the UI
--- a/src/modules/commander/HealthAndArmingChecks/Common.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp
@ -249,8 +249,8 @@ public:
 	void clearArmingBits(NavModes modes);

 	/**
-	 * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
-	 * mode is being run.
+	 * Clear can_run bits for certain modes. This will prevent mode switching.
+	 * For failsafe use the mode requirements instead, which then will clear the can_run bits.
 	 * @param modes affected modes
 	 */
 	void clearCanRunBits(NavModes modes);
@ -259,6 +259,8 @@ public:
 	const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }

 	bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
+
+	bool addExternalEvent(const event_s &event, NavModes modes);
 private:

 	/**
@ -307,6 +309,7 @@ private:
 	NavModes getModeGroup(uint8_t nav_state) const;

 	friend class HealthAndArmingChecks;
+	friend class ExternalChecks;
 	FRIEND_TEST(ReporterTest, basic_no_checks);
 	FRIEND_TEST(ReporterTest, basic_fail_all_modes);
 	FRIEND_TEST(ReporterTest, arming_checks_mode_category);
--- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
@ -69,6 +69,7 @@
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"
 #include "checks/openDroneIDCheck.hpp"
+#include "checks/externalChecks.hpp"

 class HealthAndArmingChecks : public ModuleParams
 {
@ -101,6 +102,10 @@ public:

 	const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }

+#ifndef CONSTRAINED_FLASH
+	ExternalChecks &externalChecks() { return _external_checks; }
+#endif
+
 protected:
 	void updateParams() override;
 private:
@ -143,8 +148,14 @@ private:
 	RcAndDataLinkChecks _rc_and_data_link_checks;
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
+#ifndef CONSTRAINED_FLASH
+	ExternalChecks _external_checks;
+#endif

-	HealthAndArmingCheckBase *_checks[31] = {
+	HealthAndArmingCheckBase *_checks[40] = {
+#ifndef CONSTRAINED_FLASH
+		&_external_checks,
+#endif
 		&_accelerometer_checks,
 		&_airspeed_checks,
 		&_arm_permission_checks,
@ -161,7 +172,7 @@ private:
 		&_home_position_checks,
 		&_mission_checks,
 		&_offboard_checks, // must be after _estimator_checks
-		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
+		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks, _external_checks
 		&_open_drone_id_checks,
 		&_parachute_checks,
 		&_power_checks,
--- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
@ -0,0 +1,335 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "externalChecks.hpp"
+
+static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits)
+{
+	if (requirement_set) {
+		bits |= 1u << nav_state;
+	}
+
+	if (replaces_nav_state != -1) {
+		if (requirement_set) {
+			bits |= 1u << replaces_nav_state;
+
+		} else {
+			bits &= ~(1u << replaces_nav_state);
+		}
+	}
+}
+
+int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state)
+{
+	int free_registration_index = -1;
+
+	for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
+		if (!registrationValid(i)) {
+			free_registration_index = i;
+			break;
+		}
+	}
+
+	if (free_registration_index != -1) {
+		_active_registrations_mask |= 1 << free_registration_index;
+		_registrations[free_registration_index].nav_mode_id = nav_mode_id;
+		_registrations[free_registration_index].replaces_nav_state = replaces_nav_state;
+		_registrations[free_registration_index].num_no_response = 0;
+		_registrations[free_registration_index].unresponsive = false;
+		_registrations[free_registration_index].total_num_unresponsive = 0;
+
+		if (!_registrations[free_registration_index].reply) {
+			_registrations[free_registration_index].reply = new arming_check_reply_s();
+		}
+	}
+
+	return free_registration_index;
+}
+
+bool ExternalChecks::removeRegistration(int registration_id, int8_t nav_mode_id)
+{
+	if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
+		return false;
+	}
+
+	if (registrationValid(registration_id)) {
+		if (_registrations[registration_id].nav_mode_id == nav_mode_id) {
+			_active_registrations_mask &= ~(1u << registration_id);
+			return true;
+		}
+	}
+
+	PX4_ERR("trying to remove inactive external check");
+	return false;
+}
+
+bool ExternalChecks::isUnresponsive(int registration_id)
+{
+	if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
+		return false;
+	}
+
+	if (registrationValid(registration_id)) {
+		return _registrations[registration_id].unresponsive;
+	}
+
+	return false;
+}
+
+
+void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
+{
+	checkNonRegisteredModes(context, reporter);
+
+	if (_active_registrations_mask == 0) {
+		return;
+	}
+
+	NavModes unresponsive_modes{NavModes::None};
+
+	for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
+		if (!registrationValid(reg_idx) || !_registrations[reg_idx].reply) {
+			continue;
+		}
+
+		arming_check_reply_s &reply = *_registrations[reg_idx].reply;
+
+		int8_t nav_mode_id = _registrations[reply.registration_id].nav_mode_id;
+
+		if (_registrations[reply.registration_id].unresponsive) {
+
+			if (nav_mode_id != -1) {
+				unresponsive_modes = unresponsive_modes | reporter.getModeGroup(nav_mode_id);
+				setOrClearRequirementBits(true, nav_mode_id, -1, reporter.failsafeFlags().mode_req_other);
+			}
+
+		} else {
+			NavModes modes;
+
+			// We distinguish between two cases:
+			// - external navigation mode: in that case we set the single arming can_run bit for the mode
+			// - generic external arming check: set all arming bits
+			if (nav_mode_id == -1) {
+				modes = NavModes::All;
+
+			} else {
+				modes = reporter.getModeGroup(nav_mode_id);
+
+				int8_t replaces_nav_state = _registrations[reply.registration_id].replaces_nav_state;
+
+				if (replaces_nav_state != -1) {
+					modes = modes | reporter.getModeGroup(replaces_nav_state);
+					// Also clear the arming bits for the replaced mode, as the user intention is always set to the
+					// replaced mode.
+					// We only have to clear the bits, as for the internal/replaced mode, the bits are not cleared yet.
+				}
+
+				if (!reply.can_arm_and_run) {
+					setOrClearRequirementBits(true, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_other);
+				}
+
+				// Mode requirements
+				// A replacement mode will also replace the mode requirements of the internal/replaced mode
+				setOrClearRequirementBits(reply.mode_req_angular_velocity, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_angular_velocity);
+				setOrClearRequirementBits(reply.mode_req_attitude, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_attitude);
+				setOrClearRequirementBits(reply.mode_req_local_alt, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_local_alt);
+				setOrClearRequirementBits(reply.mode_req_local_position, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_local_position);
+				setOrClearRequirementBits(reply.mode_req_local_position_relaxed, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_local_position_relaxed);
+				setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_global_position);
+				setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_mission);
+				setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_home_position);
+				setOrClearRequirementBits(reply.mode_req_prevent_arming, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_prevent_arming);
+				setOrClearRequirementBits(reply.mode_req_manual_control, nav_mode_id, replaces_nav_state,
+							  reporter.failsafeFlags().mode_req_manual_control);
+			}
+
+			if (!reply.can_arm_and_run) {
+				reporter.clearArmingBits(modes);
+			}
+
+			if (reply.health_component_index > 0) {
+				reporter.setHealth((health_component_t)(1ull << reply.health_component_index),
+						   reply.health_component_is_present, reply.health_component_warning,
+						   reply.health_component_error);
+			}
+
+			for (int i = 0; i < reply.num_events; ++i) {
+				// set the modes, which is the first argument
+				memcpy(reply.events[i].arguments, &modes, sizeof(modes));
+
+				reporter.addExternalEvent(reply.events[i], modes);
+			}
+		}
+	}
+
+	if (unresponsive_modes != NavModes::None) {
+		/* EVENT
+		 * @description
+		 * The application running the mode might have crashed or the CPU load is too high.
+		 */
+		reporter.armingCheckFailure(unresponsive_modes, health_component_t::system,
+					    events::ID("check_external_modes_unresponsive"),
+					    events::Log::Critical, "Mode is unresponsive");
+	}
+
+}
+
+void ExternalChecks::update()
+{
+	if (_active_registrations_mask == 0) {
+		return;
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	// Check for incoming replies
+	arming_check_reply_s reply;
+	int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;
+
+	while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
+		if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
+		    && _current_request_id == reply.request_id) {
+			_reply_received_mask |= 1u << reply.registration_id;
+			_registrations[reply.registration_id].num_no_response = 0;
+
+			// Prevent toggling between unresponsive & responsive state
+			if (_registrations[reply.registration_id].total_num_unresponsive <= 3) {
+				_registrations[reply.registration_id].unresponsive = false;
+			}
+
+			if (_registrations[reply.registration_id].reply) {
+				*_registrations[reply.registration_id].reply = reply;
+			}
+
+//			PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events);
+		}
+	}
+
+	if (_last_update > 0) {
+		if (_reply_received_mask == _active_registrations_mask) { // Got all responses
+			// Nothing to do
+		} else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout
+			_had_timeout = true;
+			unsigned no_reply = _active_registrations_mask & ~_reply_received_mask;
+
+			for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
+				if ((1u << i) & no_reply) {
+					if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) {
+						// Clear immediately if not a mode
+						if (_registrations[i].nav_mode_id == -1) {
+							removeRegistration(i, -1);
+							PX4_WARN("No response from %i, removing", i);
+
+						} else {
+							_registrations[i].unresponsive = true;
+
+							if (_registrations[i].total_num_unresponsive < 100) {
+								++_registrations[i].total_num_unresponsive;
+							}
+
+							PX4_WARN("No response from %i, flagging unresponsive", i);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// Start a new request?
+	if (now > _last_update + UPDATE_INTERVAL) {
+		_reply_received_mask = 0;
+		_last_update = now;
+		_had_timeout = false;
+
+		// Request the state from all registered components
+		arming_check_request_s request{};
+		request.request_id = ++_current_request_id;
+		request.timestamp = hrt_absolute_time();
+		_arming_check_request_pub.publish(request);
+	}
+}
+
+void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state)
+{
+	_first_external_nav_state = first_external_nav_state;
+	_last_external_nav_state = last_external_nav_state;
+}
+
+void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const
+{
+	// Clear the arming bits for all non-registered external modes.
+	// But only report if one of them is selected, so we don't need to generate the extra event in most cases.
+	bool report_mode_not_available = false;
+
+	for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state;
+	     ++external_nav_state) {
+		bool found = false;
+
+		for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
+			if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			if (external_nav_state == context.status().nav_state_user_intention) {
+				report_mode_not_available = true;
+			}
+
+			reporter.clearArmingBits(reporter.getModeGroup(external_nav_state));
+			setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other);
+		}
+	}
+
+	if (report_mode_not_available) {
+		/* EVENT
+		 * @description
+		 * The application running the mode is not started.
+		 */
+		reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention),
+					    health_component_t::system,
+					    events::ID("check_external_modes_unavailable"),
+					    events::Log::Error, "Mode is not registered");
+	}
+}
+
--- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp
@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+#include <uORB/topics/arming_check_request.h>
+#include <uORB/topics/arming_check_reply.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
+static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
+	      health_component_t::avoidance, "enum definition missmatch");
+
+class ExternalChecks : public HealthAndArmingCheckBase
+{
+public:
+	static constexpr int MAX_NUM_REGISTRATIONS = 8;
+
+	ExternalChecks() = default;
+	~ExternalChecks() = default;
+
+	void setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state);
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+	bool hasFreeRegistrations() const { return _active_registrations_mask != (1u << MAX_NUM_REGISTRATIONS) - 1; }
+	/**
+	 * Add registration
+	 * @param nav_mode_id associated mode, -1 if none
+	 * @param replaces_nav_state replaced mode, -1 if none
+	 * @return registration id, or -1
+	 */
+	int addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state);
+	bool removeRegistration(int registration_id, int8_t nav_mode_id);
+	void update();
+
+	bool isUnresponsive(int registration_id);
+
+private:
+	static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
+	static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
+	static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval");
+	static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL
+
+	void checkNonRegisteredModes(const Context &context, Report &reporter) const;
+
+	bool registrationValid(int reg_idx) const { return ((1u << reg_idx) & _active_registrations_mask) != 0; }
+
+	struct Registration {
+		~Registration() { delete reply; }
+
+		int8_t nav_mode_id{-1}; ///< associated mode, -1 if none
+		int8_t replaces_nav_state{-1};
+
+		uint8_t num_no_response{0};
+		bool unresponsive{false};
+		uint8_t total_num_unresponsive{0};
+		arming_check_reply_s *reply{nullptr};
+	};
+
+	unsigned _active_registrations_mask{0};
+	Registration _registrations[MAX_NUM_REGISTRATIONS] {};
+
+	uint8_t _first_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
+	uint8_t _last_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
+
+	// Current requests (async updates)
+	hrt_abstime _last_update{0};
+	unsigned _reply_received_mask{0};
+	bool _had_timeout{false};
+
+	uint8_t _current_request_id{0};
+
+	uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
+
+	uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
+};
--- a/src/modules/commander/ModeManagement.cpp
+++ b/src/modules/commander/ModeManagement.cpp
@ -0,0 +1,471 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef CONSTRAINED_FLASH
+
+#include "ModeManagement.hpp"
+
+#include <px4_platform_common/events.h>
+
+bool ModeExecutors::hasFreeExecutors() const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_mode_executors[i].valid) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor)
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_mode_executors[i].valid) {
+			_mode_executors[i] = executor;
+			_mode_executors[i].valid = true;
+			return i + FIRST_EXECUTOR_ID;
+		}
+	}
+
+	PX4_ERR("logic error");
+	return -1;
+}
+
+void ModeExecutors::removeExecutor(int id)
+{
+	if (valid(id)) {
+		_mode_executors[id - FIRST_EXECUTOR_ID].valid = false;
+	}
+}
+
+void ModeExecutors::printStatus(int executor_in_charge) const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (_mode_executors[i].valid) {
+			int executor_id = i + FIRST_EXECUTOR_ID;
+			PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state,
+				 executor_id == executor_in_charge ? "yes" : "no");
+
+		}
+	}
+}
+
+bool Modes::hasFreeExternalModes() const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_modes[i].valid) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+uint8_t Modes::addExternalMode(const Modes::Mode &mode)
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_modes[i].valid) {
+			_modes[i] = mode;
+			_modes[i].valid = true;
+			return i + FIRST_EXTERNAL_NAV_STATE;
+		}
+	}
+
+	PX4_ERR("logic error");
+	return -1;
+}
+
+bool Modes::removeExternalMode(uint8_t nav_state, const char *name)
+{
+	if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) {
+		_modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false;
+		return true;
+	}
+
+	PX4_ERR("trying to remove invalid mode %s", name);
+	return false;
+}
+
+void Modes::printStatus() const
+{
+	for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+		if (valid(i)) {
+			const Modes::Mode &cur_mode = mode(i);
+			PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i,
+				 cur_mode.name);
+
+			if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE
+			    && cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
+				PX4_INFO("  Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]);
+			}
+		}
+	}
+}
+
+ModeManagement::ModeManagement(ExternalChecks &external_checks)
+	: _external_checks(external_checks)
+{
+	_external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE);
+}
+
+void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
+{
+	register_ext_component_request_s request;
+	int max_updates = 5;
+
+	while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request)
+	       && --max_updates >= 0) {
+		request.name[sizeof(request.name) - 1] = '\0';
+		PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id,
+			  request.register_arming_check, request.register_mode, request.register_mode_executor);
+		register_ext_component_reply_s reply{};
+		reply.mode_executor_id = -1;
+		reply.mode_id = -1;
+		reply.arming_check_id = -1;
+		static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
+		memcpy(reply.name, request.name, sizeof(request.name));
+		reply.request_id = request.request_id;
+		reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
+
+		// validate
+		bool request_valid = true;
+
+		if (request.register_mode_executor && !request.register_mode) {
+			request_valid = false;
+		}
+
+		if (request.register_mode && !request.register_arming_check) {
+			request_valid = false;
+		}
+
+		reply.success = false;
+
+		if (request_valid) {
+			// check free space
+			reply.success = true;
+
+			if (request.register_arming_check && !_external_checks.hasFreeRegistrations()) {
+				PX4_WARN("No free slots for arming checks");
+				reply.success = false;
+			}
+
+			if (request.register_mode) {
+				if (!_modes.hasFreeExternalModes()) {
+					PX4_WARN("No free slots for modes");
+					reply.success = false;
+
+				} else if (request.enable_replace_internal_mode) {
+					// Check if another one already replaces the same mode
+					for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+						if (_modes.valid(i)) {
+							const Modes::Mode &cur_mode = _modes.mode(i);
+
+							if (cur_mode.replaces_nav_state == request.replace_internal_mode) {
+								// TODO: we could add priorities and allow the highest priority to do the replacement
+								PX4_ERR("Trying to replace an already replaced mode (%i)", request.replace_internal_mode);
+								reply.success = false;
+							}
+						}
+					}
+				}
+			}
+
+			if (request.register_mode_executor && !_mode_executors.hasFreeExecutors()) {
+				PX4_WARN("No free slots for executors");
+				reply.success = false;
+			}
+
+			// register component(s)
+			if (reply.success) {
+				int nav_mode_id = -1;
+
+				if (request.register_mode) {
+					Modes::Mode mode{};
+					strncpy(mode.name, request.name, sizeof(mode.name));
+
+					if (request.enable_replace_internal_mode) {
+						mode.replaces_nav_state = request.replace_internal_mode;
+					}
+
+					nav_mode_id = _modes.addExternalMode(mode);
+					reply.mode_id = nav_mode_id;
+				}
+
+				if (request.register_mode_executor) {
+					ModeExecutors::ModeExecutor executor{};
+					executor.owned_nav_state = nav_mode_id;
+					int registration_id = _mode_executors.addExecutor(executor);
+
+					if (nav_mode_id != -1) {
+						_modes.mode(nav_mode_id).mode_executor_registration_id = registration_id;
+					}
+
+					reply.mode_executor_id = registration_id;
+				}
+
+				if (request.register_arming_check) {
+					int8_t replace_nav_state = request.enable_replace_internal_mode ? request.replace_internal_mode : -1;
+					int registration_id = _external_checks.addRegistration(nav_mode_id, replace_nav_state);
+
+					if (nav_mode_id != -1) {
+						_modes.mode(nav_mode_id).arming_check_registration_id = registration_id;
+					}
+
+					reply.arming_check_id = registration_id;
+				}
+
+				// Activate the mode?
+				if (request.register_mode_executor && request.activate_mode_immediately && nav_mode_id != -1) {
+					update_request.change_user_intended_nav_state = true;
+					update_request.user_intended_nav_state = nav_mode_id;
+				}
+			}
+		}
+
+		reply.timestamp = hrt_absolute_time();
+		_register_ext_component_reply_pub.publish(reply);
+	}
+}
+
+void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request)
+{
+	unregister_ext_component_s request;
+	int max_updates = 5;
+
+	while (!update_request.change_user_intended_nav_state && _unregister_ext_component_sub.update(&request)
+	       && --max_updates >= 0) {
+		request.name[sizeof(request.name) - 1] = '\0';
+		PX4_DEBUG("got unregistration request: %s arming: %i mode: %i executor: %i", request.name,
+			  (int)request.arming_check_id, (int)request.mode_id, (int)request.mode_executor_id);
+
+		if (request.arming_check_id != -1) {
+			_external_checks.removeRegistration(request.arming_check_id, request.mode_id);
+		}
+
+		if (request.mode_id != -1) {
+			if (_modes.removeExternalMode(request.mode_id, request.name)) {
+				removeModeExecutor(request.mode_executor_id);
+				// else: if the mode was already removed (due to a timeout), the executor was also removed already
+			}
+
+			// If the removed mode is currently active, switch to Hold
+			if (user_intended_nav_state == request.mode_id) {
+				update_request.change_user_intended_nav_state = true;
+				update_request.user_intended_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
+			}
+		}
+	}
+}
+
+void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active,
+			    UpdateRequest &update_request)
+{
+	_failsafe_action_active = failsafe_action_active;
+	_external_checks.update();
+
+	bool allow_update_while_armed = false;
+#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
+	// For simulation, allow registering modes while armed for developer convenience
+	allow_update_while_armed = true;
+#endif
+
+	if (armed && !allow_update_while_armed) {
+		// Reject registration requests
+		register_ext_component_request_s request;
+
+		if (_register_ext_component_request_sub.update(&request)) {
+			PX4_ERR("Not accepting registration requests while armed");
+			register_ext_component_reply_s reply{};
+			reply.success = false;
+			static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
+			memcpy(reply.name, request.name, sizeof(request.name));
+			reply.request_id = request.request_id;
+			reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
+			reply.timestamp = hrt_absolute_time();
+			_register_ext_component_reply_pub.publish(reply);
+		}
+
+	} else {
+		// Check for unresponsive modes
+		for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+			if (_modes.valid(i)) {
+				const Modes::Mode &mode = _modes.mode(i);
+
+				// Remove only if not currently selected
+				if (user_intended_nav_state != i && _external_checks.isUnresponsive(mode.arming_check_registration_id)) {
+					PX4_DEBUG("Removing unresponsive mode %i", i);
+					_external_checks.removeRegistration(mode.arming_check_registration_id, i);
+					removeModeExecutor(mode.mode_executor_registration_id);
+					_modes.removeExternalMode(i, mode.name);
+				}
+			}
+		}
+
+		// As we're disarmed we can use the user intended mode, as no failsafe will be active
+		checkNewRegistrations(update_request);
+		checkUnregistrations(user_intended_nav_state, update_request);
+	}
+
+	update_request.control_setpoint_update = checkConfigControlSetpointUpdates();
+}
+
+void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state)
+{
+	// Update mode executor in charge
+	int mode_executor_for_intended_nav_state = -1;
+
+	if (_modes.valid(user_intended_nav_state)) {
+		mode_executor_for_intended_nav_state = _modes.mode(user_intended_nav_state).mode_executor_registration_id;
+	}
+
+	if (mode_executor_for_intended_nav_state == -1) {
+		// Not an owned mode: check source
+		if (source == ModeChangeSource::User) {
+			// Give control to the pilot
+			_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
+		}
+
+	} else {
+		// Switched into an owned mode: put executor in charge
+		_mode_executor_in_charge = mode_executor_for_intended_nav_state;
+	}
+}
+
+uint8_t ModeManagement::getNavStateReplacementIfValid(uint8_t nav_state, bool report_error)
+{
+	for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+		if (_modes.valid(i)) {
+			Modes::Mode &mode = _modes.mode(i);
+
+			if (mode.replaces_nav_state == nav_state) {
+				if (_external_checks.isUnresponsive(mode.arming_check_registration_id)) {
+					if (!mode.unresponsive_reported && report_error) {
+						mode.unresponsive_reported = true;
+						events::send(events::ID("commander_mode_fallback_internal"), events::Log::Critical,
+							     "External mode is unresponsive, falling back to internal");
+					}
+
+					return nav_state;
+
+				} else {
+					return i;
+				}
+			}
+		}
+	}
+
+	return nav_state;
+}
+
+uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
+{
+	if (_modes.valid(nav_state)) {
+		const Modes::Mode &mode = _modes.mode(nav_state);
+
+		if (mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
+			return mode.replaces_nav_state;
+		}
+	}
+
+	return nav_state;
+}
+
+void ModeManagement::removeModeExecutor(int mode_executor_id)
+{
+	if (mode_executor_id == -1) {
+		return;
+	}
+
+	if (_mode_executor_in_charge == mode_executor_id) {
+		_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
+	}
+
+	_mode_executors.removeExecutor(mode_executor_id);
+}
+
+int ModeManagement::modeExecutorInCharge() const
+{
+	if (_failsafe_action_active) {
+		return ModeExecutors::AUTOPILOT_EXECUTOR_ID;
+	}
+
+	return _mode_executor_in_charge;
+}
+
+bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const
+{
+	bool ret = false;
+
+	if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) {
+		if (_modes.valid(nav_state)) {
+			control_mode = _modes.mode(nav_state).config_control_setpoint;
+			ret = true;
+
+		} else {
+			Modes::Mode::setControlModeDefaults(control_mode);
+		}
+	}
+
+	return ret;
+}
+
+void ModeManagement::printStatus() const
+{
+	_modes.printStatus();
+	_mode_executors.printStatus(modeExecutorInCharge());
+}
+
+bool ModeManagement::checkConfigControlSetpointUpdates()
+{
+	bool had_update = false;
+	vehicle_control_mode_s config_control_setpoint;
+	int max_updates = 5;
+
+	while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) {
+		if (_modes.valid(config_control_setpoint.source_id)) {
+			_modes.mode(config_control_setpoint.source_id).config_control_setpoint = config_control_setpoint;
+			had_update = true;
+
+		} else {
+			if (!_invalid_mode_printed) {
+				PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id);
+				_invalid_mode_printed = true;
+			}
+		}
+	}
+
+	return had_update;
+}
+
+#endif /* CONSTRAINED_FLASH */
--- a/src/modules/commander/ModeManagement.hpp
+++ b/src/modules/commander/ModeManagement.hpp
@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/register_ext_component_request.h>
+#include <uORB/topics/register_ext_component_reply.h>
+#include <uORB/topics/unregister_ext_component.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+
+#include "ModeUtil/ui.hpp"
+#include "UserModeIntention.hpp"
+#include "HealthAndArmingChecks/checks/externalChecks.hpp"
+
+class ModeExecutors
+{
+public:
+	static constexpr int AUTOPILOT_EXECUTOR_ID = 0;
+	static constexpr int FIRST_EXECUTOR_ID = 1;
+	static constexpr int MAX_NUM = 5;
+
+	struct ModeExecutor {
+		uint8_t owned_nav_state{};
+		bool valid{false};
+	};
+
+	void printStatus(int executor_in_charge) const;
+
+	bool valid(int id) const { return id >= FIRST_EXECUTOR_ID && id < FIRST_EXECUTOR_ID + MAX_NUM && _mode_executors[id - FIRST_EXECUTOR_ID].valid; }
+	const ModeExecutor &executor(int id) const { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
+	ModeExecutor &executor(int id) { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
+
+	bool hasFreeExecutors() const;
+	int addExecutor(const ModeExecutor &executor);
+	void removeExecutor(int id);
+private:
+	ModeExecutor _mode_executors[MAX_NUM] {};
+};
+
+class Modes
+{
+public:
+	static constexpr uint8_t FIRST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
+	static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
+	static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1;
+
+	struct Mode {
+		Mode()
+		{
+			// Set defaults for control mode
+			setControlModeDefaults(config_control_setpoint);
+		}
+		static void setControlModeDefaults(vehicle_control_mode_s &config_control_setpoint_)
+		{
+			config_control_setpoint_.flag_control_position_enabled = true;
+			config_control_setpoint_.flag_control_velocity_enabled = true;
+			config_control_setpoint_.flag_control_altitude_enabled = true;
+			config_control_setpoint_.flag_control_climb_rate_enabled = true;
+			config_control_setpoint_.flag_control_acceleration_enabled = true;
+			config_control_setpoint_.flag_control_rates_enabled = true;
+			config_control_setpoint_.flag_control_attitude_enabled = true;
+			config_control_setpoint_.flag_control_allocation_enabled = true;
+		}
+
+		static constexpr uint8_t REPLACES_NAV_STATE_NONE = 0xff;
+
+		char name[sizeof(register_ext_component_request_s::name)] {};
+		bool valid{false};
+		uint8_t replaces_nav_state{REPLACES_NAV_STATE_NONE};
+		bool unresponsive_reported{false};
+		int arming_check_registration_id{-1};
+		int mode_executor_registration_id{-1};
+		vehicle_control_mode_s config_control_setpoint{};
+	};
+
+	void printStatus() const;
+
+	bool valid(uint8_t nav_state) const { return nav_state >= FIRST_EXTERNAL_NAV_STATE && nav_state <= LAST_EXTERNAL_NAV_STATE && _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid; }
+	Mode &mode(uint8_t nav_state) { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
+	const Mode &mode(uint8_t nav_state) const { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
+
+	bool hasFreeExternalModes() const;
+	uint8_t addExternalMode(const Mode &mode);
+	bool removeExternalMode(uint8_t nav_state, const char *name);
+
+private:
+	Mode _modes[MAX_NUM] {};
+};
+
+
+#ifndef CONSTRAINED_FLASH
+
+class ModeManagement : public ModeChangeHandler
+{
+public:
+	ModeManagement(ExternalChecks &external_checks);
+	~ModeManagement() = default;
+
+	struct UpdateRequest {
+		bool change_user_intended_nav_state{false};
+		uint8_t user_intended_nav_state{};
+		bool control_setpoint_update{false};
+	};
+
+	void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request);
+
+	/**
+	 * Mode executor ID for who is currently in charge (and can send commands etc).
+	 * This is ModeExecutors::AUTOPILOT_EXECUTOR_ID if no executor is in charge currently.
+	 */
+	int modeExecutorInCharge() const;
+
+	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override;
+	uint8_t getReplacedModeIfAny(uint8_t nav_state) override;
+
+	uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true);
+
+	bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const;
+
+	void printStatus() const;
+
+
+private:
+	bool checkConfigControlSetpointUpdates();
+	void checkNewRegistrations(UpdateRequest &update_request);
+	void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request);
+
+	void removeModeExecutor(int mode_executor_id);
+
+	uORB::Subscription _config_control_setpoints_sub{ORB_ID(config_control_setpoints)};
+	uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)};
+	uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)};
+	uORB::Publication<register_ext_component_reply_s> _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)};
+
+	ExternalChecks &_external_checks;
+	ModeExecutors _mode_executors;
+	Modes _modes;
+
+	bool _failsafe_action_active{false};
+	int _mode_executor_in_charge{ModeExecutors::AUTOPILOT_EXECUTOR_ID};
+
+	bool _invalid_mode_printed{false};
+};
+
+#else /* CONSTRAINED_FLASH */
+
+class ModeManagement : public ModeChangeHandler
+{
+public:
+	ModeManagement() = default;
+	~ModeManagement() = default;
+
+	struct UpdateRequest {
+		bool change_user_intended_nav_state{false};
+		uint8_t user_intended_nav_state{};
+		bool control_setpoint_update{false};
+	};
+
+	void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request) {}
+
+	int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; }
+
+	void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {}
+	uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; }
+
+	uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; }
+
+	bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const { return false; }
+
+	void printStatus() const {}
+
+	void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const
+	{
+		valid_nav_state_mask = mode_util::getValidNavStates();
+		can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
+	}
+
+	void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { }
+
+private:
+};
+
+#endif /* CONSTRAINED_FLASH */
--- a/src/modules/commander/ModeUtil/control_mode.cpp
+++ b/src/modules/commander/ModeUtil/control_mode.cpp
@ -42,11 +42,10 @@ static bool stabilization_required(uint8_t vehicle_type)
 	return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 }

-void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
+void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 			   const offboard_control_mode_s &offboard_control_mode,
 			   vehicle_control_mode_s &vehicle_control_mode)
 {
-	vehicle_control_mode.flag_armed = armed;
 	vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default

 	switch (nav_state) {
@ -163,17 +162,11 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_velocity_enabled = true;
 		break;

+	// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
 	default:
 		break;
 	}

-	vehicle_control_mode.flag_multicopter_position_control_enabled =
-		(vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
-		&& (vehicle_control_mode.flag_control_altitude_enabled
-		    || vehicle_control_mode.flag_control_climb_rate_enabled
-		    || vehicle_control_mode.flag_control_position_enabled
-		    || vehicle_control_mode.flag_control_velocity_enabled
-		    || vehicle_control_mode.flag_control_acceleration_enabled);
 }

 } // namespace mode_util
--- a/src/modules/commander/ModeUtil/control_mode.hpp
+++ b/src/modules/commander/ModeUtil/control_mode.hpp
@ -41,7 +41,7 @@
 namespace mode_util
 {

-void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
+void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 			   const offboard_control_mode_s &offboard_control_mode,
 			   vehicle_control_mode_s &vehicle_control_mode);

--- a/src/modules/commander/ModeUtil/conversions.hpp
+++ b/src/modules/commander/ModeUtil/conversions.hpp
@ -75,9 +75,25 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;

 	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return navigation_mode_t::external1;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return navigation_mode_t::external2;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return navigation_mode_t::external3;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return navigation_mode_t::external4;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return navigation_mode_t::external5;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return navigation_mode_t::external6;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
 	}

-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 23, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");

 	return navigation_mode_t::unknown;
 }
@ -104,7 +120,16 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
 	"AUTO_LAND",
 	"AUTO_FOLLOW_TARGET",
 	"AUTO_PRECLAND",
-	"ORBIT"
+	"ORBIT",
+	"AUTO_VTOL_TAKEOFF",
+	"EXTERNAL1",
+	"EXTERNAL2",
+	"EXTERNAL3",
+	"EXTERNAL4",
+	"EXTERNAL5",
+	"EXTERNAL6",
+	"EXTERNAL7",
+	"EXTERNAL8",
 };

 } // namespace mode_util
--- a/src/modules/commander/ModeUtil/mode_requirements.cpp
+++ b/src/modules/commander/ModeUtil/mode_requirements.cpp
@ -170,8 +170,9 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt);

+	// NAVIGATION_STATE_EXTERNALx: handled outside

-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements");
 }

 } // namespace mode_util
--- a/src/modules/commander/UserModeIntention.cpp
+++ b/src/modules/commander/UserModeIntention.cpp
@ -35,14 +35,22 @@
 #include "UserModeIntention.hpp"

 UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
-				     const HealthAndArmingChecks &health_and_arming_checks)
-	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
+				     const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler)
+	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks),
+	  _handler(handler)
 {
 }

-bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
+bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback,
+			       bool force)
 {
 	_ever_had_mode_change = true;
+	_had_mode_change = true;
+
+	if (_handler) {
+		// If a replacement mode is selected, select the internal one instead. The replacement will be selected after.
+		user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state);
+	}

 	// Always allow mode change while disarmed
 	bool always_allow = force || !isArmed();
@ -68,6 +76,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
 			_nav_state_after_disarming = user_intended_nav_state;
 		}
+
+		if (_handler) {
+			_handler->onUserIntendedNavStateChange(source, user_intended_nav_state);
+		}
 	}

 	return allow_change;
--- a/src/modules/commander/UserModeIntention.hpp
+++ b/src/modules/commander/UserModeIntention.hpp
@ -37,21 +37,42 @@
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include <px4_platform_common/module_params.h>

+enum class ModeChangeSource {
+	User,           ///< RC or MAVLink
+	ModeExecutor,
+};
+
+class ModeChangeHandler
+{
+public:
+	virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0;
+
+	/**
+	 * Get the replaced (internal) mode for a given (external) mode
+	 * @param nav_state
+	 * @return nav_state or the mode that nav_state replaces
+	 */
+	virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
+};
+
+
 class UserModeIntention : ModuleParams
 {
 public:
 	UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
-			  const HealthAndArmingChecks &health_and_arming_checks);
+			  const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler);
 	~UserModeIntention() = default;

 	/**
 	 * Change the user intended mode
 	 * @param user_intended_nav_state new mode
+	 * @param source calling reason
 	 * @param allow_fallback allow to fallback to a lower mode if current mode cannot run
 	 * @param force always set if true
 	 * @return true if successfully set (also if unchanged)
 	 */
-	bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
+	bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User,
+		    bool allow_fallback = false, bool force = false);

 	uint8_t get() const { return _user_intented_nav_state; }

@ -72,6 +93,7 @@ private:

 	const vehicle_status_s &_vehicle_status;
 	const HealthAndArmingChecks &_health_and_arming_checks;
+	ModeChangeHandler *const _handler{nullptr};

 	uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
 	uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@ -63,7 +63,15 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
-	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
+	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL1,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL2,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL3,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL4,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL5,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL6,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL7,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL8,
 };

 enum PX4_CUSTOM_SUB_MODE_POSCTL {
@ -171,6 +179,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
 		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL2;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL3;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL4;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL5;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL6;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL7;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
+		break;
 	}

 	return custom_mode;
--- a/src/modules/mavlink/mavlink_command_sender.cpp
+++ b/src/modules/mavlink/mavlink_command_sender.cpp
@ -68,7 +68,8 @@ MavlinkCommandSender::~MavlinkCommandSender()
 int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
 {
 	// commands > uint16 are PX4 internal only
-	if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
+	if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
+	    || command.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
 		return 0;
 	}

--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@ -2354,7 +2354,8 @@ Mavlink::task_main(int argc, char *argv[])

 					if (!command_ack.from_external
 					    && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
-					    && is_target_known) {
+					    && is_target_known
+					    && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {

 						mavlink_command_ack_t msg{};
 						msg.result = command_ack.result;
--- a/src/modules/uxrce_dds_client/dds_topics.yaml
+++ b/src/modules/uxrce_dds_client/dds_topics.yaml
@ -5,12 +5,24 @@
 #####
 publications:

+  - topic: /fmu/out/register_ext_component_reply
+    type: px4_msgs::msg::RegisterExtComponentReply
+
+  - topic: /fmu/out/arming_check_request
+    type: px4_msgs::msg::ArmingCheckRequest
+
+  - topic: /fmu/out/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
  - topic: /fmu/out/collision_constraints
    type: px4_msgs::msg::CollisionConstraints

  - topic: /fmu/out/failsafe_flags
    type: px4_msgs::msg::FailsafeFlags

+  - topic: /fmu/out/manual_control_setpoint
+    type: px4_msgs::msg::ManualControlSetpoint
+
  - topic: /fmu/out/position_setpoint_triplet
    type: px4_msgs::msg::PositionSetpointTriplet

@ -29,6 +41,9 @@ publications:
  - topic: /fmu/out/vehicle_control_mode
    type: px4_msgs::msg::VehicleControlMode

+  - topic: /fmu/out/vehicle_command_ack
+    type: px4_msgs::msg::VehicleCommandAck
+
  - topic: /fmu/out/vehicle_global_position
    type: px4_msgs::msg::VehicleGlobalPosition

@ -48,6 +63,20 @@ publications:
    type: px4_msgs::msg::VehicleTrajectoryWaypoint

 subscriptions:
+  - topic: /fmu/in/register_ext_component_request
+    type: px4_msgs::msg::RegisterExtComponentRequest
+
+  - topic: /fmu/in/unregister_ext_component
+    type: px4_msgs::msg::UnregisterExtComponent
+
+  - topic: /fmu/in/arming_check_reply
+    type: px4_msgs::msg::ArmingCheckReply
+
+  - topic: /fmu/in/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
+  - topic: /fmu/in/config_control_setpoints
+    type: px4_msgs::msg::VehicleControlMode

  - topic: /fmu/in/offboard_control_mode
    type: px4_msgs::msg::OffboardControlMode
@ -82,6 +111,9 @@ subscriptions:
  - topic: /fmu/in/vehicle_command
    type: px4_msgs::msg::VehicleCommand

+  - topic: /fmu/in/vehicle_command_mode_executor
+    type: px4_msgs::msg::VehicleCommand
+
  - topic: /fmu/in/vehicle_trajectory_bezier
    type: px4_msgs::msg::VehicleTrajectoryBezier