PX4 NuttX with Fat wrap fix Backport

Escape param metadata to fix broken tags
yaw_est: use error-state covariance prediction
2026-06-10 17:10:06 +08:00 · 2023-11-13 12:39:44 -08:00 · 2023-11-14 09:48:03 +01:00 · 2023-11-13 11:37:27 -05:00 · 2023-11-13 12:43:58 +01:00 · 2023-11-13 11:57:19 +01:00
121 changed files with 1014 additions and 3260 deletions
@@ -75,3 +75,4 @@
 [submodule "src/lib/heatshrink/heatshrink"]
 	path = src/lib/heatshrink/heatshrink
 	url = https://github.com/PX4/heatshrink.git
+	branch = px4
@@ -47,6 +47,8 @@ param set-default PWM_MAIN_FUNC6 201
 param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203

+param set-default FW_AIRSPD_MAX 25
+param set-default FW_THR_ASPD_MAX 0.4
 param set-default NPFG_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
@@ -62,19 +64,18 @@ param set-default FW_T_SINK_MAX 2.7
 param set-default FW_T_SINK_MIN 2.2

 param set-default MC_AIRMODE 1
+param set-default MC_ROLL_P 4
 param set-default MC_ROLLRATE_P 0.3
 param set-default MC_YAW_P 1.6
+param set-default MC_YAWRATE_P 0.3
+

 param set-default MIS_TAKEOFF_ALT 10

-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
 param set-default NAV_ACC_RAD 5

 param set-default VT_FWD_THRUST_EN 4
+param set-default VT_FWD_THRUST_SC 1
 param set-default VT_F_TRANS_THR 0.75
 param set-default VT_TYPE 2

@@ -76,7 +76,6 @@ param set-default CA_SV_CS1_TRQ_R 0.5
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1.0

-param set-default FW_L1_PERIOD 12
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
@@ -36,8 +36,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 		exit 1
 	fi

-elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
-
+elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then

 	# set local coordinate frame reference
 	if [ -n "${PX4_HOME_LAT}" ]; then
@@ -60,40 +59,36 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 		. ../gz_env.sh
 	fi

-	# Only start up Gazebo if STANDALONE set to false
-	if [ "$STANDALONE" != '1' ]; then
+	# "gz sim" only avaiilable in Garden and later
+	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+	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

-		# "gz sim" only avaiilable in Garden and later
-		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-		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
+	# look for running ${gz_command} gazebo world
+	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+
+	# 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

-		# look for running ${gz_command} gazebo world
-		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
-
-		# 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
+	else
+		echo "INFO  [init] gazebo already running world: ${gz_world}"
+		PX4_GZ_WORLD=${gz_world}
 	fi

 	# start gz_bridge
@@ -274,32 +274,26 @@ else
 		. $FCONFIG
 	fi

-	#
-	# Start IO for PWM output or RC input if enabled
-	#
-	if param compare -s SYS_USE_IO 1
+	# Check if PX4IO present and update firmware if needed.
+	if [ -f $IOFW ]
 	then
-		# Check if PX4IO present and update firmware if needed.
-		if [ -f $IOFW ]
+		if ! px4io checkcrc ${IOFW}
 		then
-			if ! px4io checkcrc ${IOFW}
-			then
-				# tune Program PX4IO
-				tune_control play -t 16 # tune 16 = PROG_PX4IO
+			# tune Program PX4IO
+			tune_control play -t 16 # tune 16 = PROG_PX4IO

-				if px4io update ${IOFW}
+			if px4io update ${IOFW}
+			then
+				usleep 10000
+				tune_control stop
+				if px4io checkcrc ${IOFW}
 				then
-					usleep 10000
-					tune_control stop
-					if px4io checkcrc ${IOFW}
-					then
-						tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
-					else
-						tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
-					fi
+					tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
 				else
-					tune_control stop
+					tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
 				fi
+			else
+				tune_control stop
 			fi
 		fi

@@ -310,6 +304,7 @@ else
 		fi
 	fi

+
 	#
 	# RC update (map raw RC input to calibrate manual control)
 	#  start before commander
@@ -171,61 +171,6 @@ 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
@@ -58,7 +58,6 @@ 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)
 }@
@@ -75,7 +74,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), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
+ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
@[end for]

 void print_message(const orb_metadata *meta, const @uorb_struct& message)
@@ -250,22 +250,6 @@ 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))

@@ -69,7 +69,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae</uri>
+            <uri>model://rc_cessna/meshes/body.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -184,7 +184,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -229,7 +229,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae</uri>
+            <uri>model://rc_cessna/meshes/left_aileron.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -385,7 +385,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae</uri>
+            <uri>model://rc_cessna/meshes/right_aileron.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -412,7 +412,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae</uri>
+            <uri>model://rc_cessna/meshes/left_flap.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -439,7 +439,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae</uri>
+            <uri>model://rc_cessna/meshes/right_flap.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -466,7 +466,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae</uri>
+            <uri>model://rc_cessna/meshes/elevators.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -493,7 +493,7 @@
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae</uri>
+            <uri>model://rc_cessna/meshes/rudder.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -806,7 +806,7 @@
    <plugin filename="gz-sim-multicopter-motor-model-system" name="gz::sim::systems::MulticopterMotorModel">
      <jointName>rotor_puller_joint</jointName>
      <linkName>rotor_puller</linkName>
-      <turningDirection>cw</turningDirection>
+      <turningDirection>ccw</turningDirection>
      <timeConstantUp>0.0125</timeConstantUp>
      <timeConstantDown>0.025</timeConstantDown>
      <maxRotVelocity>1000</maxRotVelocity>
@@ -43,7 +43,7 @@
        <geometry>
          <mesh>
            <scale>0.001 0.001 0.001</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae</uri>
+            <uri>model://standard_vtol/meshes/x8_wing.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -183,7 +183,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -246,7 +246,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -309,7 +309,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -372,7 +372,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -436,7 +436,7 @@
        <geometry>
          <mesh>
            <scale>0.8 0.8 0.8</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
+            <uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -483,7 +483,7 @@
        <geometry>
          <mesh>
            <scale>0.001 0.001 0.001</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae</uri>
+            <uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -510,7 +510,7 @@
        <geometry>
          <mesh>
            <scale>0.001 0.001 0.001</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae</uri>
+            <uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
          </mesh>
        </geometry>
        <material>
@@ -23,7 +23,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae</uri>
+            <uri>model://x500/meshes/NXP-HGD-CF.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -32,7 +32,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
+            <uri>model://x500/meshes/5010Base.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -41,7 +41,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
+            <uri>model://x500/meshes/5010Base.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -50,7 +50,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
+            <uri>model://x500/meshes/5010Base.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -59,7 +59,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
+            <uri>model://x500/meshes/5010Base.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -77,7 +77,7 @@
          <specular>1.0 1.0 1.0</specular>
          <pbr>
            <metal>
-              <albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
+              <albedo_map>model://x500/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>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
+              <albedo_map>model://x500/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>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png</albedo_map>
+              <albedo_map>model://x500/materials/textures/rd.png</albedo_map>
            </metal>
          </pbr>
        </material>
@@ -250,7 +250,7 @@
        <geometry>
          <mesh>
            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
+            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
@@ -265,7 +265,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
+            <uri>model://x500/meshes/5010Bell.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -322,7 +322,7 @@
        <geometry>
          <mesh>
            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
+            <uri>model://x500/meshes/1345_prop_ccw.stl</uri>
          </mesh>
        </geometry>
        <material>
@@ -337,7 +337,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
+            <uri>model://x500/meshes/5010Bell.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -394,7 +394,7 @@
        <geometry>
          <mesh>
            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
+            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
@@ -409,7 +409,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
+            <uri>model://x500/meshes/5010Bell.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -466,7 +466,7 @@
        <geometry>
          <mesh>
            <scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
+            <uri>model://x500/meshes/1345_prop_cw.stl</uri>
          </mesh>
        </geometry>
        <material>
@@ -481,7 +481,7 @@
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
-            <uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
+            <uri>model://x500/meshes/5010Bell.dae</uri>
          </mesh>
        </geometry>
      </visual>
@@ -32,11 +32,4 @@ then
 	param set-default SENS_TEMP_ID 3014666
 fi

-if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007
-then
-	param set-default SYS_USE_IO 0
-else
-	param set-default SYS_USE_IO 1
-fi
-
 safety_button start
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -14,6 +14,4 @@ param set-default SENS_EN_THERMAL 0

 param set-default -s SENS_TEMP_ID 2621474

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 17
 # Disable IMU thermal control
 param set-default SENS_EN_THERMAL 0

-param set-default SYS_USE_IO 1
-
 set IOFW "/etc/extras/cubepilot_io-v2_default.bin"
@@ -11,5 +11,3 @@ param set-default BAT2_A_PER_V 36.367515152

 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
-
-param set-default SYS_USE_IO 1
@@ -9,7 +9,5 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152

-param set-default SYS_USE_IO 1
-
 rgbled_pwm start
 safety_button start
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 10.177939394
 param set-default BAT1_A_PER_V 15.391030303
-
-param set-default SYS_USE_IO 1
@@ -13,6 +13,4 @@ param set-default BAT2_A_PER_V 26.4
 param set-default EKF2_MULTI_IMU 2
 param set-default SENS_IMU_MODE 0

-param set-default SYS_USE_IO 1
-
 set LOGGER_BUF 64
@@ -9,13 +9,6 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152

-if ver hwtypecmp V5004000 V5006000
-then
-	param set-default SYS_USE_IO 0
-else
-	param set-default SYS_USE_IO 1
-fi
-
 if ver hwtypecmp V5005000 V5005002 V5006000 V5006002
 then
 	# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs
@@ -111,4 +111,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_MAVLINK_DIALECT="development"
@@ -16,8 +16,6 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-param set-default SYS_USE_IO 1
-
 if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
@@ -9,5 +9,3 @@ param set-default BAT2_V_DIV 18.1

 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -16,8 +16,6 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-param set-default SYS_USE_IO 1
-
 if ver hwtypecmp V6X009010 V6X010010
 then
 	# Skynode: use the "custom participant" config for uxrce_dds_client
@@ -10,5 +10,3 @@ param set-default BAT1_A_PER_V 36.367515152

 # Enable IMU thermal control
 param set-default SENS_EN_THERMAL 1
-
-param set-default SYS_USE_IO 1
@@ -25,8 +25,6 @@ param set-default BAT_V_OFFS_CURR 0.413
 # Disable safety switch
 param set-default CBRK_IO_SAFETY 22027

-param set-default SYS_USE_IO 1
-
 safety_button start

 set LOGGER_BUF 32
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -5,5 +5,3 @@

 param set-default BAT1_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
-
-param set-default SYS_USE_IO 1
@@ -1,34 +0,0 @@
-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
-
@@ -1,7 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-# broadcast message to request all registered arming checks to be reported
-
-uint8 request_id
-
-
@@ -49,8 +49,6 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
-	ArmingCheckReply.msg
-	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg
@@ -60,7 +58,6 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
-	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DatamanRequest.msg
@@ -133,8 +130,6 @@ set(msg_files
 	ManualControlSwitches.msg
 	MavlinkLog.msg
 	MavlinkTunnel.msg
-	MessageFormatRequest.msg
-	MessageFormatResponse.msg
 	Mission.msg
 	MissionResult.msg
 	MountOrientation.msg
@@ -166,8 +161,6 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
-	RegisterExtComponentReply.msg
-	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -205,7 +198,6 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
-	UnregisterExtComponent.msg
 	VehicleAcceleration.msg
 	VehicleAirData.msg
 	VehicleAngularAccelerationSetpoint.msg
@@ -1,19 +0,0 @@
-# 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
-
@@ -1,10 +0,0 @@
-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
-
@@ -1,11 +0,0 @@
-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
-
@@ -1,14 +0,0 @@
-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
-
@@ -1,21 +0,0 @@
-# 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
@@ -1,10 +0,0 @@
-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)
-
-
-
@@ -71,7 +71,6 @@ 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)|
@@ -104,7 +103,6 @@ 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. |
@@ -176,10 +174,8 @@ 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
-uint16 source_component	# Component / mode executor sending the command
+uint8 source_component		# Component sending the command
 uint8 confirmation		# 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
 bool from_external

-uint16 COMPONENT_MODE_EXECUTOR_START = 1000
-
-# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
+# TOPICS vehicle_command gimbal_v1_command
@@ -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
-uint16 target_component 				# Target component / mode executor
+uint8 target_component

 bool from_external					# Indicates if the command came from an external source
@@ -15,8 +15,3 @@ 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
@@ -52,20 +52,7 @@ 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_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
+uint8 NAVIGATION_STATE_MAX = 23

 # Bitmask of detected failures
 uint16 failure_detector_status
@@ -91,13 +78,8 @@ 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
@@ -51,7 +51,6 @@ 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 */
 };

@@ -100,15 +99,13 @@ 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, _message_hash, _orb_id_enum)		\
+#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum)		\
 	const struct orb_metadata __orb_##_name = {	\
 		#_name,					\
 		sizeof(_struct),		\
 		_size_no_padding,			\
-		_message_hash,				\
 		_orb_id_enum				\
 	}; struct hack

@@ -336,8 +336,7 @@ private:
 		(ParamInt<px4::params::RC_RSSI_PWM_MAX>) _param_rc_rssi_pwm_max,
 		(ParamInt<px4::params::RC_RSSI_PWM_MIN>) _param_rc_rssi_pwm_min,
 		(ParamInt<px4::params::SENS_EN_THERMAL>) _param_sens_en_themal,
-		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl,
-		(ParamInt<px4::params::SYS_USE_IO>) _param_sys_use_io
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
 	)
 };

@@ -468,7 +467,7 @@ int PX4IO::init()
 	}

 	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
-	if (_param_sys_hitl.get() <= 0 && _param_sys_use_io.get() == 1) {
+	if (_param_sys_hitl.get() <= 0) {
 		_mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL);
 	}

@@ -42,18 +42,6 @@
 #include <px4_platform_common/px4_config.h>
 #include <parameters/param.h>

-/**
- * Set usage of IO board
- *
- * Can be used to use a configure the use of the IO board.
- *
- * @value 0 IO PWM disabled (RC only)
- * @value 1 IO enabled (RC & PWM)
- * @reboot_required true
- * @group System
- */
-PARAM_DEFINE_INT32(SYS_USE_IO, 0);
-
 /**
 * S.BUS out
 *
@@ -206,6 +206,10 @@ void Battery::sumDischarged(const hrt_abstime &timestamp, float current_a)

 float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const float current_a)
 {
+	if (_params.n_cells == 0) {
+		return -1.0f;
+	}
+
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;

@@ -110,38 +110,6 @@
                        "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"
                        }
                    }
                },
@@ -564,38 +532,6 @@
                            "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]"
@@ -769,15 +705,7 @@
                    "19": [134479872],
                    "20": [151257088],
                    "21": [16973824],
-                    "22": [168034304],
-                    "23": [184811520],
-                    "24": [201588736],
-                    "25": [218365952],
-                    "26": [235143168],
-                    "27": [251920384],
-                    "28": [268697600],
-                    "29": [285474816],
-                    "30": [302252032]
+                    "22": [168034304]
                }
            },
            "supported_protocols": [ "health_and_arming_check" ]
@@ -38,7 +38,7 @@
 #include <uORB/topics/vehicle_command.h>

 /**
- * Functions: Peripheral_via_Actuator_Set1 ... Peripheral_via_Actuator_Set6
+ * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
 */
 class FunctionActuatorSet : public FunctionProviderBase
 {
@@ -72,7 +72,7 @@ public:
 		}
 	}

-	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Peripheral_via_Actuator_Set1]; }
+	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }

 private:
 	static constexpr int max_num_actuators = 6;
@@ -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::Peripheral_via_Actuator_Set1, OutputFunction::Peripheral_via_Actuator_Set6, &FunctionActuatorSet::allocate},
+	{OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate},
 	{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
 	{OutputFunction::Landing_Gear_Wheel, &FunctionLandingGearWheel::allocate},
 	{OutputFunction::Parachute, &FunctionParachute::allocate},
@@ -13,7 +13,7 @@ functions:
    Servo:
      start: 201
      count: 8
-    Peripheral_via_Actuator_Set:
+    Offboard_Actuator_Set:
      start: 301
      count: 6

@@ -1,133 +0,0 @@
-/****************************************************************************
- *
- *   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
@@ -134,21 +134,20 @@ const Vector3f PositionSmoothing::_getCrossingPoint(const Vector3f &position, co
 	}

 	// Get the crossing point using L1-style guidance
-	auto l1_point = _getL1Point(position, waypoints);
-	return {l1_point(0), l1_point(1), target(2)};
+	return _getL1Point(position, waypoints);
 }

-const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
+const Vector3f PositionSmoothing::_getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const
 {
-	const Vector2f pos_traj(_trajectory[0].getCurrentPosition(),
-				_trajectory[1].getCurrentPosition());
-	const Vector2f u_prev_to_target = Vector2f(waypoints[1] - waypoints[0]).unit_or_zero();
-	const Vector2f prev_to_pos(pos_traj - Vector2f(waypoints[0]));
-	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
-	const Vector2f closest_pt = Vector2f(waypoints[0]) + prev_to_closest;
+	const Vector3f pos_traj(_trajectory[0].getCurrentPosition(), _trajectory[1].getCurrentPosition(),
+				_trajectory[2].getCurrentPosition());
+	const Vector3f u_prev_to_target = (waypoints[1] - waypoints[0]).unit_or_zero();
+	const Vector3f prev_to_pos(pos_traj - waypoints[0]);
+	const Vector3f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+	const Vector3f closest_pt = waypoints[0] + prev_to_closest;

 	// Compute along-track error using L1 distance and cross-track error
-	const float crosstrack_error = Vector2f(closest_pt - pos_traj).length();
+	const float crosstrack_error = (closest_pt - pos_traj).length();

 	const float l1 = math::max(_target_acceptance_radius, 5.f);
 	float alongtrack_error = 0.f;
@@ -159,9 +158,7 @@ const Vector2f PositionSmoothing::_getL1Point(const Vector3f &position, const Ve
 	}

 	// Position of the point on the line where L1 intersect the line between the two waypoints
-	const Vector2f l1_point = closest_pt + alongtrack_error * u_prev_to_target;
-
-	return l1_point;
+	return closest_pt + alongtrack_error * u_prev_to_target;
 }

 const Vector3f PositionSmoothing::_generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
@@ -438,7 +438,7 @@ private:
 	const Vector3f _generateVelocitySetpoint(const Vector3f &position, const Vector3f(&waypoints)[3],
 			bool is_single_waypoint,
 			const Vector3f &feedforward_velocity_setpoint);
-	const Vector2f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
+	const Vector3f _getL1Point(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	const Vector3f _getCrossingPoint(const Vector3f &position, const Vector3f(&waypoints)[3]) const;
 	float _getMaxXYSpeed(const Vector3f(&waypoints)[3]) const;
 	float _getMaxZSpeed(const Vector3f(&waypoints)[3]) const;
@@ -1,5 +1,6 @@
 from xml.sax.saxutils import escape
 import codecs
+import html

 class MarkdownTablesOutput():
    def __init__(self, groups):
@@ -45,7 +46,9 @@ table {
            for param in group.GetParams():
                code = param.GetName()
                name = param.GetFieldValue("short_desc") or ''
+                name = html.escape(name)
                long_desc = param.GetFieldValue("long_desc") or ''
+                long_desc = html.escape(long_desc)
                min_val = param.GetFieldValue("min") or ''
                max_val = param.GetFieldValue("max") or ''
                increment = param.GetFieldValue("increment") or ''
@@ -68,16 +71,16 @@ table {
                        min_val='?'
                    if not max_val:
                        max_val='?'
-                    max_min_combined+='[%s, %s] ' % (min_val, max_val)
+                    max_min_combined+=f"[{min_val}, {max_val}] "
                if increment:
                    max_min_combined+='(%s)' % increment

                if long_desc != '':
-                    long_desc = '<p><strong>Comment:</strong> %s</p>' % long_desc
+                    long_desc = f"<p><strong>Comment:</strong> {long_desc}</p>"

                if name == code:
                    name = ""
-                code='<strong id="%s">%s</strong>' % (code, code)
+                code=f"<strong id=\"{code}\">{code}</strong>"

                if reboot_required:
                    reboot_required='<p><b>Reboot required:</b> %s</p>\n' % reboot_required
@@ -89,8 +92,8 @@ table {
                    enum_output+='<strong>Values:</strong><ul>'
                    enum_codes=sorted(enum_codes,key=float)
                    for item in enum_codes:
-                        enum_output+='\n<li><strong>%s:</strong> %s</li> \n' % (item, param.GetEnumValue(item))
-                    enum_output+='</ul>\n'
+                        enum_output+=f"\n<li><strong>{item}:</strong> {html.escape(param.GetEnumValue(item))}</li>"
+                    enum_output+='\n</ul>'


                bitmask_list=param.GetBitmaskList() #Gets bitmask values for parameter
@@ -100,7 +103,8 @@ table {
                    bitmask_output+='<strong>Bitmask:</strong><ul>'
                    for bit in bitmask_list:
                        bit_text = param.GetBitmaskBit(bit)
-                        bitmask_output+='  <li><strong>%s:</strong> %s</li> \n' % (bit, bit_text)
+                        bit_text = html.escape(bit_text)
+                        bitmask_output+=f"  <li><strong>{bit}:</strong> {bit_text}</li>\n"
                    bitmask_output+='</ul>\n'

                if is_boolean and def_val=='1':
@@ -108,7 +112,7 @@ table {
                if is_boolean and def_val=='0':
                    def_val='Disabled (0)'

-                result += '<tr>\n <td>%s (%s)</td>\n <td>%s %s %s %s %s</td>\n <td>%s</td>\n <td>%s</td>\n <td>%s</td>\n</tr>\n' % (code, type, name, long_desc, enum_output, bitmask_output, reboot_required, max_min_combined, def_val, unit)
+                result += f"<tr>\n <td>{code} ({type})</td>\n <td>{name} {long_desc} {enum_output} {bitmask_output} {reboot_required}</td>\n <td>{max_min_combined}</td>\n <td>{def_val}</td>\n <td>{unit}</td>\n</tr>\n"

            #Close the table.
            result += '</tbody></table>\n\n'
@@ -11,7 +11,7 @@
 * @decimal 2
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f);
+PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-1f);

 /**
 * Wind estimator true airspeed scale process noise spectral density
@@ -51,7 +51,7 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f);
 * @decimal 3
 * @group Airspeed Validator
 */
-PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f);
+PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.15f);

 /**
 * Gate size for true airspeed fusion
@@ -63,7 +63,7 @@ PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f);
 * @unit SD
 * @group Airspeed Validator
 */
-PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3);
+PARAM_DEFINE_INT32(ASPD_TAS_GATE, 4);

 /**
 * Gate size for sideslip angle fusion
@@ -53,8 +53,6 @@ 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
@@ -62,8 +60,6 @@ px4_add_module(
 		Safety.cpp
 		UserModeIntention.cpp
 		worker_thread.cpp
-	MODULE_CONFIG
-		module.yaml
 	DEPENDS
 		ArmAuthorization
 		circuit_breaker
@@ -79,4 +75,3 @@ px4_add_module(
 	)

 px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
-px4_add_functional_gtest(SRC ModeManagementTest.cpp LINKLIBS modules__commander)
@@ -48,8 +48,6 @@
 #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>
@@ -409,10 +407,6 @@ 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]);
 			}
@@ -481,9 +475,8 @@ 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[_vehicle_status.nav_state_user_intention]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
 	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;
@@ -734,7 +727,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, getSourceFromCommand(cmd))) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 				} else {
@@ -814,10 +807,6 @@ 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");
@@ -859,7 +848,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, getSourceFromCommand(cmd))) {
+				if (_user_mode_intention.change(desired_nav_state)) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@@ -880,18 +869,6 @@ 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
@@ -992,7 +969,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, getSourceFromCommand(cmd))) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
 				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;
@@ -1006,7 +983,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, getSourceFromCommand(cmd))) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 			} else {
@@ -1020,7 +997,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, getSourceFromCommand(cmd))) {
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) {
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;

 		} else {
@@ -1034,7 +1011,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, getSourceFromCommand(cmd))) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
 				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");
@@ -1048,7 +1025,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, getSourceFromCommand(cmd))) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
 				mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
 				events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
 					     "Landing using precision landing");
@@ -1072,7 +1049,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, getSourceFromCommand(cmd))
+					if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 					    && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {

 						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1111,7 +1088,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, getSourceFromCommand(cmd))) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@@ -1120,7 +1097,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, getSourceFromCommand(cmd))) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@@ -1405,24 +1382,6 @@ 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);
@@ -1486,43 +1445,6 @@ 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())) {
@@ -1647,7 +1569,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, ModeChangeSource::User, true)) {
+		if (!_user_mode_intention.change(action_request.mode, true)) {
 			printRejectMode(action_request.mode);
 		}

@@ -1772,6 +1694,8 @@ void Commander::run()

 		safetyButtonUpdate();

+		_multicopter_throw_launch.update(isArmed());
+
 		vtolStatusUpdate();

 		_home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed);
@@ -1795,8 +1719,6 @@ void Commander::run()
 			_status_changed = true;
 		}

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

 		const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
@@ -1819,8 +1741,6 @@ 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();
@@ -1855,7 +1775,8 @@ void Commander::run()
 		_actuator_armed.prearmed = getPrearmState();
 		_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
 		_actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION)
-					    || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON));
+					    || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)
+					    || _multicopter_throw_launch.isThrowLaunchInProgress());
 		// _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL
 		_actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
 		// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
@@ -1881,7 +1802,6 @@ 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);

@@ -1963,8 +1883,7 @@ void Commander::checkForMissionUpdate()

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

 			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
 			    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
@@ -2169,6 +2088,9 @@ void Commander::updateTunes()
 	} else if (_vehicle_status.failsafe && isArmed()) {
 		tune_failsafe(true);

+	} else if (_multicopter_throw_launch.isReadyToThrow()) {
+		set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
+
 	} else {
 		set_tune(tune_control_s::TUNE_ID_STOP);
 	}
@@ -2215,10 +2137,8 @@ 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 (auto_disarm_land_enabled && _have_taken_off_since_arming) {
+			if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
 				_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());

@@ -2227,7 +2147,7 @@ void Commander::handleAutoDisarm()
 				_auto_disarm_landed.set_state_and_update(true, hrt_absolute_time());
 			}

-			if (_auto_disarm_landed.get_state()) {
+			if (_auto_disarm_landed.get_state() && !_multicopter_throw_launch.isThrowLaunchInProgress()) {
 				if (_have_taken_off_since_arming) {
 					disarm(arm_disarm_reason_t::auto_disarm_land);

@@ -2246,6 +2166,9 @@ void Commander::handleAutoDisarm()
 			auto_disarm |= _actuator_armed.lockdown;
 		}

+		//don't disarm if throw launch is in progress
+		auto_disarm &= !_multicopter_throw_launch.isThrowLaunchInProgress();
+
 		_auto_disarm_killed.set_state_and_update(auto_disarm, hrt_absolute_time());

 		if (_auto_disarm_killed.get_state()) {
@@ -2267,7 +2190,6 @@ 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();
@@ -2287,16 +2209,13 @@ 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, ModeChangeSource::User, false, true);
+		_user_mode_intention.change(updated_user_intented_mode, false, true);
 		_user_mode_intention.getHadModeChangeAndClear();
 	}

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

 	switch (_failsafe.selectedAction()) {
 	case FailsafeBase::Action::Disarm:
@@ -2318,24 +2237,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 		_vehicle_status.nav_state_timestamp = hrt_absolute_time();
 	}

-	_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;
+	return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
 }

 void Commander::checkAndInformReadyForTakeoff()
@@ -2355,21 +2257,6 @@ 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()) {
@@ -2420,6 +2307,10 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 			led_mode = led_control_s::MODE_BLINK_FAST;
 			led_color = led_control_s::COLOR_PURPLE;

+		} else if (_multicopter_throw_launch.isReadyToThrow()) {
+			led_mode = led_control_s::MODE_BLINK_FAST;
+			led_color = led_control_s::COLOR_YELLOW;
+
 		} else if (isArmed()) {
 			led_mode = led_control_s::MODE_ON;
 			set_normal_color = true;
@@ -2515,19 +2406,8 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 void Commander::updateControlMode()
 {
 	_vehicle_control_mode = {};
-
-	mode_util::getVehicleControlMode(_vehicle_status.nav_state,
+	mode_util::getVehicleControlMode(isArmed(), _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);
 }
@@ -2877,7 +2757,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, ModeChangeSource::User, true)) {
+						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, 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");
@@ -2894,7 +2774,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, ModeChangeSource::User, false, true);
+				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true);
 			}
 		}
 	}
@@ -2956,7 +2836,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|ext1",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");
@@ -38,7 +38,8 @@
 #include "failure_detector/FailureDetector.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
-#include "ModeManagement.hpp"
+#include "MulticopterThrowLaunch/MulticopterThrowLaunch.hpp"
+#include "Safety.hpp"
 #include "UserModeIntention.hpp"
 #include "worker_thread.hpp"

@@ -123,7 +124,6 @@ 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);

@@ -194,10 +194,6 @@ private:

 	void checkAndInformReadyForTakeoff();

-	void handleCommandsFromModeExecutors();
-
-	void modeManagementUpdate();
-
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -220,17 +216,11 @@ private:
 	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};
@@ -291,7 +281,6 @@ 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)};

@@ -311,7 +300,6 @@ 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)};

@@ -65,7 +65,6 @@ px4_add_library(health_and_arming_checks
 	checks/vtolCheck.cpp
 	checks/windCheck.cpp

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

@@ -87,30 +87,6 @@ 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
@@ -249,8 +249,8 @@ public:
 	void clearArmingBits(NavModes modes);

 	/**
-	 * Clear can_run bits for certain modes. This will prevent mode switching.
-	 * For failsafe use the mode requirements instead, which then will clear the can_run bits.
+	 * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
+	 * mode is being run.
 	 * @param modes affected modes
 	 */
 	void clearCanRunBits(NavModes modes);
@@ -259,8 +259,6 @@ 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:

 	/**
@@ -309,7 +307,6 @@ 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);
@@ -69,7 +69,6 @@
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"
 #include "checks/openDroneIDCheck.hpp"
-#include "checks/externalChecks.hpp"

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

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

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

-	HealthAndArmingCheckBase *_checks[40] = {
-#ifndef CONSTRAINED_FLASH
-		&_external_checks,
-#endif
+	HealthAndArmingCheckBase *_checks[31] = {
 		&_accelerometer_checks,
 		&_airspeed_checks,
 		&_arm_permission_checks,
@@ -172,7 +161,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, _external_checks
+		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
 		&_open_drone_id_checks,
 		&_parachute_checks,
 		&_power_checks,
@@ -1,335 +0,0 @@
-/****************************************************************************
- *
- *   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");
-	}
-}
-
@@ -1,108 +0,0 @@
-/****************************************************************************
- *
- *   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)};
-};
@@ -1,639 +0,0 @@
-/****************************************************************************
- *
- *   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 */
@@ -1,225 +0,0 @@
-/****************************************************************************
- *
- *   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 */
@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- *   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());
-}
@@ -42,10 +42,11 @@ static bool stabilization_required(uint8_t vehicle_type)
 	return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 }

-void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
+void getVehicleControlMode(bool armed, 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) {
@@ -162,11 +163,17 @@ void getVehicleControlMode(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
@@ -41,7 +41,7 @@
 namespace mode_util
 {

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

@@ -75,27 +75,36 @@ 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  == 31, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 23, "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
@@ -170,9 +170,8 @@ 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 == 31, "update mode requirements");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements");
 }

 } // namespace mode_util
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(MulticopterThrowLaunch
+	MulticopterThrowLaunch.cpp
+)
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 "MulticopterThrowLaunch.hpp"
+#include <px4_platform_common/events.h>
+
+MulticopterThrowLaunch::MulticopterThrowLaunch(ModuleParams *parent) :
+	ModuleParams(parent)
+{}
+
+void MulticopterThrowLaunch::update(const bool armed)
+{
+	if (_param_com_throw_en.get()) {
+		if (_vehicle_local_position_sub.updated()) {
+			vehicle_local_position_s vehicle_local_position{};
+
+			if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
+				_last_velocity = matrix::Vector3f(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+			}
+		}
+
+		if (!armed && _throw_launch_state != ThrowLaunchState::IDLE) {
+			events::send(events::ID("mc_throw_launch_not_ready"), events::Log::Critical, "Disarmed, don't throw");
+			_throw_launch_state = ThrowLaunchState::IDLE;
+		}
+
+		switch (_throw_launch_state) {
+		case ThrowLaunchState::IDLE:
+			if (armed) {
+				events::send(events::ID("mc_throw_launch_ready"), events::Log::Critical, "Ready for throw launch");
+				_throw_launch_state = ThrowLaunchState::ARMED;
+			}
+
+			break;
+
+		case ThrowLaunchState::ARMED:
+			if (_last_velocity.longerThan(_param_com_throw_min_speed.get())) {
+				PX4_INFO("Throw detected, motors will start once falling");
+				_throw_launch_state = ThrowLaunchState::UNSAFE;
+			}
+
+			break;
+
+		case ThrowLaunchState::UNSAFE:
+			if (_last_velocity(2) > 0.f) {
+				PX4_INFO("Throw and fall detected, starting motors");
+				_throw_launch_state = ThrowLaunchState::FLYING;
+			}
+
+			break;
+
+		case ThrowLaunchState::DISABLED:
+		case ThrowLaunchState::FLYING:
+			// Nothing to do
+			break;
+		}
+
+	} else if (_throw_launch_state != ThrowLaunchState::DISABLED) {
+		// make sure everything is reset when the throw launch is disabled
+		_throw_launch_state = ThrowLaunchState::DISABLED;
+	}
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,48 +31,61 @@
 *
 ****************************************************************************/

-#ifndef CURRENT_MODE_HPP
-#define CURRENT_MODE_HPP
+/**
+ * @file MulticopterThrowLaunch.hpp
+ *
+ * Changes to manage a takeoff of a multicopter by manually throwing it into the air.
+ *
+ * @author Michał Barciś <mbarcis@mbarcis.net>
+ */

-#include <uORB/topics/vehicle_status.h>
-#include <lib/modes/standard_modes.hpp>
+#pragma once

-class MavlinkStreamCurrentMode : public MavlinkStream
+#include <lib/matrix/matrix/math.hpp>
+#include <px4_platform_common/module_params.h>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_local_position.h>
+
+class MulticopterThrowLaunch : public ModuleParams
 {
 public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCurrentMode(mavlink); }
+	explicit MulticopterThrowLaunch(ModuleParams *parent);
+	~MulticopterThrowLaunch() override = default;

-	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 false if feature disabled or already flying
+	 */
+	bool isThrowLaunchInProgress() const
 	{
-		return MAVLINK_MSG_ID_CURRENT_MODE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return _throw_launch_state != ThrowLaunchState::DISABLED
+		       && _throw_launch_state != ThrowLaunchState::FLYING;
 	}

+	bool isReadyToThrow() const { return _throw_launch_state == ThrowLaunchState::ARMED; }
+
+	/**
+	 * Main update of the state
+	 * @param armed true if vehicle is armed
+	 */
+	void update(const bool armed);
+
+	enum class ThrowLaunchState {
+		DISABLED = 0,
+		IDLE = 1,
+		ARMED = 2,
+		UNSAFE = 3,
+		FLYING = 4
+	};
+
 private:
-	explicit MavlinkStreamCurrentMode(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};

-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	ThrowLaunchState _throw_launch_state{ThrowLaunchState::DISABLED};
+	matrix::Vector3f _last_velocity{};

-	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;
-	}
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::COM_THROW_EN>) _param_com_throw_en,
+		(ParamFloat<px4::params::COM_THROW_SPEED>) _param_com_throw_min_speed
+	);
 };
-
-#endif // CURRENT_MODE_HPP
@@ -35,22 +35,14 @@
 #include "UserModeIntention.hpp"

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

-bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback,
-			       bool force)
+bool UserModeIntention::change(uint8_t user_intended_nav_state, 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();
@@ -76,10 +68,6 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource
 		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;
@@ -37,42 +37,21 @@
 #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, ModeChangeHandler *handler);
+			  const HealthAndArmingChecks &health_and_arming_checks);
 	~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, ModeChangeSource source = ModeChangeSource::User,
-		    bool allow_fallback = false, bool force = false);
+	bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);

 	uint8_t get() const { return _user_intented_nav_state; }

@@ -93,7 +72,6 @@ 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
@@ -345,6 +345,150 @@ 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
 *
@@ -990,3 +1134,30 @@ PARAM_DEFINE_INT32(COM_ARMABLE, 1);
 * @group Commander
 */
 PARAM_DEFINE_FLOAT(COM_ARM_BAT_MIN, 0.f);
+
+/**
+ * Enable throw-start
+ *
+ * Allows to start the vehicle by throwing it into the air.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_THROW_EN, 0);
+
+/**
+ * Minimum speed for the throw start
+ *
+ * When the throw launch is enabled, the drone will only arm after this speed is exceeded before detecting
+ * the freefall. This is a safety feature to ensure the drone does not turn on after accidental drop or
+ * a rapid movement before the throw.
+ *
+ * Set to 0 to disable.
+ *
+ * @group Commander
+ * @min 0
+ * @decimal 1
+ * @increment 0.1
+ * @unit m/s
+ */
+PARAM_DEFINE_FLOAT(COM_THROW_SPEED, 5);
@@ -1,50 +0,0 @@
-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
@@ -50,8 +50,7 @@ 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_TERMINATION
+	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
 };

 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -64,15 +63,7 @@ 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_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,
+	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
 };

 enum PX4_CUSTOM_SUB_MODE_POSCTL {
@@ -140,7 +131,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_TERMINATION;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 		break;

 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
@@ -180,46 +171,6 @@ 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;
@@ -283,6 +283,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
 	const float sin_yaw = sinf(_ekf_gsf[model_index].X(2));
 	const float dvx =   del_vel_NED(0) * cos_yaw + del_vel_NED(1) * sin_yaw;
 	const float dvy = - del_vel_NED(0) * sin_yaw + del_vel_NED(1) * cos_yaw;
+	const float daz = Vector3f(_ahrs_ekf_gsf[model_index].R * delta_ang)(2);

 	// delta velocity process noise double if we're not in air
 	const float accel_noise = in_air ? _accel_noise : 2.f * _accel_noise;
@@ -292,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang
 	const float d_ang_var = sq(_gyro_noise * delta_ang_dt);

 	sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P,
-				     Vector2f(dvx, dvy), d_vel_var, d_ang_var, &_ekf_gsf[model_index].P);
+				     Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var, &_ekf_gsf[model_index].P);

 	// covariance matrix is symmetrical, so copy upper half to lower half
 	_ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1);
@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
 """
-    Copyright (c) 2022 PX4 Development Team
+    Copyright (c) 2022-2023 PX4 Development Team
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions
    are met:
@@ -37,51 +37,86 @@ import symforce
 symforce.set_epsilon_to_symbol()

 import symforce.symbolic as sf
+from symforce.values import Values
 from derivation_utils import *

-class State:
-    vx = 0
-    vy = 1
-    yaw = 2
-    n_states = 3
+State = Values(
+    vel = sf.V2(),
+    R = sf.Rot2() # 2D rotation to handle angle wrap
+)

-class VState(sf.Matrix):
-    SHAPE = (State.n_states, 1)
+class VTangent(sf.Matrix):
+    SHAPE = (State.tangent_dim(), 1)

-class MState(sf.Matrix):
-    SHAPE = (State.n_states, State.n_states)
+class MTangent(sf.Matrix):
+    SHAPE = (State.tangent_dim(), State.tangent_dim())
+
+def rot2_small_angle(angle: sf.V1):
+    # Approximation for small "delta angles" to avoid trigonometric functions
+    return sf.Rot2(sf.Complex(1, angle[0]))

 def yaw_est_predict_covariance(
-        state: VState,
-        P: MState,
+        state: VTangent,
+        P: MTangent,
        d_vel: sf.V2,
        d_vel_var: sf.Scalar,
-        d_ang_var: sf.Scalar
+        d_ang: sf.Scalar,
+        d_ang_var: sf.Scalar,
 ):
-    d_ang = sf.Symbol("d_ang") # does not appear in the jacobians
+    state = State.from_tangent(state)
+    d_ang = sf.V1(d_ang) # cast to vector to gain group properties (e.g.: to_tangent)

-    # derive the body to nav direction transformation matrix
-    Tbn = sf.Matrix([[sf.cos(state[State.yaw]) , -sf.sin(state[State.yaw])],
-                [sf.sin(state[State.yaw]) ,  sf.cos(state[State.yaw])]])
+    state_error = Values(
+        vel = sf.V2.symbolic("delta_vel"),
+        yaw = sf.V1.symbolic("delta_yaw")
+    )

-    # attitude update equation
-    yaw_new = state[State.yaw] + d_ang
+    # True state kinematics
+    state_t = Values(
+        vel = state["vel"] + state_error["vel"],
+        R = state["R"] * rot2_small_angle(state_error["yaw"])
+    )

-    # velocity update equations
-    v_new = sf.V2(state[State.vx], state[State.vy]) + Tbn * d_vel
+    noise = Values(
+        d_vel = sf.V2.symbolic("a_n"),
+        d_ang = sf.V1.symbolic("w_n"),
+    )

-    # Define vector of process equations
-    state_new = VState.block_matrix([[v_new], [sf.V1(yaw_new)]])
+    input_t = Values(
+        d_vel = d_vel - noise["d_vel"],
+        d_ang = d_ang - noise["d_ang"]
+    )
+
+    state_t_pred = Values(
+        vel = state_t["vel"] + state_t["R"] * input_t["d_vel"],
+        R = state_t["R"] * rot2_small_angle(input_t["d_ang"])
+    )
+
+    # Nominal state kinematics
+    state_pred = Values(
+        vel = state["vel"] + state["R"] * d_vel,
+        R = state["R"] * rot2_small_angle(d_ang)
+    )
+
+    # Error state kinematics
+    delta_rot = (state_pred["R"].inverse() * state_t_pred["R"])
+    state_error_pred = Values(
+        vel = state_t_pred["vel"] - state_pred["vel"],
+        yaw = sf.simplify(delta_rot.z.imag) # small angle appriximation; use simplify to cancel R.T*R
+    )
+
+    zero_state_error = {state_error[key]: state_error[key].zero() for key in state_error.keys()}
+    zero_noise = {noise[key]: noise[key].zero() for key in noise.keys()}

    # Calculate state transition matrix
-    F = state_new.jacobian(state)
+    F = VTangent(state_error_pred.to_storage()).jacobian(state_error).subs(zero_state_error).subs(zero_noise)

    # Derive the covariance prediction equations
    # Error growth in the inertial solution is assumed to be driven by 'noise' in the delta angles and
    # velocities, after bias effects have been removed.

-    # derive the control(disturbance) influence matrix from IMU noise to state noise
-    G = state_new.jacobian(sf.V3.block_matrix([[d_vel], [sf.V1(d_ang)]]))
+    # derive the control(disturbance) influence matrix from IMU noise to error-state noise
+    G = VTangent(state_error_pred.to_storage()).jacobian(noise).subs(zero_state_error).subs(zero_noise)

    # derive the state error matrix
    var_u = sf.Matrix.diag([d_vel_var, d_vel_var, d_ang_var])
@@ -93,15 +128,15 @@ def yaw_est_predict_covariance(
    # Generate the equations for the upper triangular matrix and the diagonal only
    # Since the matrix is symmetric, the lower triangle does not need to be derived
    # and can simply be copied in the implementation
-    for index in range(State.n_states):
-        for j in range(State.n_states):
+    for index in range(State.tangent_dim()):
+        for j in range(State.tangent_dim()):
            if index > j:
                P_new[index,j] = 0

    return P_new

 def yaw_est_compute_measurement_update(
-        P: MState,
+        P: MTangent,
        vel_obs_var: sf.Scalar,
        epsilon : sf.Scalar
 ):
@@ -123,8 +158,8 @@ def yaw_est_compute_measurement_update(
    # Generate the equations for the upper triangular matrix and the diagonal only
    # Since the matrix is symmetric, the lower triangle does not need to be derived
    # and can simply be copied in the implementation
-    for index in range(State.n_states):
-        for j in range(State.n_states):
+    for index in range(State.tangent_dim()):
+        for j in range(State.tangent_dim()):
            if index > j:
                P_new[index,j] = 0

@@ -20,6 +20,7 @@ namespace sym {
 *     P: Matrix33
 *     d_vel: Matrix21
 *     d_vel_var: Scalar
+ *     d_ang: Scalar
 *     d_ang_var: Scalar
 *
 * Outputs:
@@ -29,13 +30,13 @@ template <typename Scalar>
 void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
                             const matrix::Matrix<Scalar, 3, 3>& P,
                             const matrix::Matrix<Scalar, 2, 1>& d_vel, const Scalar d_vel_var,
-                             const Scalar d_ang_var,
+                             const Scalar d_ang, const Scalar d_ang_var,
                             matrix::Matrix<Scalar, 3, 3>* const P_new = nullptr) {
-  // Total ops: 33
+  // Total ops: 39

  // Input arrays

-  // Intermediate terms (7)
+  // Intermediate terms (8)
  const Scalar _tmp0 = std::cos(state(2, 0));
  const Scalar _tmp1 = std::sin(state(2, 0));
  const Scalar _tmp2 = -_tmp0 * d_vel(1, 0) - _tmp1 * d_vel(0, 0);
@@ -44,6 +45,7 @@ void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
      std::pow(_tmp0, Scalar(2)) * d_vel_var + std::pow(_tmp1, Scalar(2)) * d_vel_var;
  const Scalar _tmp5 = _tmp0 * d_vel(0, 0) - _tmp1 * d_vel(1, 0);
  const Scalar _tmp6 = P(1, 2) + P(2, 2) * _tmp5;
+  const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1;

  // Output terms (1)
  if (P_new != nullptr) {
@@ -55,9 +57,9 @@ void YawEstPredictCovariance(const matrix::Matrix<Scalar, 3, 1>& state,
    _p_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5;
    _p_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6;
    _p_new(2, 1) = 0;
-    _p_new(0, 2) = _tmp3;
-    _p_new(1, 2) = _tmp6;
-    _p_new(2, 2) = P(2, 2) + d_ang_var;
+    _p_new(0, 2) = _tmp3 * _tmp7;
+    _p_new(1, 2) = _tmp6 * _tmp7;
+    _p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var;
  }
 }  // NOLINT(readability/fn_size)

@@ -71,9 +71,7 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 				resetWindToZero();
 			}

-			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
-				fuseSideslip(_aid_src_sideslip);
-			}
+			fuseSideslip(_aid_src_sideslip);
 		}
 	}
 }
@@ -342,7 +342,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f);
 * @unit m/s^2/sqrt(Hz)
 * @decimal 3
 */
-PARAM_DEFINE_FLOAT(EKF2_WIND_NSD, 1.0e-2f);
+PARAM_DEFINE_FLOAT(EKF2_WIND_NSD, 5.0e-2f);

 /**
 * Measurement noise for gps horizontal velocity.
@@ -601,7 +601,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f);
 * @unit SD
 * @decimal 1
 */
-PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f);
+PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 5.0f);

 /**
 * Will be removed after v1.14 release
@@ -57,7 +57,6 @@ px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_senso
 px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_yaw_estimator_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_drag_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
@@ -1,230 +0,0 @@
-/****************************************************************************
- *
- *   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 <gtest/gtest.h>
-#include "EKF/ekf.h"
-#include "test_helper/comparison_helper.h"
-
-#include "../EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h"
-#include "../EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h"
-
-using namespace matrix;
-typedef SquareMatrix<float, 3> SquareMatrix3f;
-
-SquareMatrix3f createRandomCovarianceMatrix3f()
-{
-	// Create a symmetric square matrix
-	SquareMatrix3f P;
-
-	for (int col = 0; col <= 2; col++) {
-		for (int row = 0; row <= col; row++) {
-			if (row == col) {
-				P(row, col) = randf();
-
-			} else {
-				P(col, row) = P(row, col) = 2.0f * (randf() - 0.5f);
-			}
-		}
-	}
-
-	// Make it positive definite
-	P = P.transpose() * P;
-
-	return P;
-}
-
-void sympyYawEstUpdate(const SquareMatrix3f &P, float velObsVar, SquareMatrix<float, 2> &S_inverse,
-		       float &S_det_inverse, Matrix<float, 3, 2> &K, SquareMatrix3f &P_new)
-{
-	const float P00 = P(0, 0);
-	const float P01 = P(0, 1);
-	const float P02 = P(0, 2);
-	const float P11 = P(1, 1);
-	const float P12 = P(1, 2);
-	const float P22 = P(2, 2);
-
-	// optimized auto generated code from SymPy script src/lib/ecl/EKF/python/ekf_derivation/main.py
-	const float t0 = powf(P01, 2.f);
-	const float t1 = -t0;
-	const float t2 = P00 * P11 + P00 * velObsVar + P11 * velObsVar + t1 + powf(velObsVar, 2.f);
-
-	if (fabsf(t2) < 1e-6f) {
-		return;
-	}
-
-	const float t3 = 1.0F / t2;
-	const float t4 = P11 + velObsVar;
-	const float t5 = P01 * t3;
-	const float t6 = -t5;
-	const float t7 = P00 + velObsVar;
-	const float t8 = P00 * t4 + t1;
-	const float t9 = t5 * velObsVar;
-	const float t10 = P11 * t7;
-	const float t11 = t1 + t10;
-	const float t12 = P01 * P12;
-	const float t13 = P02 * t4;
-	const float t14 = P01 * P02;
-	const float t15 = P12 * t7;
-	const float t16 = t0 * velObsVar;
-	const float t17 = powf(t2, -2);
-	const float t18 = t4 * velObsVar + t8;
-	const float t19 = t17 * t18;
-	const float t20 = t17 * (t16 + t7 * t8);
-	const float t21 = t0 - t10;
-	const float t22 = t17 * t21;
-	const float t23 = t14 - t15;
-	const float t24 = P01 * t23;
-	const float t25 = t12 - t13;
-	const float t26 = t16 - t21 * t4;
-	const float t27 = t17 * t26;
-	const float t28 = t11 + t7 * velObsVar;
-	const float t30 = t17 * t28;
-	const float t31 = P01 * t25;
-	const float t32 = t23 * t4 + t31;
-	const float t33 = t17 * t32;
-	const float t35 = t24 + t25 * t7;
-	const float t36 = t17 * t35;
-
-	S_det_inverse = t3;
-
-	S_inverse(0, 0) = t3 * t4;
-	S_inverse(0, 1) = t6;
-	S_inverse(1, 1) = t3 * t7;
-	S_inverse(1, 0) = S_inverse(0, 1);
-
-	K(0, 0) = t3 * t8;
-	K(1, 0) = t9;
-	K(2, 0) = t3 * (-t12 + t13);
-	K(0, 1) = t9;
-	K(1, 1) = t11 * t3;
-	K(2, 1) = t3 * (-t14 + t15);
-
-	P_new(0, 0) = P00 - t16 * t19 - t20 * t8;
-	P_new(0, 1) = P01 * (t18 * t22 - t20 * velObsVar + 1);
-	P_new(1, 1) = P11 - t16 * t30 + t22 * t26;
-	P_new(0, 2) = P02 + t19 * t24 + t20 * t25;
-	P_new(1, 2) = P12 + t23 * t27 + t30 * t31;
-	P_new(2, 2) = P22 - t23 * t33 - t25 * t36;
-	P_new(1, 0) = P_new(0, 1);
-	P_new(2, 0) = P_new(0, 2);
-	P_new(2, 1) = P_new(1, 2);
-}
-
-void sympyYawEstPrediction(const Vector3f &state, const SquareMatrix3f &P, const Vector2f &d_vel, float d_vel_var,
-			   float d_ang_var, SquareMatrix3f &P_new)
-{
-	const float P00 = P(0, 0);
-	const float P01 = P(0, 1);
-	const float P02 = P(0, 2);
-	const float P11 = P(1, 1);
-	const float P12 = P(1, 2);
-	const float P22 = P(2, 2);
-	const float psi = state(2);
-	const float dvx = d_vel(0);
-	const float dvy = d_vel(1);
-	const float dvxVar = d_vel_var;
-	const float dvyVar = d_vel_var;
-	const float dazVar = d_ang_var;
-
-	const float S0 = cosf(psi);
-	const float S1 = powf(S0, 2.f);
-	const float S2 = sinf(psi);
-	const float S3 = powf(S2, 2.f);
-	const float S4 = S0 * dvy + S2 * dvx;
-	const float S5 = P02 - P22 * S4;
-	const float S6 = S0 * dvx - S2 * dvy;
-	const float S7 = S0 * S2;
-	const float S8 = P01 + S7 * dvxVar - S7 * dvyVar;
-	const float S9 = P12 + P22 * S6;
-
-	P_new(0, 0) = P00 - P02 * S4 + S1 * dvxVar + S3 * dvyVar - S4 * S5;
-	P_new(0, 1) = -P12 * S4 + S5 * S6 + S8;
-	P_new(1, 1) = P11 + P12 * S6 + S1 * dvyVar + S3 * dvxVar + S6 * S9;
-	P_new(0, 2) = S5;
-	P_new(1, 2) = S9;
-	P_new(2, 2) = P22 + dazVar;
-}
-
-TEST(YawEstimatorGenerated, SympyVsSymforceUpdate)
-{
-	const float R = sq(0.1f);
-
-	SquareMatrix<float, 3> P = createRandomCovarianceMatrix3f();
-
-	SquareMatrix<float, 2> innov_var_inv_sympy;
-	float innov_var_det_inv_sympy;
-	SquareMatrix3f P_new_sympy;
-	Matrix<float, 3, 2> K_sympy;
-	sympyYawEstUpdate(P, R, innov_var_inv_sympy, innov_var_det_inv_sympy, K_sympy, P_new_sympy);
-
-	SquareMatrix<float, 2> innov_var_inv_symforce;
-	float innov_var_det_inv_symforce;
-	SquareMatrix3f P_new_symforce;
-	Matrix<float, 3, 2> K_symforce;
-	sym::YawEstComputeMeasurementUpdate(P, R, FLT_EPSILON,
-					    &innov_var_inv_symforce,
-					    &innov_var_det_inv_symforce, &K_symforce, &P_new_symforce);
-	// copy upper to lower diagonal
-	P_new_symforce(1, 0) = P_new_symforce(0, 1);
-	P_new_symforce(2, 0) = P_new_symforce(0, 2);
-	P_new_symforce(2, 1) = P_new_symforce(1, 2);
-
-	EXPECT_FLOAT_EQ(innov_var_det_inv_sympy, innov_var_det_inv_symforce);
-	EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce));
-	EXPECT_TRUE(isEqual(K_sympy, K_symforce));
-	EXPECT_TRUE(isEqual(innov_var_inv_sympy, innov_var_inv_symforce));
-}
-
-TEST(YawEstimatorGenerated, SympyVsSymforcePrediction)
-{
-	const float dt = 0.01f;
-	const Vector2f d_vel = Vector2f(0.1f, -0.5f) * dt;
-	const float d_vel_var = sq(2.f * dt);
-	const float d_ang_var = sq(0.1f * dt);
-
-	const Vector3f state(3.f, 10.f, M_PI_F / 2.f);
-
-	SquareMatrix<float, 3> P = createRandomCovarianceMatrix3f();
-	SquareMatrix<float, 3> P_new_symforce;
-
-	sym::YawEstPredictCovariance(state, P, d_vel, d_vel_var, d_ang_var, &P_new_symforce);
-
-	EXPECT_GT(P_new_symforce(0, 0), P(0, 0));
-	EXPECT_GT(P_new_symforce(1, 1), P(1, 1));
-	EXPECT_GT(P_new_symforce(2, 2), P(2, 2));
-
-	SquareMatrix<float, 3> P_new_sympy;
-	sympyYawEstPrediction(state, P, d_vel, d_vel_var, d_ang_var, P_new_sympy);
-
-	EXPECT_TRUE(isEqual(P_new_sympy, P_new_symforce));
-}
@@ -137,9 +137,7 @@ 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)
-	    || ((_vehicle_status_sub.get().nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
-		&& (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))) {
+	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 		switchTask(FlightTaskIndex::None);
 		return;
 	}
@@ -55,7 +55,6 @@ 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");
@@ -555,15 +555,6 @@ 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;
 }
@@ -68,8 +68,7 @@ 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
-	    || command.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
+	if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
 		return 0;
 	}

@@ -1030,6 +1030,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	// Special case for gimbals that need to forward GIMBAL_DEVICE_ATTITUDE_STATUS.
 	else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS) {
 		Mavlink::forward_message(msg, this);
+
+	} else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION) {
+		Mavlink::forward_message(msg, this);
 	}
 }

@@ -1186,9 +1189,8 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 		return OK;
 	}

-	// if we reach here, the stream list does not contain the stream.
-	// 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)
+	/* 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
 	return PX4_OK;
 #else
 	PX4_WARN("stream %s not found", stream_name);
@@ -1400,11 +1402,9 @@ 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);
@@ -1472,11 +1472,9 @@ 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);
@@ -1548,11 +1546,9 @@ 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);
@@ -1632,11 +1628,9 @@ 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);
@@ -1729,10 +1723,8 @@ 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);
@@ -2365,8 +2357,7 @@ 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
-					    && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
+					    && is_target_known) {

 						mavlink_command_ack_t msg{};
 						msg.result = command_ack.result;
@@ -124,11 +124,6 @@
 #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"
@@ -139,6 +134,7 @@
 # include "streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp"
 # include "streams/GIMBAL_MANAGER_INFORMATION.hpp"
 # include "streams/GIMBAL_MANAGER_STATUS.hpp"
+# include "streams/GIMBAL_DEVICE_INFORMATION.hpp"
 # include "streams/GPS2_RAW.hpp"
 # include "streams/HIGH_LATENCY2.hpp"
 # include "streams/LINK_NODE_STATUS.hpp"
@@ -342,6 +338,9 @@ static const StreamListItem streams_list[] = {
 #if defined(GIMBAL_MANAGER_INFORMATION_HPP)
 	create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
 #endif // GIMBAL_MANAGER_INFORMATION_HPP
+#if defined(GIMBAL_DEVICE_INFORMATION_HPP)
+	create_stream_list_item<MavlinkStreamGimbalDeviceInformation>(),
+#endif // GIMBAL_DEVICE_INFORMATION_HPP
 #if defined(GIMBAL_MANAGER_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
 #endif // GIMBAL_MANAGER_STATUS_HPP
@@ -497,14 +496,8 @@ 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)
@@ -75,4 +75,6 @@ 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_ */
@@ -2431,10 +2431,10 @@ MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)

 	} else if (landing_target.position_valid) {
 		// We only support MAV_FRAME_LOCAL_NED. In this case, the frame was unsupported.
-		mavlink_log_critical(&_mavlink_log_pub, "landing target: coordinate frame %" PRIu8 " unsupported\t",
+		mavlink_log_critical(&_mavlink_log_pub, "Landing target: coordinate frame %" PRIu8 " unsupported\t",
 				     landing_target.frame);
 		events::send<uint8_t>(events::ID("mavlink_rcv_lnd_target_unsup_coord"), events::Log::Error,
-				      "landing target: unsupported coordinate frame {1}", landing_target.frame);
+				      "Landing target: unsupported coordinate frame {1}", landing_target.frame);

 	} else {
 		irlock_report_s irlock_report{};
@@ -1,239 +0,0 @@
-/****************************************************************************
- *
- *   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
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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 GIMBAL_DEVICE_INFORMATION_HPP
+#define GIMBAL_DEVICE_INFORMATION_HPP
+
+#include <uORB/topics/gimbal_device_information.h>
+
+class MavlinkStreamGimbalDeviceInformation : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalDeviceInformation(mavlink); }
+
+	static constexpr const char *get_name_static() { return "GIMBAL_DEVICE_INFORMATION"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		if (_gimbal_device_information_sub.advertised()) {
+			return MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		}
+
+		return 0;
+	}
+
+private:
+	explicit MavlinkStreamGimbalDeviceInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
+
+	bool send() override
+	{
+		gimbal_device_information_s gimbal_device_information;
+
+		if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) {
+			// send out gimbal_device_info with info from gimbal_device_information
+			mavlink_gimbal_device_information_t msg{};
+
+			msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
+			memcpy(msg.vendor_name, gimbal_device_information.vendor_name, sizeof(gimbal_device_information.vendor_name));
+			memcpy(msg.model_name, gimbal_device_information.model_name, sizeof(gimbal_device_information.model_name));
+			memcpy(msg.custom_name, gimbal_device_information.custom_name, sizeof(gimbal_device_information.custom_name));
+			msg.firmware_version = gimbal_device_information.firmware_version;
+			msg.hardware_version = gimbal_device_information.hardware_version;
+			msg.uid = gimbal_device_information.uid;
+			msg.cap_flags = gimbal_device_information.cap_flags;
+			msg.roll_min = gimbal_device_information.roll_min;
+			msg.roll_max = gimbal_device_information.roll_max;
+			msg.pitch_min = gimbal_device_information.pitch_min;
+			msg.pitch_max = gimbal_device_information.pitch_max;
+			msg.yaw_min = gimbal_device_information.yaw_min;
+			msg.yaw_max = gimbal_device_information.yaw_max;
+
+			mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // GIMBAL_DEVICE_INFORMATION_HPP
@@ -466,10 +466,11 @@ void MulticopterPositionControl::Run()
 				_vehicle_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
 			}

+			bool skip_takeoff = _param_com_throw_en.get();
 			// handle smooth takeoff
 			_takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed,
 						    _vehicle_constraints.want_takeoff,
-						    _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample);
+						    _vehicle_constraints.speed_up, skip_takeoff, vehicle_local_position.timestamp_sample);

 			const bool not_taken_off             = (_takeoff.getTakeoffState() < TakeoffState::rampup);
 			const bool flying                    = (_takeoff.getTakeoffState() >= TakeoffState::flight);
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
David Sidrane	03a32aa2ce	PX4 NuttX with Fat wrap fix Backport	2023-11-13 12:39:44 -08:00
Hamish Willee	50d9d05c10	Escape param metadata to fix broken tags	2023-11-14 09:48:03 +01:00
bresch	00568985c0	yaw_est: use error-state covariance prediction Convergence improvements in high yaw rate conditions	2023-11-13 11:37:27 -05:00
Konrad	32127ca789	loiter: set loiter position to current position setpoint, if it is a loiter item	2023-11-13 12:43:58 +01:00
Michał Barciś	6800a448b0	ThrowLaunch: check ThrowMode parameter to skip takoff in MulticopterPositionControl Signed-off-by: Michał Barciś <mbarcis@mbarcis.net>	2023-11-13 11:57:19 +01:00
Matthias Grob	9d455d5f1f	ThrowLaunch: move into separate class	2023-11-13 11:57:19 +01:00
Michał Barciś	1a48f311ea	ThrowLaunch changes after PR Signed-off-by: Michał Barciś <mbarcis@mbarcis.net>	2023-11-13 11:57:19 +01:00
Michał Barciś	c2b4f051f9	Added Throw Launch feature More details in the PR: https://github.com/PX4/PX4-Autopilot/pull/21170 Signed-off-by: Michał Barciś <mbarcis@mbarcis.net>	2023-11-13 11:57:19 +01:00
Julian Oes	84577ce2c2	battery: skip charge estimation if N cells is 0 This triggers the undefined behaviour fuzzer, so let's try to fix it. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-13 09:55:39 +01:00
Konrad	44d1003f8e	rtl: Only use approaches for vtols in fixed wing mode	2023-11-12 15:41:28 +01:00
murata	f29c5742be	mavlink: Capitalize the first letter of Landing	2023-11-12 15:33:51 +01:00
Damien SIX	833cd79e67	fix: remove depreciated parameter	2023-11-12 15:33:33 +01:00
Daniel Agar	1592aedc11	gitmodules add heatshrink branch	2023-11-10 17:45:47 -05:00
Julian Oes	10a2e7e44f	SITL: fix parse error on start This fixes the error: etc/init.d-posix/rcS: 39: [: Illegal number: Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-11 11:26:20 +13:00
Matthias Grob	4485c7aa11	PositionSmoothing: fix corner altitude bug During a round corner the L1 distance calculation was only done in 2D and the z-axis coordinate was directly coming from the next waypoint. This lead to an unpredictable altitude profile between two waypoints. Sometimes almost all altitude difference was already covered during the turn instead of going diagonally.	2023-11-10 10:02:20 +01:00
Julian Oes	19b681ca1f	Remove SYS_USE_IO param The param is not really required anymore with the actuator configuration. Also, when it is set to 0, RC doesn't work for some boards which would be nice to avoid. Signed-off-by: Julian Oes <julian@oes.ch>	2023-11-10 12:17:23 +13:00
Niklas Hauser	1463f9dec8	stm32h7: Prevent UART from waiting on TXDMA semaphore The UART7 TXDMA services TELEM1 with flow control. If CTS is high, the transmitting thread will wait on a semaphore, which may block other threads from acquiring necessary resources to make progress, for example, preventing MAVLINK instances from transmitting. This change in NuttX makes the TXDMA acquire the semaphore in a non-blocking way, preventing this issue.	2023-11-09 07:53:48 -05:00
Frederik Markus	fc5b868138	fixing the turning direction of the Cessna Propeller (#22276 ) Signed-off-by: frederik <frederik@auterion.com> Co-authored-by: frederik <frederik@auterion.com>	2023-11-09 13:08:54 +01:00
Silvan Fuhrer	5efcde7412	mavsdk_tests: add two new tests for VTOLs - add functionality to specify world name for simulation in case name - add test for triggering an airspeed invalidation in case of pitot blockage - add test for high wind (ramped up wind over short period) to NOT invalidate airspeed in this case Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	29714a0fba	AirspeedValidator: make wind estimate more dynamic Increase wind process noise default (ASPD_WIND_NSD) and gate (ASPD_TAS_GATE) to be able to catch rapid wind increases with the internal wind estimator of the airspeed validator. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	041e5069cb	ROMFS: update tuning of SITL standard VTOL Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	0d36d54fbe	SimulatorMavlink: add airspeed icing failure (ramp down scale) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	a4b04c3982	SimulatorMavlink: rename _airspeed_blocked to _airspeed_disconnected Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	cf03b99ab4	AirspeedValidator: inrease default of ASPD_BETA_NOISE Trust the beta innovations more compated to the TAS innovations. That should help with detecting real airspeed failures even with a dynamic wind estimate (as long as vehicle doesn't fly straight) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	0a271b1982	EKF2: increase default of EKF2_WIND_NSD and EKF2_TAS_GATE Increasing the wind process noise results in a more dynamic wind estimation, which is capable of catching fast-varying winds. As wind is used in the lateral guidance it's important that we don't filter it too much. Furher the gate of the airspeed fusion is increased, to reduce the likelihood of airspeed fusion stopping due to dynamic wind conditions. The airspeed is validated in the airspeed validator (EKF consumes the validated one). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Silvan Fuhrer	ec20f92558	EKF2: sideslip_fusion: remove construced airspeed magnitude check to fuse beta Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-11-09 09:42:43 +01:00
Konrad	72c6db6b23	vtol_rtl_test: explicitly disable RTL_FORCE_APPROACH for RTL direct Home testcase	2023-11-08 17:48:34 +01:00
Claudio Micheli	457d261278	Add mavlink support for GIMBAL_DEVICE_INFORMATION Signed-off-by: Claudio Micheli <claudio@auterion.com>	2023-11-08 17:01:32 +01:00