updated warning (#22367)

Signed-off-by: frederik <frederik@auterion.com>
Co-authored-by: frederik <frederik@auterion.com>

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -38,6 +38,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 
 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
 
+
 	# set local coordinate frame reference
 	if [ -n "${PX4_HOME_LAT}" ]; then
 		param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
@@ -59,36 +60,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 		. ../gz_env.sh
 	fi
 
-	# "gz sim" only avaiilable in Garden and later
+	# Only start up Gazebo if STANDALONE set to false
-	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+	if [ "$STANDALONE" != '1' ]; then
-	if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-	then
-		# "gz sim" from Garden on
-		gz_command="gz"
-		gz_sub_command="sim"
-	else
-		echo "ERROR [init] Gazebo gz please install gz-garden"
-		exit 1
-	fi
 
-	# look for running ${gz_command} gazebo world
+		# "gz sim" only avaiilable in Garden and later
-	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-
+		if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-	# shellcheck disable=SC2153
+		then
-	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+			# "gz sim" from Garden on
-
+			gz_command="gz"
-		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+			gz_sub_command="sim"
-
+		else
-		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+			echo "ERROR [init] Gazebo gz please install gz-garden"
-
+			exit 1
-		if [ -z "${HEADLESS}" ]; then
-			# HEADLESS not set, starting gui
-			${gz_command} ${gz_sub_command} -g &
 		fi
 
-	else
+		# look for running ${gz_command} gazebo world
-		echo "INFO  [init] gazebo already running world: ${gz_world}"
+		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-		PX4_GZ_WORLD=${gz_world}
+
+		# shellcheck disable=SC2153
+		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+
+			echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+
+			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+
+			if [ -z "${HEADLESS}" ]; then
+				# HEADLESS not set, starting gui
+				${gz_command} ${gz_sub_command} -g &
+			fi
+
+		else
+			echo "INFO  [init] gazebo already running world: ${gz_world}"
+			PX4_GZ_WORLD=${gz_world}
+		fi
 	fi
 
 	# start gz_bridge

Tools/msg/px_generate_uorb_topic_helper.py

@@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
     return spec_temp.parsed_fields()
 
 
+def get_message_fields_str_for_message_hash(msg_fields, search_path):
+    """
+    Get all fields (including for nested types) in the form of:
+'''
+uint64 timestamp
+uint8 esc_count
+uint8 esc_online_flags
+EscReport[8] esc
+uint64 timestamp
+uint32 esc_errorcount
+int32 esc_rpm
+float32 esc_voltage
+uint16 failures
+int8 esc_power
+'''
+    """
+    all_fields_str = ''
+    for field in msg_fields:
+        if field.is_header:
+            continue
+
+        type_name = field.type
+        # detect embedded types
+        sl_pos = type_name.find('/')
+        if sl_pos >= 0:
+            type_name = type_name[sl_pos + 1:]
+
+        all_fields_str += type_name + ' ' + field.name + '\n'
+
+        if sl_pos >= 0: # nested type, add all nested fields
+            children_fields = get_children_fields(field.base_type, search_path)
+            all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
+
+    return all_fields_str
+
+
+def hash_32_fnv1a(data: str):
+    hash_val = 0x811c9dc5
+    prime = 0x1000193
+    for i in range(len(data)):
+        value = ord(data[i])
+        hash_val = hash_val ^ value
+        hash_val *= prime
+        hash_val &= 0xffffffff
+    return hash_val
+
+
+def get_message_hash(msg_fields, search_path):
+    """
+    Get a 32 bit message hash over all fields
+    """
+    all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
+    return hash_32_fnv1a(all_fields_str)
+
+
 def add_padding_bytes(fields, search_path):
     """
     Add padding fields before the embedded types, at the end and calculate the

Tools/msg/templates/uorb/msg.cpp.em

@@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
 
 uorb_struct = '%s_s'%name_snake_case
 
+message_hash = get_message_hash(spec.parsed_fields(), search_path)
 sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
 struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 }@
@@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
 
 @[for topic in topics]@
 static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
-ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
 @[end for]
 
 void print_message(const orb_metadata *meta, const @uorb_struct& message)

Tools/px4events/srcparser.py

@@ -250,6 +250,22 @@ class SourceParser(object):
                             event.group = "arming_check"
                         event.prepend_arguments([('navigation_mode_group_t', 'modes'),
                                 ('uint8_t', 'health_component_index')])
+                    elif call in ['reporter.healthFailureExt', 'reporter.armingCheckFailureExt']: # from ROS2
+                        assert len(args_split) == num_args + 3, \
+                            "Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
+                        m = self.re_event_id.search(args_split[0])
+                        if m:
+                            _, event_name = m.group(1, 2)
+                        else:
+                            raise Exception("Could not extract event ID from {:}".format(args_split[0]))
+                        event.name = event_name
+                        event.message = args_split[2][1:-1]
+                        if 'health' in call:
+                            event.group = "health"
+                        else:
+                            event.group = "arming_check"
+                        event.prepend_arguments([('navigation_mode_group_t', 'modes'),
+                                ('uint8_t', 'health_component_index')])
                     else:
                         raise Exception("unknown event method call: {}, args: {}".format(call, args))
 

Tools/simulation/gz/models/rc_cessna/model.sdf

@@ -69,7 +69,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/body.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -184,7 +184,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -229,7 +229,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/left_aileron.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -385,7 +385,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/right_aileron.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -412,7 +412,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/left_flap.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -439,7 +439,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/right_flap.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -466,7 +466,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/elevators.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -493,7 +493,7 @@
         <geometry>
           <mesh>
             <scale>0.1 0.1 0.1</scale>
-            <uri>model://rc_cessna/meshes/rudder.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae</uri>
           </mesh>
         </geometry>
         <material>

Tools/simulation/gz/models/standard_vtol/model.sdf

@@ -43,7 +43,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -183,7 +183,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -246,7 +246,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -309,7 +309,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -372,7 +372,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -436,7 +436,7 @@
         <geometry>
           <mesh>
             <scale>0.8 0.8 0.8</scale>
-            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -483,7 +483,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -510,7 +510,7 @@
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
-            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae</uri>
           </mesh>
         </geometry>
         <material>

Tools/simulation/gz/models/x500/model.sdf

@@ -23,7 +23,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/NXP-HGD-CF.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -32,7 +32,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -41,7 +41,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -50,7 +50,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -59,7 +59,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Base.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -77,7 +77,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -96,7 +96,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -115,7 +115,7 @@
           <specular>1.0 1.0 1.0</specular>
           <pbr>
             <metal>
-              <albedo_map>model://x500/materials/textures/rd.png</albedo_map>
+              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png</albedo_map>
             </metal>
           </pbr>
         </material>
@@ -250,7 +250,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -265,7 +265,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -322,7 +322,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -337,7 +337,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -394,7 +394,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -409,7 +409,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>
@@ -466,7 +466,7 @@
         <geometry>
           <mesh>
             <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
           </mesh>
         </geometry>
         <material>
@@ -481,7 +481,7 @@
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
-            <uri>model://x500/meshes/5010Bell.dae</uri>
+            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
           </mesh>
         </geometry>
       </visual>

boards/px4/fmu-v5x/default.px4board

@@ -111,3 +111,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_MAVLINK_DIALECT="development"

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
+

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
+
+

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
@@ -58,6 +60,7 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
+	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DatamanRequest.msg
@@ -130,6 +133,8 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
+	MessageFormatRequest.msg
+	MessageFormatResponse.msg
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
@@ -161,6 +166,8 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
+	RegisterExtComponentReply.msg
+	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -198,6 +205,7 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UnregisterExtComponent.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg

msg/ConfigOverrides.msg

@@ -0,0 +1,19 @@
+# Configurable overrides by (external) modes or mode executors
+uint64 timestamp		# time since system start (microseconds)
+
+bool disable_auto_disarm         # Prevent the drone from automatically disarming after landing (if configured)
+
+bool defer_failsafes             # Defer all failsafes that can be deferred (until the flag is cleared)
+int16 defer_failsafes_timeout_s  # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout
+
+
+int8 SOURCE_TYPE_MODE = 0
+int8 SOURCE_TYPE_MODE_EXECUTOR = 1
+int8 source_type
+
+uint8 source_id                  # ID depending on source_type
+
+uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS config_overrides config_overrides_request
+

msg/MessageFormatRequest.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Request to PX4 to get the hash of a message, to check for message compatibility
+
+uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+

msg/MessageFormatResponse.msg

@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+# Response from PX4 with the format of a message
+
+uint16 protocol_version           # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
+
+char[50] topic_name  # E.g. /fmu/in/vehicle_command
+
+bool success
+uint32 message_hash # hash over all message fields
+

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
+

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

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

msg/VehicleCommand.msg

@@ -71,6 +71,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 ==
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
+uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262		# Enable the specified standard MAVLink mode |MAV_STANDARD_MODE|
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283	# Command to ask information about a low level gimbal
 
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
@@ -103,6 +104,7 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)
 uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000       # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
+uint32 VEHICLE_CMD_SET_NAV_STATE = 100001               # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty|
 
 uint8 VEHICLE_MOUNT_MODE_RETRACT = 0			# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
 uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1			# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
@@ -174,8 +176,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

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

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

msg/VehicleStatus.msg

@@ -52,7 +52,20 @@ 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)
+
+uint32 valid_nav_states_mask                    # Bitmask for all valid nav_state values
+uint32 can_set_nav_states_mask                  # Bitmask for all modes that a user can select
 
 # Bitmask of detected failures
 uint16 failure_detector_status
@@ -78,8 +91,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
+uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
+uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
+uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
+
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
 
 # Link loss
 bool gcs_connection_lost              # datalink to GCS lost

platforms/common/uORB/uORB.h

@@ -51,6 +51,7 @@ struct orb_metadata {
 	const char    *o_name;              /**< unique object name */
 	const uint16_t o_size;              /**< object size */
 	const uint16_t o_size_no_padding;   /**< object size w/o padding at the end (for logger) */
+	uint32_t message_hash;	/**< Hash over all fields for message compatibility checks */
 	orb_id_size_t  o_id;                /**< ORB_ID enum */
 };
 
@@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
  * @param _name		The name of the topic.
  * @param _struct	The structure the topic provides.
  * @param _size_no_padding	Struct size w/o padding at the end
+ * @param _message_hash	32 bit message hash over all fields
  * @param _orb_id_enum	ORB ID enum e.g.: ORB_ID::vehicle_status
  */
-#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum)		\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum)		\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
 		_size_no_padding,			\
+		_message_hash,				\
 		_orb_id_enum				\
 	}; struct hack
 

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" ]

src/lib/mixer_module/functions/FunctionActuatorSet.hpp

@@ -38,7 +38,7 @@
 #include <uORB/topics/vehicle_command.h>
 
 /**
- * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
+ * Functions: Peripheral_via_Actuator_Set1 ... Peripheral_via_Actuator_Set6
  */
 class FunctionActuatorSet : public FunctionProviderBase
 {
@@ -72,7 +72,7 @@ public:
 		}
 	}
 
-	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
+	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Peripheral_via_Actuator_Set1]; }
 
 private:
 	static constexpr int max_num_actuators = 6;

src/lib/mixer_module/mixer_module.cpp

@@ -57,7 +57,7 @@ static const FunctionProvider all_function_providers[] = {
 	{OutputFunction::Constant_Max, &FunctionConstantMax::allocate},
 	{OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate},
 	{OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate},
-	{OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate},
+	{OutputFunction::Peripheral_via_Actuator_Set1, OutputFunction::Peripheral_via_Actuator_Set6, &FunctionActuatorSet::allocate},
 	{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
 	{OutputFunction::Landing_Gear_Wheel, &FunctionLandingGearWheel::allocate},
 	{OutputFunction::Parachute, &FunctionParachute::allocate},

src/lib/mixer_module/output_functions.yaml

@@ -13,7 +13,7 @@ functions:
     Servo:
       start: 201
       count: 8
-    Offboard_Actuator_Set:
+    Peripheral_via_Actuator_Set:
       start: 301
       count: 6
 

src/lib/modes/standard_modes.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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/topics/vehicle_status.h>
+
+#include <stdint.h>
+
+namespace mode_util
+{
+
+// This matches the definition from MAVLink MAV_STANDARD_MODE
+enum class StandardMode : uint8_t {
+	NON_STANDARD = 0,
+	POSITION_HOLD = 1,
+	ORBIT = 2,
+	CRUISE = 3,
+	ALTITUDE_HOLD = 4,
+	RETURN_HOME = 5,
+	SAFE_RECOVERY = 6,
+	MISSION = 7,
+	LAND = 8,
+	TAKEOFF = 9,
+};
+
+/**
+ * @return Get MAVLink standard mode from nav_state
+ */
+static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
+{
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF;
+		// Note: all other standard modes do not directly map, or are vehicle-type specific
+	}
+
+	return StandardMode::NON_STANDARD;
+}
+
+/**
+ * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported
+ */
+static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
+{
+	switch (mode) {
+	case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+	case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+
+	case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+	case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
+
+	default: break;
+	}
+
+	return vehicle_status_s::NAVIGATION_STATE_MAX;
+}
+
+
+} // namespace mode_util

src/lib/modes/ui.hpp

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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/topics/vehicle_status.h>
+
+#include <stdint.h>
+
+namespace mode_util
+{
+
+/**
+ * @return Bitmask with all valid modes
+ */
+static inline uint32_t getValidNavStates()
+{
+	return (1u << vehicle_status_s::NAVIGATION_STATE_MANUAL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_STAB) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
+
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
+}
+
+const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
+	"Manual",
+	"Altitude",
+	"Position",
+	"Mission",
+	"Hold",
+	"Return",
+	"6: unallocated",
+	"7: unallocated",
+	"8: unallocated",
+	"9: unallocated",
+	"Acro",
+	"11: UNUSED",
+	"Descend",
+	"Termination",
+	"Offboard",
+	"Stabilized",
+	"16: UNUSED2",
+	"Takeoff",
+	"Land",
+	"Follow Target",
+	"Precision Landing",
+	"Orbit",
+	"VTOL Takeoff",
+	"External 1",
+	"External 2",
+	"External 3",
+	"External 4",
+	"External 5",
+	"External 6",
+	"External 7",
+	"External 8",
+};
+
+/**
+ * @return returns true for advanced modes
+ */
+static inline bool isAdvanced(uint8_t nav_state)
+{
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return false;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return false;
+
+	}
+
+	return true;
+}
+
+} // namespace mode_util

src/modules/commander/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-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
@@ -31,11 +31,12 @@
 #
 ############################################################################
 
+add_subdirectory(Arming)
+add_subdirectory(failsafe)
 add_subdirectory(failure_detector)
 add_subdirectory(HealthAndArmingChecks)
-add_subdirectory(failsafe)
-add_subdirectory(Arming)
 add_subdirectory(ModeUtil)
+add_subdirectory(MulticopterThrowLaunch)
 
 px4_add_module(
 	MODULE modules__commander
@@ -52,24 +53,30 @@ px4_add_module(
 		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		HomePosition.cpp
+		ModeManagement.cpp
 		UserModeIntention.cpp
 		level_calibration.cpp
 		lm_fit.cpp
 		mag_calibration.cpp
 		rc_calibration.cpp
 		Safety.cpp
+		UserModeIntention.cpp
 		worker_thread.cpp
+	MODULE_CONFIG
+		module.yaml
 	DEPENDS
+		ArmAuthorization
 		circuit_breaker
+		failsafe
 		failure_detector
 		geo
 		health_and_arming_checks
 		hysteresis
-		ArmAuthorization
+		mode_util
+		MulticopterThrowLaunch
 		sensor_calibration
 		world_magnetic_model
-		mode_util
-		failsafe
 	)
 
 px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
+px4_add_functional_gtest(SRC ModeManagementTest.cpp LINKLIBS modules__commander)

src/modules/commander/Commander.cpp

@@ -48,6 +48,8 @@
 #include "px4_custom_mode.h"
 #include "ModeUtil/control_mode.hpp"
 #include "ModeUtil/conversions.hpp"
+#include <lib/modes/ui.hpp>
+#include <lib/modes/standard_modes.hpp>
 
 /* PX4 headers */
 #include <drivers/drv_hrt.h>
@@ -407,6 +409,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 +481,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 +734,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 +814,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 +859,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 {
@@ -869,6 +880,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS
+			uint8_t desired_nav_state = (uint8_t)(cmd.param1 + 0.5f);
+
+			if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 
 			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
@@ -969,7 +992,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 +1006,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 +1020,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 +1034,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 +1048,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 +1072,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 +1111,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 +1120,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 {
@@ -1382,6 +1405,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			break;
 		}
 
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
+			mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
+			uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode);
+
+			if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+
+			} else {
+				if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				}
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
 		_health_and_arming_checks.update(true);
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -1445,6 +1486,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 +1647,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 +1795,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 +1819,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();
@@ -1799,6 +1881,7 @@ void Commander::run()
 			updateControlMode();
 
 			// vehicle_status publish (after prearm/preflight updates above)
+			_mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask);
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);
 
@@ -1880,7 +1963,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) {
@@ -2131,8 +2215,10 @@ void Commander::handleAutoDisarm()
 
 			const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 							 && !_mission_result_sub.get().finished;
+			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
+							      && !_config_overrides.disable_auto_disarm;
 
-			if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
+			if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
 				_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
 				_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
 
@@ -2181,6 +2267,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 {
 	const uint8_t prev_nav_state = _vehicle_status.nav_state;
 	const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
+	const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
 
 	FailsafeBase::State state{};
 	state.armed = isArmed();
@@ -2200,13 +2287,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_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
-	_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _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:
@@ -2228,7 +2318,24 @@ bool Commander::handleModeIntentionAndFailsafe()
 		_vehicle_status.nav_state_timestamp = hrt_absolute_time();
 	}
 
-	return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
+	_mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides);
+
+	// Apply failsafe deferring & get the current state
+	_failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s);
+
+	if (_failsafe.failsafeDeferred()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE;
+
+	} else if (_failsafe.getDeferFailsafes()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED;
+
+	} else {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED;
+	}
+
+	return prev_nav_state != _vehicle_status.nav_state ||
+	       prev_failsafe_action != _failsafe.selectedAction() ||
+	       prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state;
 }
 
 void Commander::checkAndInformReadyForTakeoff()
@@ -2248,6 +2355,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 +2515,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 +2877,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 +2894,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 +2956,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");

src/modules/commander/Commander.hpp

@@ -34,13 +34,13 @@
 #pragma once
 
 /*   Helper classes  */
-#include "failure_detector/FailureDetector.hpp"
 #include "failsafe/failsafe.h"
-#include "Safety.hpp"
+#include "failure_detector/FailureDetector.hpp"
-#include "worker_thread.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
+#include "ModeManagement.hpp"
 #include "UserModeIntention.hpp"
+#include "worker_thread.hpp"
 
 #include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
@@ -80,7 +80,6 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
 using math::constrain;
@@ -124,6 +123,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);
 
@@ -174,6 +174,10 @@ private:
 
 	void safetyButtonUpdate();
 
+	bool isThrowLaunchInProgress() const;
+
+	void throwLaunchUpdate();
+
 	void vtolStatusUpdate();
 
 	void updateTunes();
@@ -190,6 +194,10 @@ private:
 
 	void checkAndInformReadyForTakeoff();
 
+	void handleCommandsFromModeExecutors();
+
+	void modeManagementUpdate();
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -210,12 +218,19 @@ private:
 	FailsafeBase		&_failsafe{_failsafe_instance};
 	FailureDetector		_failure_detector{this};
 	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
+	MulticopterThrowLaunch  _multicopter_throw_launch{this};
 	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};
+	config_overrides_s   _config_overrides{};
 
 
 	Hysteresis _auto_disarm_landed{false};
@@ -276,6 +291,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 +311,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)};
 

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)
 

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

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
+	 * Clear can_run bits for certain modes. This will prevent mode switching.
-	 * mode is being run.
+	 * 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);

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,

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");
+	}
+}
+

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

src/modules/commander/ModeManagement.cpp

@@ -0,0 +1,639 @@
+/****************************************************************************
+ *
+ *   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)
+{
+	int32_t mode_name_hash = (int32_t)events::util::hash_32_fnv1a_const(mode.name);
+
+	if (mode_name_hash == 0) { // 0 is reserved for unused indexes
+		mode_name_hash = 1;
+	}
+
+	// Try to find the index with matching hash (if mode was already registered before),
+	// so that the same mode always gets the same index (required for RC switch mode assignment)
+	int first_unused_idx = -1;
+	int first_invalid_idx = -1;
+	int matching_idx = -1;
+
+	for (int i = 0; i < MAX_NUM; ++i) {
+		char hash_param_name[20];
+		snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", i);
+		const param_t handle = param_find(hash_param_name);
+		int32_t current_hash{};
+
+		if (handle != PARAM_INVALID && param_get(handle, &current_hash) == 0) {
+			if (!_modes[i].valid && current_hash == 0 && first_unused_idx == -1) {
+				first_unused_idx = i;
+			}
+
+			if (current_hash == mode_name_hash) {
+				matching_idx = i;
+			}
+
+			if (!_modes[i].valid && first_invalid_idx == -1) {
+				first_invalid_idx = i;
+			}
+		}
+	}
+
+	bool need_to_update_param = false;
+	int new_mode_idx = -1;
+
+	if (matching_idx != -1) {
+		// If we found a match, try to use it but check for hash collisions or duplicate mode name
+		if (_modes[matching_idx].valid) {
+			// This can happen when restarting modes while armed
+			PX4_WARN("Mode '%s' already registered (as '%s')", mode.name, _modes[matching_idx].name);
+
+			if (first_unused_idx != -1) {
+				new_mode_idx = first_unused_idx;
+				// Do not update the hash
+
+			} else {
+				// Need to overwrite a hash. Reset it as we can't store duplicate hashes anyway
+				new_mode_idx = first_invalid_idx;
+				need_to_update_param = true;
+				mode_name_hash = 0;
+			}
+
+		} else {
+			new_mode_idx = matching_idx;
+		}
+
+	} else if (first_unused_idx != -1) {
+		// Mode registers the first time and there's still unused indexes
+		need_to_update_param = true;
+		new_mode_idx = first_unused_idx;
+
+	} else {
+		// Mode registers the first time but all indexes are used so we need to overwrite one
+		need_to_update_param = true;
+		new_mode_idx = first_invalid_idx;
+	}
+
+	if (new_mode_idx != -1 && !_modes[new_mode_idx].valid) {
+		if (need_to_update_param) {
+			char hash_param_name[20];
+			snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", new_mode_idx);
+			const param_t handle = param_find(hash_param_name);
+
+			if (handle != PARAM_INVALID) {
+				param_set_no_notification(handle, &mode_name_hash);
+			}
+		}
+
+		_modes[new_mode_idx] = mode;
+		_modes[new_mode_idx].valid = true;
+		return new_mode_idx + 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();
+	checkConfigOverrides();
+}
+
+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());
+}
+
+void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out)
+{
+	config_overrides_s current_overrides;
+
+	if (_modes.valid(nav_state)) {
+		current_overrides = _modes.mode(nav_state).overrides;
+
+	} else {
+		current_overrides = {};
+	}
+
+	// Apply the overrides from executors on top (executors take precedence)
+	const int executor_in_charge = modeExecutorInCharge();
+
+	if (_mode_executors.valid(executor_in_charge)) {
+		const config_overrides_s &executor_overrides = _mode_executors.executor(executor_in_charge).overrides;
+
+		if (executor_overrides.disable_auto_disarm) {
+			current_overrides.disable_auto_disarm = true;
+		}
+
+		if (executor_overrides.defer_failsafes) {
+			current_overrides.defer_failsafes = true;
+			current_overrides.defer_failsafes_timeout_s = executor_overrides.defer_failsafes_timeout_s;
+		}
+	}
+
+	// Publish if changed or at low rate
+	current_overrides.timestamp = overrides_in_out.timestamp;
+
+	if (memcmp(&overrides_in_out, &current_overrides, sizeof(current_overrides)) != 0
+	    || hrt_elapsed_time(&current_overrides.timestamp) > 500_ms) {
+		current_overrides.timestamp = hrt_absolute_time();
+		_config_overrides_pub.publish(current_overrides);
+		overrides_in_out = current_overrides;
+	}
+}
+
+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;
+}
+
+void ModeManagement::checkConfigOverrides()
+{
+	config_overrides_s override_request;
+	int max_updates = config_overrides_s::ORB_QUEUE_LENGTH;
+
+	while (_config_overrides_request_sub.update(&override_request) && --max_updates >= 0) {
+		switch (override_request.source_type) {
+		case config_overrides_s::SOURCE_TYPE_MODE_EXECUTOR:
+			if (_mode_executors.valid(override_request.source_id)) {
+				ModeExecutors::ModeExecutor &executor = _mode_executors.executor(override_request.source_id);
+				memcpy(&executor.overrides, &override_request, sizeof(executor.overrides));
+				static_assert(sizeof(executor.overrides) == sizeof(override_request), "size mismatch");
+			}
+
+			break;
+
+		case config_overrides_s::SOURCE_TYPE_MODE:
+			if (_modes.valid(override_request.source_id)) {
+				Modes::Mode &mode = _modes.mode(override_request.source_id);
+				memcpy(&mode.overrides, &override_request, sizeof(mode.overrides));
+			}
+
+			break;
+		}
+	}
+}
+
+void ModeManagement::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);
+
+	// Add external modes
+	for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+		if (_modes.valid(i)) {
+			valid_nav_state_mask |= 1u << i;
+			can_set_nav_state_mask |= 1u << i;
+			const Modes::Mode &cur_mode = _modes.mode(i);
+
+			if (cur_mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
+				// Hide the internal mode if it's replaced
+				can_set_nav_state_mask &= ~(1u << cur_mode.replaces_nav_state);
+			}
+
+		} else {
+			// Still set the mode as valid but not as selectable. This is because an external mode could still
+			// be selected via RC when not yet running, so we make sure to display some mode label indicating it's not
+			// available.
+			valid_nav_state_mask |= 1u << i;
+		}
+	}
+}
+
+#endif /* CONSTRAINED_FLASH */

src/modules/commander/ModeManagement.hpp

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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 <uORB/topics/config_overrides.h>
+
+#include <lib/modes/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 {
+		config_overrides_s overrides{};
+		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};
+		config_overrides_s overrides{};
+		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;
+
+	void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const;
+
+	void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out);
+
+private:
+	bool checkConfigControlSetpointUpdates();
+	void checkNewRegistrations(UpdateRequest &update_request);
+	void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request);
+	void checkConfigOverrides();
+
+	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)};
+	uORB::Publication<config_overrides_s> _config_overrides_pub{ORB_ID(config_overrides)};
+	uORB::Subscription _config_overrides_request_sub{ORB_ID(config_overrides_request)};
+
+	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 */

src/modules/commander/ModeManagementTest.cpp

@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "ModeManagement.hpp"
+
+static bool modeValid(uint8_t mode)
+{
+	return mode >= Modes::FIRST_EXTERNAL_NAV_STATE && mode <= Modes::LAST_EXTERNAL_NAV_STATE;
+}
+
+static int32_t readHash(int idx)
+{
+	char buffer[20];
+	snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", idx);
+	param_t param = param_find(buffer);
+	int32_t value{};
+	param_get(param, &value);
+	return value;
+}
+
+TEST(ModeManagementTest, Hashes)
+{
+	param_control_autosave(false);
+
+	// Reset parameters
+	for (int i = 0; i < Modes::MAX_NUM; ++i) {
+		char buffer[20];
+		snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", i);
+		param_t param = param_find(buffer);
+		param_reset(param);
+	}
+
+	// Add full set of modes, which stores the hashes
+	Modes modes;
+	Modes::Mode mode;
+
+	for (int i = 0; i < Modes::MAX_NUM; ++i) {
+		snprintf(mode.name, sizeof(mode.name), "mode %i", i);
+		EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + i);
+		EXPECT_EQ(readHash(i), events::util::hash_32_fnv1a_const(mode.name));
+	}
+
+	EXPECT_FALSE(modes.hasFreeExternalModes());
+
+	// Remove all modes, except last
+	for (int i = 0; i < Modes::MAX_NUM - 1; ++i) {
+		snprintf(mode.name, sizeof(mode.name), "mode %i", i);
+		EXPECT_TRUE(modes.removeExternalMode(Modes::FIRST_EXTERNAL_NAV_STATE + i, mode.name));
+	}
+
+	// Add some mode, ensure it gets the same index
+	const int mode_to_add_idx = 3;
+	snprintf(mode.name, sizeof(mode.name), "mode %i", mode_to_add_idx);
+	EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + mode_to_add_idx);
+
+	// Try to add another one with the same name: should succeed, with the hash of the added index reset
+	uint8_t added_mode_nav_state = modes.addExternalMode(mode);
+	EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE), 0);
+
+	// 3 Modes are used now. Add N-3 new ones which must overwrite previous hashes
+	for (int i = 0; i < Modes::MAX_NUM - 3; ++i) {
+		snprintf(mode.name, sizeof(mode.name), "new mode %i", i);
+		added_mode_nav_state = modes.addExternalMode(mode);
+		EXPECT_TRUE(modeValid(added_mode_nav_state));
+		EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE),
+			  events::util::hash_32_fnv1a_const(mode.name));
+	}
+
+	EXPECT_FALSE(modes.hasFreeExternalModes());
+}

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

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

src/modules/commander/ModeUtil/conversions.hpp

@@ -75,36 +75,27 @@ 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;
 }
 
-const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
-	"MANUAL",
-	"ALTCTL",
-	"POSCTL",
-	"AUTO_MISSION",
-	"AUTO_LOITER",
-	"AUTO_RTL",
-	"6: unallocated",
-	"7: unallocated",
-	"AUTO_LANDENGFAIL",
-	"9: unallocated",
-	"ACRO",
-	"11: UNUSED",
-	"DESCEND",
-	"TERMINATION",
-	"OFFBOARD",
-	"STAB",
-	"16: UNUSED2",
-	"AUTO_TAKEOFF",
-	"AUTO_LAND",
-	"AUTO_FOLLOW_TARGET",
-	"AUTO_PRECLAND",
-	"ORBIT"
-};
-
 } // namespace mode_util

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

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)
+				     const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler)
-	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
+	: 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;

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

src/modules/commander/commander_params.c

@@ -345,150 +345,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
  */
 PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
 
-/**
- * First flightmode slot (1000-1160)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
-
-/**
- * Second flightmode slot (1160-1320)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
-
-/**
- * Third flightmode slot (1320-1480)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
-
-/**
- * Fourth flightmode slot (1480-1640)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
-
-/**
- * Fifth flightmode slot (1640-1800)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
-
-/**
- * Sixth flightmode slot (1800-2000)
- *
- * If the main switch channel is in this range the
- * selected flight mode will be applied.
- *
- * @value -1 Unassigned
- * @value 0 Manual
- * @value 1 Altitude
- * @value 2 Position
- * @value 3 Mission
- * @value 4 Hold
- * @value 10 Takeoff
- * @value 11 Land
- * @value 5 Return
- * @value 6 Acro
- * @value 7 Offboard
- * @value 8 Stabilized
- * @value 12 Follow Me
- * @value 13 Precision Land
- * @group Commander
- */
-PARAM_DEFINE_INT32(COM_FLTMODE6, -1);
-
 /**
  * Maximum EKF position innovation test ratio that will allow arming
  *

src/modules/commander/module.yaml

@@ -0,0 +1,50 @@
+module_name: Commander
+parameters:
+    - group: Commander
+      definitions:
+        COM_MODE${i}_HASH:
+            description:
+                short: External mode identifier ${i}
+                long: |
+                    This parameter is automatically set to identify external modes. It ensures that modes
+                    get assigned to the same index independent from their startup order,
+                    which is required when mapping an external mode to an RC switch.
+            type: int32
+            num_instances: 8 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8)
+            default: 0
+            volatile: true
+            category: System
+
+        COM_FLTMODE${i}:
+            description:
+                short: Mode slot ${i}
+                long: |
+                    If the main switch channel is in this range the
+                    selected flight mode will be applied.
+            type: enum
+            values:
+                -1: Unassigned
+                0: Manual
+                1: Altitude
+                2: Position
+                3: Mission
+                4: Hold
+                10: Takeoff
+                11: Land
+                5: Return
+                6: Acro
+                7: Offboard
+                8: Stabilized
+                12: Follow Me
+                13: Precision Land
+                100: External Mode 1
+                101: External Mode 2
+                102: External Mode 3
+                103: External Mode 4
+                104: External Mode 5
+                105: External Mode 6
+                106: External Mode 7
+                107: External Mode 8
+            instance_start: 1
+            num_instances: 6
+            default: -1

src/modules/commander/px4_custom_mode.h

@@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
-	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_TERMINATION
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -63,7 +64,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 {
@@ -131,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_TERMINATION;
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
@@ -171,6 +180,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;

src/modules/flight_mode_manager/FlightModeManager.cpp

@@ -137,7 +137,9 @@ void FlightModeManager::updateParams()
 void FlightModeManager::start_flight_task()
 {
 	// Do not run any flight task for VTOLs in fixed-wing mode
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+	if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
+	    || ((_vehicle_status_sub.get().nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
+		&& (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))) {
 		switchTask(FlightTaskIndex::None);
 		return;
 	}

src/modules/logger/logged_topics.cpp

@@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
+	add_topic("config_overrides");
 	add_topic("cpuload");
 	add_optional_topic("external_ins_attitude");
 	add_optional_topic("external_ins_global_position");

src/modules/manual_control/ManualControl.cpp

@@ -555,6 +555,15 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
 		case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 		case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
 		case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
+
+		case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
+		case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2;
+		case 102: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL3;
+		case 103: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL4;
+		case 104: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL5;
+		case 105: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL6;
+		case 106: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL7;
+		case 107: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
 	}
 	return -1;
 }

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

src/modules/mavlink/mavlink_main.cpp

@@ -1186,8 +1186,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		return OK;
 	}
 
-	/* if we reach here, the stream list does not contain the stream */
+	// if we reach here, the stream list does not contain the stream.
-#if defined(CONSTRAINED_FLASH) // flash constrained target's don't include all streams
+	// flash constrained target's don't include all streams, and some are only available for the development dialect
+#if defined(CONSTRAINED_FLASH) || !defined(MAVLINK_DEVELOPMENT_H)
 	return PX4_OK;
 #else
 	PX4_WARN("stream %s not found", stream_name);
@@ -1399,9 +1400,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE", 15.0f);
 		configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("DISTANCE_SENSOR", 0.5f);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESC_INFO", 1.0f);
@@ -1469,9 +1472,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream_local("ATTITUDE_TARGET", 10.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
@@ -1543,9 +1548,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
@@ -1625,9 +1632,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE", 50.0f);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream_local("ATTITUDE_TARGET", 8.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);
 		configure_stream_local("ESC_STATUS", 10.0f);
@@ -1720,8 +1729,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
@@ -2354,7 +2365,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;

src/modules/mavlink/mavlink_messages.cpp

@@ -124,6 +124,11 @@
 #include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
 #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
 
+#ifdef MAVLINK_MSG_ID_AVAILABLE_MODES // Only defined if development.xml is used
+#include "streams/AVAILABLE_MODES.hpp"
+#include "streams/CURRENT_MODE.hpp"
+#endif
+
 #if !defined(CONSTRAINED_FLASH)
 # include "streams/ADSB_VEHICLE.hpp"
 # include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
@@ -492,8 +497,14 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
 #endif // UAVIONIX_ADSB_OUT_CFG_HPP
 #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
-	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
+	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
 #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+#if defined(AVAILABLE_MODES_HPP)
+	create_stream_list_item<MavlinkStreamAvailableModes>(),
+#endif // AVAILABLE_MODES_HPP
+#if defined(CURRENT_MODE_HPP)
+	create_stream_list_item<MavlinkStreamCurrentMode>(),
+#endif // CURRENT_MODE_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/mavlink_messages.h

@@ -75,6 +75,4 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink);
 
 MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink);
 
-union px4_custom_mode get_px4_custom_mode(uint8_t nav_state);
-
 #endif /* MAVLINK_MESSAGES_H_ */

src/modules/mavlink/streams/AVAILABLE_MODES.hpp

@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ *   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 AVAILABLE_MODES_HPP
+#define AVAILABLE_MODES_HPP
+
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/register_ext_component_reply.h>
+#include <lib/modes/standard_modes.hpp>
+#include <lib/modes/ui.hpp>
+
+class MavlinkStreamAvailableModes : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAvailableModes(mavlink); }
+
+	~MavlinkStreamAvailableModes() { delete[] _external_mode_names; }
+
+	static constexpr const char *get_name_static() { return "AVAILABLE_MODES"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AVAILABLE_MODES; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _had_dynamic_update ? MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	static constexpr int MAX_NUM_EXTERNAL_MODES = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8 -
+			vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1;
+
+	explicit MavlinkStreamAvailableModes(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	struct ExternalModeName {
+		char name[sizeof(register_ext_component_reply_s::name)] {};
+	};
+	ExternalModeName *_external_mode_names{nullptr};
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};
+
+	bool _had_dynamic_update{false};
+	uint8_t _dynamic_update_seq{0};
+	uint32_t _last_valid_nav_states_mask{0};
+	uint32_t _last_can_set_nav_states_mask{0};
+
+	void send_single_mode(const vehicle_status_s &vehicle_status, int mode_index, int total_num_modes, uint8_t nav_state)
+	{
+		mavlink_available_modes_t available_modes{};
+		available_modes.mode_index = mode_index;
+		available_modes.number_modes = total_num_modes;
+		px4_custom_mode custom_mode{get_px4_custom_mode(nav_state)};
+		available_modes.custom_mode = custom_mode.data;
+		const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0;
+
+		// Set the mode name if not a standard mode
+		available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state);
+
+		if (mode_util::isAdvanced(nav_state)) {
+			available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED;
+		}
+
+		if (available_modes.standard_mode == MAV_STANDARD_MODE_NON_STANDARD) {
+			static_assert(sizeof(available_modes.mode_name) >= sizeof(ExternalModeName::name), "mode name too short");
+
+			// Is it an external mode?
+			unsigned external_mode_index = nav_state - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
+
+			if (nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 && external_mode_index < MAX_NUM_EXTERNAL_MODES) {
+				if (cannot_be_selected) {
+					// If not selectable, it's not registered
+					strcpy(available_modes.mode_name, "(Mode not available)");
+
+				} else if (_external_mode_names) {
+					strncpy(available_modes.mode_name, _external_mode_names[external_mode_index].name, sizeof(available_modes.mode_name));
+					available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0';
+				}
+
+			} else { // Internal
+				if (nav_state < sizeof(mode_util::nav_state_names) / sizeof(mode_util::nav_state_names[0])) {
+					strncpy(available_modes.mode_name, mode_util::nav_state_names[nav_state], sizeof(available_modes.mode_name));
+					available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0';
+				}
+			}
+		}
+
+		if (cannot_be_selected) {
+			available_modes.properties |= MAV_MODE_PROPERTY_NOT_USER_SELECTABLE;
+		}
+
+		mavlink_msg_available_modes_send_struct(_mavlink->get_channel(), &available_modes);
+	}
+
+	bool request_message(float param2, float param3, float param4,
+			     float param5, float param6, float param7) override
+	{
+		bool ret = false;
+		int mode_index = roundf(param2);
+		PX4_DEBUG("AVAILABLE_MODES request (%i)", mode_index);
+
+		vehicle_status_s vehicle_status;
+
+		if (!_vehicle_status_sub.copy(&vehicle_status)) {
+			return false;
+		}
+
+		int total_num_modes = math::countSetBits(vehicle_status.valid_nav_states_mask);
+
+		if (mode_index == 0) { // All
+			int cur_mode_index = 1;
+
+			for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
+				if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) {
+					send_single_mode(vehicle_status, cur_mode_index, total_num_modes, nav_state);
+					++cur_mode_index;
+				}
+			}
+
+			ret = true;
+
+		} else if (mode_index <= total_num_modes) {
+			// Find index
+			int cur_index = 0;
+			uint8_t nav_state = 0;
+
+			for (; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
+				if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) {
+					if (++cur_index == mode_index) {
+						break;
+					}
+				}
+			}
+
+			if (nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
+				send_single_mode(vehicle_status, mode_index, total_num_modes, nav_state);
+			}
+
+			ret = true;
+		}
+
+		return ret;
+	}
+
+	void update_data() override
+	{
+		// Keep track of externally registered modes
+		register_ext_component_reply_s reply;
+		bool dynamic_update = false;
+
+		if (_register_ext_component_reply_sub.update(&reply)) {
+			if (reply.success && reply.mode_id != -1) {
+				if (!_external_mode_names) {
+					_external_mode_names = new ExternalModeName[MAX_NUM_EXTERNAL_MODES];
+				}
+
+				unsigned mode_index = reply.mode_id - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
+
+				if (_external_mode_names && mode_index < MAX_NUM_EXTERNAL_MODES) {
+					memcpy(_external_mode_names[mode_index].name, reply.name, sizeof(ExternalModeName::name));
+				}
+
+				dynamic_update = true;
+			}
+		}
+
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.copy(&vehicle_status)) {
+			if (_last_valid_nav_states_mask == 0) {
+				_last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask;
+			}
+
+			if (_last_can_set_nav_states_mask == 0) {
+				_last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask;
+			}
+
+			if (vehicle_status.valid_nav_states_mask != _last_valid_nav_states_mask) {
+				dynamic_update = true;
+				_last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask;
+			}
+
+			if (vehicle_status.can_set_nav_states_mask != _last_can_set_nav_states_mask) {
+				dynamic_update = true;
+				_last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask;
+			}
+		}
+
+		if (dynamic_update) {
+			_had_dynamic_update = true;
+			++_dynamic_update_seq;
+		}
+	}
+
+	bool send() override
+	{
+		if (_had_dynamic_update) {
+			mavlink_available_modes_monitor_t monitor{};
+			monitor.seq = _dynamic_update_seq;
+			mavlink_msg_available_modes_monitor_send_struct(_mavlink->get_channel(), &monitor);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // AVAILABLE_MODES_HPP

src/modules/mavlink/streams/CURRENT_MODE.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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 CURRENT_MODE_HPP
+#define CURRENT_MODE_HPP
+
+#include <uORB/topics/vehicle_status.h>
+#include <lib/modes/standard_modes.hpp>
+
+class MavlinkStreamCurrentMode : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCurrentMode(mavlink); }
+
+	static constexpr const char *get_name_static() { return "CURRENT_MODE"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CURRENT_MODE; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_CURRENT_MODE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamCurrentMode(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	bool send() override
+	{
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.update(&vehicle_status)) {
+			mavlink_current_mode_t current_mode{};
+			current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data;
+			current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data;
+			current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state);
+			mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), &current_mode);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // CURRENT_MODE_HPP

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -42,6 +42,8 @@
 
 #include <iostream>
 #include <string>
+#include <cstdlib>
+#include <fstream>
 
 GZBridge::GZBridge(const char *world, const char *name, const char *model,
 		   const char *pose_str) :
@@ -72,7 +74,18 @@ int GZBridge::init()
 
 		// service call to create model
 		gz::msgs::EntityFactory req{};
-		req.set_sdf_filename(_model_sim + "/model.sdf");
+		std::string filename = "../../../Tools/simulation/gz/models/" + _model_sim + "/model.sdf";
+
+		std::ifstream file(filename);
+		std::string fileContent;
+
+		if (file.is_open()) {
+			// Read the file content into a string
+			fileContent = std::string((std::istreambuf_iterator<char>(file)), (std::istreambuf_iterator<char>()));
+			file.close();
+		}
+
+		req.set_sdf(fileContent);
 
 		req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
 
@@ -116,15 +129,44 @@ int GZBridge::init()
 		bool result;
 		std::string create_service = "/world/" + _world_name + "/create";
 
-		if (_node.Request(create_service, req, 1000, rep, result)) {
+		bool gz_called = false;
-			if (!rep.data() || !result) {
+		// Check if STANDALONE has been set.
-				PX4_ERR("EntityFactory service call failed");
+		char *standalone_val = std::getenv("STANDALONE");
+
+		if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) {
+			// Check if Gazebo has been called and if not attempt to reconnect.
+			while (gz_called == false) {
+				if (_node.Request(create_service, req, 1000, rep, result)) {
+					if (!rep.data() || !result) {
+						PX4_ERR("EntityFactory service call failed");
+						return PX4_ERROR;
+
+					} else {
+						gz_called = true;
+					}
+				}
+
+				// If Gazebo has not been called, wait 2 seconds and try again.
+				else {
+					printf("WARN [gz_bridge] Service call timed out as Gazebo has not been detected. \n");
+					system_usleep(2000000);
+				}
+			}
+		}
+
+
+		// If STANDALONE has not been set, do not retry to reconnect.
+		else {
+			if (_node.Request(create_service, req, 1000, rep, result)) {
+				if (!rep.data() || !result) {
+					PX4_ERR("EntityFactory service call failed");
+					return PX4_ERROR;
+				}
+
+			} else {
+				PX4_ERR("Service call timed out");
 				return PX4_ERROR;
 			}
-
-		} else {
-			PX4_ERR("Service call timed out");
-			return PX4_ERROR;
 		}
 	}
 

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -5,12 +5,27 @@
 #####
 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/message_format_response
+    type: px4_msgs::msg::MessageFormatResponse
+
   - topic: /fmu/out/position_setpoint_triplet
     type: px4_msgs::msg::PositionSetpointTriplet
 
@@ -29,6 +44,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 +66,26 @@ 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/config_overrides_request
+    type: px4_msgs::msg::ConfigOverrides
+
+  - topic: /fmu/in/arming_check_reply
+    type: px4_msgs::msg::ArmingCheckReply
+
+  - topic: /fmu/in/message_format_request
+    type: px4_msgs::msg::MessageFormatRequest
+
+  - 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 +120,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
 

src/modules/uxrce_dds_client/uxrce_dds_client.cpp

@@ -164,6 +164,67 @@ UxrceddsClient::~UxrceddsClient()
 	}
 }
 
+static void fillMessageFormatResponse(const message_format_request_s &message_format_request,
+				      message_format_response_s &message_format_response)
+{
+	message_format_response.protocol_version = message_format_request_s::LATEST_PROTOCOL_VERSION;
+	message_format_response.success = false;
+
+	if (message_format_request.protocol_version == message_format_request_s::LATEST_PROTOCOL_VERSION) {
+		static_assert(sizeof(message_format_request.topic_name) == sizeof(message_format_response.topic_name), "size mismatch");
+		memcpy(message_format_response.topic_name, message_format_request.topic_name,
+		       sizeof(message_format_response.topic_name));
+
+		// Get the topic name by searching for the last '/'
+		int idx_last_slash = -1;
+		bool found_null = false;
+
+		for (int i = 0; i < (int)sizeof(message_format_request.topic_name); ++i) {
+			if (message_format_request.topic_name[i] == 0) {
+				found_null = true;
+				break;
+			}
+
+			if (message_format_request.topic_name[i] == '/') {
+				idx_last_slash = i;
+			}
+		}
+
+		if (found_null && idx_last_slash != -1) {
+			const char *topic_name = message_format_request.topic_name + idx_last_slash + 1;
+			// Find the format
+			const orb_metadata *const *topics = orb_get_topics();
+			const orb_metadata *topic_meta{nullptr};
+
+			for (size_t i = 0; i < orb_topics_count(); i++) {
+				if (strcmp(topic_name, topics[i]->o_name) == 0) {
+					topic_meta = topics[i];
+					break;
+				}
+			}
+
+			if (topic_meta) {
+				message_format_response.message_hash = topic_meta->message_hash;
+				// The topic type is already checked by DDS
+				message_format_response.success = true;
+			}
+		}
+	}
+
+	message_format_response.timestamp = hrt_absolute_time();
+}
+
+void UxrceddsClient::handleMessageFormatRequest()
+{
+	message_format_request_s message_format_request;
+
+	if (_message_format_request_sub.update(&message_format_request)) {
+		message_format_response_s message_format_response;
+		fillMessageFormatResponse(message_format_request, message_format_response);
+		_message_format_response_pub.publish(message_format_response);
+	}
+}
+
 void UxrceddsClient::run()
 {
 	if (!_comm) {
@@ -359,7 +420,17 @@ void UxrceddsClient::run()
 
 			_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
 
-			uxr_run_session_timeout(&session, 0);
+			// Run the session until we receive no more data or up to a maximum number of iterations.
+			// The maximum observed number of iterations was 6 (SITL). If we were to run only once, data starts to get
+			// delayed, causing registered flight modes to time out.
+			for (int i = 0; i < 10; ++i) {
+				const uint32_t prev_num_payload_received = _pubs->num_payload_received;
+				uxr_run_session_timeout(&session, 0);
+
+				if (_pubs->num_payload_received == prev_num_payload_received) {
+					break;
+				}
+			}
 
 			// time sync session
 			if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
@@ -369,6 +440,8 @@ void UxrceddsClient::run()
 				}
 			}
 
+			handleMessageFormatRequest();
+
 			// Check for a ping response
 			/* PONG_IN_SESSION_STATUS */
 			if (session.on_pong_flag == 1) {

src/modules/uxrce_dds_client/uxrce_dds_client.h

@@ -38,6 +38,10 @@
 
 #include <src/modules/uxrce_dds_client/dds_topics.h>
 
+#include <uORB/topics/message_format_request.h>
+#include <uORB/topics/message_format_response.h>
+#include <uORB/Subscription.hpp>
+
 #include <lib/timesync/Timesync.hpp>
 
 class UxrceddsClient : public ModuleBase<UxrceddsClient>, public ModuleParams
@@ -74,6 +78,11 @@ public:
 private:
 	int setBaudrate(int fd, unsigned baud);
 
+	void handleMessageFormatRequest();
+
+	uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
+	uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
+
 	const bool _localhost_only;
 	const bool _custom_participant;
 	const char *_client_namespace;