ekf2-ev: filter q_error for frame correction

Signed-off-by: Claudio Micheli <claudio@auterion.com>

ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane

@@ -5,9 +5,6 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
-
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12

ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam

@@ -5,10 +5,8 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
 
-param set-default FW_LND_AIRSPD_SC 1
+
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12

ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult

@@ -5,10 +5,9 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
+param set-default FW_LAUN_DETCN_ON 1
+param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming
 
-param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12
@@ -47,8 +46,6 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
 
-param set-default RWTO_TKOFF 1
-
 param set-default CA_AIRFRAME 1
 
 param set-default CA_ROTOR_COUNT 1
@@ -74,5 +71,3 @@ param set-default PWM_MAIN_FUNC8 203
 param set-default PWM_MAIN_FUNC9 206
 param set-default PWM_MAIN_REV 256
 
-param set-default RWTO_TKOFF 0
-

ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod

@@ -5,8 +5,7 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
+
 
 param set-default FW_LND_ANG 8
 

ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer

@@ -5,8 +5,7 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
+
 
 param set-default FW_LND_ANG 8
 

ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider

@@ -5,10 +5,8 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
 
-param set-default FW_LND_AIRSPD_SC 1
+
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12
@@ -47,8 +45,6 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
 
-param set-default RWTO_TKOFF 1
-
 param set-default CA_AIRFRAME 1
 
 param set-default CA_ROTOR_COUNT 1
@@ -75,4 +71,3 @@ param set-default PWM_MAIN_FUNC9 206
 param set-default PWM_MAIN_REV 256
 
 param set-default FW_THR_TRIM 0.0
-param set-default RWTO_TKOFF 0

ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal

@@ -5,7 +5,6 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20

ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane

@@ -5,8 +5,7 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
+
 
 param set-default FW_LND_ANG 8
 param set-default FW_THR_LND_MAX 0

ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar

@@ -5,10 +5,8 @@
 
 . ${R}etc/init.d/rc.fw_defaults
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
 
-param set-default FW_LND_AIRSPD_SC 1
+
 param set-default FW_LND_ANG 8
 
 param set-default NPFG_PERIOD 12

ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2

@@ -26,7 +26,6 @@ param set-default NAV_ACC_RAD 20
 param set-default NAV_DLL_ACT 2
 param set-default NAV_LOITER_RAD 50
 
-param set-default RWTO_TKOFF 0
 # Parameters related to autogyro takeoff PR
 #param set-default AG_TKOFF 1
 #param set-default AG_PROT_TYPE 1

ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna

@@ -17,8 +17,7 @@ param set-default SENS_EN_BAROSIM 0
 param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
-param set-default EKF2_MAG_ACCLIM 0
-param set-default EKF2_MAG_YAWLIM 0
+
 
 param set-default FW_LND_ANG 8
 

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -284,7 +284,14 @@ then
 else
 	param set UXRCE_DDS_DOM_ID 0
 fi
-uxrce_dds_client start -t udp -h 127.0.0.1 -p 8888 $uxrce_dds_ns
+uxrce_dds_port=8888
+if [ -n "$PX4_UXRCE_DDS_PORT" ]
+then
+	# Override port if environment variable is defined
+	uxrce_dds_port="$PX4_UXRCE_DDS_PORT"
+fi
+
+uxrce_dds_client start -t udp -h 127.0.0.1 -p $uxrce_dds_port $uxrce_dds_ns
 
 if param greater -s MNT_MODE_IN -1
 then

Tools/px4events/srcparser.py

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

boards/px4/fmu-v5x/default.px4board

@@ -45,6 +45,7 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
+CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
@@ -108,3 +109,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y

boards/px4/fmu-v5x/init/rc.board_mavlink

@@ -7,4 +7,7 @@ if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X
 then
 	# Start MAVLink on the UART connected to the mission computer
 	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
+	mavlink stream -d /dev/ttyS4 -s ATTITUDE_QUATERNION -r 30
+	mavlink stream -d /dev/ttyS4 -s HIGHRES_IMU -r 20
+
 fi

msg/ArmingCheckReply.msg

@@ -0,0 +1,33 @@
+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
+
+
+uint8 ORB_QUEUE_LENGTH = 4
+

msg/ArmingCheckRequest.msg

@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+# broadcast message to request all registered arming checks to be reported
+
+uint8 request_id
+
+

msg/CMakeLists.txt

@@ -49,6 +49,8 @@ set(msg_files
 	Airspeed.msg
 	AirspeedValidated.msg
 	AirspeedWind.msg
+	ArmingCheckReply.msg
+	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
 	ButtonEvent.msg
@@ -58,6 +60,7 @@ set(msg_files
 	CellularStatus.msg
 	CollisionConstraints.msg
 	CollisionReport.msg
+	ConfigOverrides.msg
 	ControlAllocatorStatus.msg
 	Cpuload.msg
 	DebugArray.msg
@@ -154,6 +157,8 @@ set(msg_files
 	RateCtrlStatus.msg
 	RcChannels.msg
 	RcParameterMap.msg
+	RegisterExtComponentReply.msg
+	RegisterExtComponentRequest.msg
 	Rpm.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -190,6 +195,7 @@ set(msg_files
 	UavcanParameterValue.msg
 	UlogStream.msg
 	UlogStreamAck.msg
+	UnregisterExtComponent.msg
 	UwbDistance.msg
 	UwbGrid.msg
 	VehicleAcceleration.msg

msg/ConfigOverrides.msg

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

msg/RegisterExtComponentReply.msg

@@ -0,0 +1,12 @@
+uint64 timestamp # time since system start (microseconds)
+
+uint64 request_id          # ID from the request
+char[25] name              # name from the request
+
+bool success
+int8 arming_check_id      # arming check registration ID (-1 if invalid)
+int8 mode_id              # assigned mode ID (-1 if invalid)
+int8 mode_executor_id     # assigned mode executor ID (-1 if invalid)
+
+uint8 ORB_QUEUE_LENGTH = 2
+

msg/RegisterExtComponentRequest.msg

@@ -0,0 +1,17 @@
+# 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
+
+# Components to be registered
+bool register_arming_check
+bool register_mode                 # registering a mode also requires arming_check to be set
+bool register_mode_executor        # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
+
+bool enable_replace_internal_mode  # set to true if an internal mode should be replaced
+uint8 replace_internal_mode        # vehicle_status::NAVIGATION_STATE_*
+bool activate_mode_immediately     # switch to the registered mode (can only be set in combination with an executor)
+
+
+uint8 ORB_QUEUE_LENGTH = 2

msg/UnregisterExtComponent.msg

@@ -0,0 +1,10 @@
+uint64 timestamp # time since system start (microseconds)
+
+char[25] name                      # either the mode name, or component name
+
+int8 arming_check_id      # arming check registration ID (-1 if not registered)
+int8 mode_id              # assigned mode ID (-1 if not registered)
+int8 mode_executor_id     # assigned mode executor ID (-1 if not registered)
+
+
+

msg/VehicleCommand.msg

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

msg/VehicleCommandAck.msg

@@ -28,6 +28,6 @@ uint8 result						# Command result
 uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
 int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
-uint8 target_component
+uint16 target_component 				# Target component / mode executor
 
 bool from_external					# Indicates if the command came from an external source

msg/VehicleControlMode.msg

@@ -14,3 +14,8 @@ bool flag_control_position_enabled		# true if position is controlled
 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
+
+# TODO: use dedicated topic for external requests
+uint8 source_id                  # Mode ID (nav_state)
+
+# TOPICS vehicle_control_mode config_control_setpoints

msg/VehicleOdometry.msg

@@ -27,5 +27,5 @@ float32[3] velocity_variance
 uint8 reset_counter
 int8 quality
 
-# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry
+# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry vehicle_fake_visual_odometry
 # TOPICS estimator_odometry

msg/VehicleStatus.msg

@@ -57,7 +57,20 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19  # Auto Follow
 uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20       # Precision land with landing target
 uint8 NAVIGATION_STATE_ORBIT = 21               # Orbit in a circle
 uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
+uint8 NAVIGATION_STATE_EXTERNAL1 = 23
+uint8 NAVIGATION_STATE_EXTERNAL2 = 24
+uint8 NAVIGATION_STATE_EXTERNAL3 = 25
+uint8 NAVIGATION_STATE_EXTERNAL4 = 26
+uint8 NAVIGATION_STATE_EXTERNAL5 = 27
+uint8 NAVIGATION_STATE_EXTERNAL6 = 28
+uint8 NAVIGATION_STATE_EXTERNAL7 = 29
+uint8 NAVIGATION_STATE_EXTERNAL8 = 30
+uint8 NAVIGATION_STATE_MAX = 31
+
+uint8 executor_in_charge                        # Current mode executor in charge (0=Autopilot)
+
+uint32 valid_nav_states_mask                    # Bitmask for all valid nav_state values
+uint32 can_set_nav_states_mask                  # Bitmask for all modes that a user can select
 
 # Bitmask of detected failures
 uint16 failure_detector_status
@@ -83,8 +96,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
 uint8 VEHICLE_TYPE_AIRSHIP = 4
 
+uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
+uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
+uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
+
 bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
 bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
+uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
 
 # Link loss
 bool gcs_connection_lost              # datalink to GCS lost

src/lib/conversion/rotation.h

@@ -91,7 +91,9 @@ enum Rotation : uint8_t {
 	ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
 	ROTATION_PITCH_315                = 39,
 	ROTATION_ROLL_90_PITCH_315        = 40,
-
+	RESERVED_0                        = 41,
+	RESERVED_1                        = 42,
+	ROTATION_PITCH_90_YAW_45          = 43,
 	ROTATION_MAX
 };
 
@@ -143,6 +145,9 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
 	{ 90,  68, 293 },
 	{  0, 315,   0 },
 	{ 90, 315,   0 },
+	{  0,   0,   0 },
+	{  0,   0,   0 },
+	{  0,  90,  45 },
 };
 
 /**

src/lib/events/enums.json

@@ -110,6 +110,38 @@
                         "4194304": {
                             "name": "vtol_takeoff",
                             "description": "VTOL Takeoff"
+                        },
+                        "8388608": {
+                            "name": "external1",
+                            "description": "External 1"
+                        },
+                        "16777216": {
+                            "name": "external2",
+                            "description": "External 2"
+                        },
+                        "33554432": {
+                            "name": "external3",
+                            "description": "External 3"
+                        },
+                        "67108864": {
+                            "name": "external4",
+                            "description": "External 4"
+                        },
+                        "134217728": {
+                            "name": "external5",
+                            "description": "External 5"
+                        },
+                        "268435456": {
+                            "name": "external6",
+                            "description": "External 6"
+                        },
+                        "536870912": {
+                            "name": "external7",
+                            "description": "External 7"
+                        },
+                        "1073741824": {
+                            "name": "external8",
+                            "description": "External 8"
                         }
                     }
                 },
@@ -528,6 +560,38 @@
                             "name": "auto_vtol_takeoff",
                             "description": "Vtol Takeoff"
                         },
+                        "16": {
+                            "name": "external1",
+                            "description": "External 1"
+                        },
+                        "17": {
+                            "name": "external2",
+                            "description": "External 2"
+                        },
+                        "18": {
+                            "name": "external3",
+                            "description": "External 3"
+                        },
+                        "19": {
+                            "name": "external4",
+                            "description": "External 4"
+                        },
+                        "20": {
+                            "name": "external5",
+                            "description": "External 5"
+                        },
+                        "21": {
+                            "name": "external6",
+                            "description": "External 6"
+                        },
+                        "22": {
+                            "name": "external7",
+                            "description": "External 7"
+                        },
+                        "23": {
+                            "name": "external8",
+                            "description": "External 8"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"
@@ -701,7 +765,15 @@
                     "19": [134479872],
                     "20": [151257088],
                     "21": [16973824],
-                    "22": [168034304]
+                    "22": [168034304],
+                    "23": [184811520],
+                    "24": [201588736],
+                    "25": [218365952],
+                    "26": [235143168],
+                    "27": [251920384],
+                    "28": [268697600],
+                    "29": [285474816],
+                    "30": [302252032]
                 }
             },
             "supported_protocols": [ "health_and_arming_check" ]

src/lib/tecs/TECS.cpp

@@ -211,6 +211,7 @@ void TECSAltitudeReferenceModel::initialize(const AltitudeReferenceState &state)
 	const float init_state_alt_rate = PX4_ISFINITE(state.alt_rate) ? state.alt_rate : 0.f;
 
 	_alt_control_traj_generator.reset(0.0f, init_state_alt_rate, init_state_alt);
+	_velocity_control_traj_generator.reset(0.f, init_state_alt_rate, init_state_alt);
 }
 
 void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag)

src/modules/commander/CMakeLists.txt

@@ -52,6 +52,7 @@ px4_add_module(
 		factory_calibration_storage.cpp
 		gyro_calibration.cpp
 		HomePosition.cpp
+		ModeManagement.cpp
 		UserModeIntention.cpp
 		level_calibration.cpp
 		lm_fit.cpp

src/modules/commander/Commander.cpp

@@ -48,6 +48,8 @@
 #include "px4_custom_mode.h"
 #include "ModeUtil/control_mode.hpp"
 #include "ModeUtil/conversions.hpp"
+#include "ModeUtil/ui.hpp"
+#include "ModeUtil/standard_modes.hpp"
 
 /* PX4 headers */
 #include <drivers/drv_hrt.h>
@@ -407,6 +409,10 @@ int Commander::custom_command(int argc, char *argv[])
 				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
 						     PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
 
+			} else if (!strcmp(argv[1], "ext1")) {
+				send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
+						     PX4_CUSTOM_SUB_MODE_EXTERNAL1);
+
 			} else {
 				PX4_ERR("argument %s unsupported.", argv[1]);
 			}
@@ -475,8 +481,9 @@ int Commander::print_status()
 {
 	PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
 	PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
-	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
+	PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
 	PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
+	_mode_management.printStatus();
 	perf_print_counter(_loop_perf);
 	perf_print_counter(_preflight_check_perf);
 	return 0;
@@ -698,7 +705,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			// Check if a mode switch had been requested
 			if ((((uint32_t)cmd.param2) & 1) > 0) {
-				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+				if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
 					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
@@ -781,6 +788,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 							break;
 
+						case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
+							desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
+							break;
+
 						default:
 							main_ret = TRANSITION_DENIED;
 							mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
@@ -822,7 +833,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}
 
 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-				if (_user_mode_intention.change(desired_nav_state)) {
+				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;
 
 				} else {
@@ -843,6 +854,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
+	case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS
+			uint8_t desired_nav_state = (uint8_t)(cmd.param1 + 0.5f);
+
+			if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
 
 			// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
@@ -974,7 +997,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
 			/* switch to RTL which ends the mission */
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
 				events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -988,7 +1011,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
 			/* ok, home set, use it to take off */
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
@@ -1001,7 +1024,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
 
 		/* ok, home set, use it to take off */
-		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) {
+		if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) {
 			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else {
@@ -1012,7 +1035,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
@@ -1026,7 +1049,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) {
 				mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
 				events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
 					     "Landing using precision landing");
@@ -1050,7 +1073,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
 
 					// switch to AUTO_MISSION and ARM
-					if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
+					if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd))
 					    && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
 
 						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -1089,7 +1112,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 		} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
 			// for fixed wings the behavior of orbit is the same as loiter
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
 				main_ret = TRANSITION_CHANGED;
 
 			} else {
@@ -1098,7 +1121,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 		} else {
 			// Switch to orbit state and let the orbit task handle the command further
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) {
 				main_ret = TRANSITION_CHANGED;
 
 			} else {
@@ -1359,6 +1382,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			break;
 		}
 
+	case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
+			mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
+			uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode);
+
+			if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
+
+			} else {
+				if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
+
+				} else {
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				}
+			}
+		}
+		break;
+
 	case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
 		_health_and_arming_checks.update(true);
 		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@@ -1422,6 +1463,43 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	return true;
 }
 
+ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd)
+{
+	return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor :
+	       ModeChangeSource::User;
+}
+
+void Commander::handleCommandsFromModeExecutors()
+{
+	if (_vehicle_command_mode_executor_sub.updated()) {
+		const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation();
+		vehicle_command_s cmd;
+
+		if (_vehicle_command_mode_executor_sub.copy(&cmd)) {
+			if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) {
+				PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation,
+					_vehicle_command_mode_executor_sub.get_last_generation());
+			}
+
+			// For commands from mode executors, we check if it is in charge and then publish it on the official
+			// command topic
+			const int mode_executor_in_charge = _mode_management.modeExecutorInCharge();
+
+			// source_system is set to the mode executor
+			if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) {
+				cmd.source_system = _vehicle_status.system_id;
+				cmd.timestamp = hrt_absolute_time();
+				_vehicle_command_pub.publish(cmd);
+
+			} else {
+				cmd.source_system = _vehicle_status.system_id;
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge);
+			}
+		}
+	}
+}
+
 unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
 {
 	if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
@@ -1547,7 +1625,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 	case action_request_s::ACTION_SWITCH_MODE:
 
-		if (!_user_mode_intention.change(action_request.mode, true)) {
+		if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) {
 			printRejectMode(action_request.mode);
 		}
 
@@ -1703,6 +1781,8 @@ void Commander::run()
 			_status_changed = true;
 		}
 
+		modeManagementUpdate();
+
 		const hrt_abstime now = hrt_absolute_time();
 
 		const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
@@ -1725,6 +1805,8 @@ void Commander::run()
 		}
 
 		// handle commands last, as the system needs to be updated to handle them
+		handleCommandsFromModeExecutors();
+
 		if (_vehicle_command_sub.updated()) {
 			// got command
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@@ -1795,6 +1877,8 @@ 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.executor_in_charge = _mode_management.modeExecutorInCharge();
 			_vehicle_status.arming_state = _arm_state_machine.getArmState();
 			_vehicle_status.timestamp = hrt_absolute_time();
 			_vehicle_status_pub.publish(_vehicle_status);
@@ -1879,7 +1963,8 @@ void Commander::checkForMissionUpdate()
 
 		if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
 		    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
-		    && mission_result.finished) {
+		    && mission_result.finished
+		    && _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) {
 
 			if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
 			    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
@@ -2126,8 +2211,10 @@ void Commander::handleAutoDisarm()
 
 			const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
 							 && !_mission_result_sub.get().finished;
+			const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
+							      && !_config_overrides.disable_auto_disarm;
 
-			if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
+			if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
 				_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
 				_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
 
@@ -2176,6 +2263,7 @@ bool Commander::handleModeIntentionAndFailsafe()
 {
 	const uint8_t prev_nav_state = _vehicle_status.nav_state;
 	const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
+	const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
 
 	FailsafeBase::State state{};
 	state.armed = _arm_state_machine.isArmed();
@@ -2195,13 +2283,15 @@ bool Commander::handleModeIntentionAndFailsafe()
 
 	// Force intended mode if changed by the failsafe state machine
 	if (state.user_intended_mode != updated_user_intented_mode) {
-		_user_mode_intention.change(updated_user_intented_mode, false, true);
+		_user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true);
 		_user_mode_intention.getHadModeChangeAndClear();
 	}
 
 	// Handle failsafe action
-	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
-	_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get());
+	_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
+			false);
+	_vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(
+					    _failsafe.selectedAction(), _user_mode_intention.get()));
 
 	switch (_failsafe.selectedAction()) {
 	case FailsafeBase::Action::Disarm:
@@ -2230,7 +2320,24 @@ bool Commander::handleModeIntentionAndFailsafe()
 		_vehicle_status.nav_state_timestamp = hrt_absolute_time();
 	}
 
-	return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
+	_mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides);
+
+	// Apply failsafe deferring & get the current state
+	_failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s);
+
+	if (_failsafe.failsafeDeferred()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE;
+
+	} else if (_failsafe.getDeferFailsafes()) {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED;
+
+	} else {
+		_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED;
+	}
+
+	return prev_nav_state != _vehicle_status.nav_state ||
+	       prev_failsafe_action != _failsafe.selectedAction() ||
+	       prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state;
 }
 
 void Commander::checkAndInformReadyForTakeoff()
@@ -2250,6 +2357,21 @@ void Commander::checkAndInformReadyForTakeoff()
 #endif // CONFIG_ARCH_BOARD_PX4_SITL
 }
 
+void Commander::modeManagementUpdate()
+{
+	ModeManagement::UpdateRequest mode_management_update{};
+	_mode_management.update(_arm_state_machine.isArmed(), _vehicle_status.nav_state_user_intention,
+				_failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update);
+
+	if (!_arm_state_machine.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()) {
@@ -2404,8 +2526,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
 void Commander::updateControlMode()
 {
 	_vehicle_control_mode = {};
-	mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
+
+	mode_util::getVehicleControlMode(_vehicle_status.nav_state,
 					 _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
+	_mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode);
+
+	_vehicle_control_mode.flag_armed = _arm_state_machine.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);
 }
@@ -2755,7 +2888,7 @@ void Commander::manualControlCheck()
 				if (override_enabled) {
 					// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
 					if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
-						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) {
+						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
 							tune_positive(true);
 							mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
 							events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
@@ -2772,7 +2905,7 @@ void Commander::manualControlCheck()
 
 			// if there's never been a mode change force position control as initial state
 			if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) {
-				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true);
+				_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true);
 			}
 		}
 	}
@@ -2835,7 +2968,7 @@ The commander module contains the state machine for mode switching and failsafe
 	PRINT_MODULE_USAGE_COMMAND("land");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
-	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
+	PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
 			"Flight mode", false);
 	PRINT_MODULE_USAGE_COMMAND("pair");
 	PRINT_MODULE_USAGE_COMMAND("lockdown");

src/modules/commander/Commander.hpp

@@ -41,6 +41,7 @@
 #include "worker_thread.hpp"
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include "HomePosition.hpp"
+#include "ModeManagement.hpp"
 #include "UserModeIntention.hpp"
 
 #include <lib/controllib/blocks.hpp>
@@ -116,6 +117,8 @@ public:
 	void enable_hil();
 
 private:
+	static ModeChangeSource getSourceFromCommand(const vehicle_command_s &cmd);
+
 	void answer_command(const vehicle_command_s &cmd, uint8_t result);
 
 	transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
@@ -183,6 +186,10 @@ private:
 
 	void checkAndInformReadyForTakeoff();
 
+	void handleCommandsFromModeExecutors();
+
+	void modeManagementUpdate();
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -206,11 +213,17 @@ private:
 	FailureDetector		_failure_detector{this};
 	HealthAndArmingChecks	_health_and_arming_checks{this, _vehicle_status};
 	Safety			_safety{};
-	UserModeIntention	_user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
 	WorkerThread 		_worker_thread{};
+	ModeManagement  	_mode_management{
+#ifndef CONSTRAINED_FLASH
+		_health_and_arming_checks.externalChecks()
+#endif
+	};
+	UserModeIntention	_user_mode_intention {this, _vehicle_status, _health_and_arming_checks, &_mode_management};
 
 	const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
 	HomePosition 		_home_position{_failsafe_flags};
+	config_overrides_s   _config_overrides{};
 
 
 	Hysteresis _auto_disarm_landed{false};
@@ -275,6 +288,7 @@ private:
 	uORB::Subscription					_manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription					_system_power_sub{ORB_ID(system_power)};
 	uORB::Subscription					_vehicle_command_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription					_vehicle_command_mode_executor_sub{ORB_ID(vehicle_command_mode_executor)};
 	uORB::Subscription					_vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription					_vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
@@ -294,6 +308,7 @@ private:
 	uORB::Publication<actuator_test_s>			_actuator_test_pub{ORB_ID(actuator_test)};
 	uORB::Publication<failure_detector_status_s>		_failure_detector_status_pub{ORB_ID(failure_detector_status)};
 	uORB::Publication<vehicle_command_ack_s>		_vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_s>			_vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_control_mode_s>		_vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
 	uORB::Publication<vehicle_status_s>			_vehicle_status_pub{ORB_ID(vehicle_status)};
 

src/modules/commander/HealthAndArmingChecks/CMakeLists.txt

@@ -61,6 +61,7 @@ px4_add_library(health_and_arming_checks
 	checks/rcAndDataLinkCheck.cpp
 	checks/vtolCheck.cpp
 	checks/offboardCheck.cpp
+	checks/externalChecks.cpp
 )
 add_dependencies(health_and_arming_checks mode_util)
 

src/modules/commander/HealthAndArmingChecks/Common.cpp

@@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve
 	return header;
 }
 
+bool Report::addExternalEvent(const event_s &event, NavModes modes)
+{
+	unsigned args_size = sizeof(event.arguments);
+
+	// trim 0's
+	while (args_size > 0 && event.arguments[args_size - 1] == '\0') {
+		--args_size;
+	}
+
+	unsigned total_size = sizeof(EventBufferHeader) + args_size;
+
+	if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
+		_buffer_overflowed = true;
+		return false;
+	}
+
+	events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))};
+	memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size);
+	addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size);
+	return true;
+}
+
+
+
 NavModes Report::reportedModes(NavModes required_modes)
 {
 	// Make sure optional checks are still shown in the UI

src/modules/commander/HealthAndArmingChecks/Common.hpp

@@ -249,8 +249,8 @@ public:
 	void clearArmingBits(NavModes modes);
 
 	/**
-	 * Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
-	 * mode is being run.
+	 * Clear can_run bits for certain modes. This will prevent mode switching.
+	 * For failsafe use the mode requirements instead, which then will clear the can_run bits.
 	 * @param modes affected modes
 	 */
 	void clearCanRunBits(NavModes modes);
@@ -259,6 +259,8 @@ public:
 	const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
 
 	bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
+
+	bool addExternalEvent(const event_s &event, NavModes modes);
 private:
 
 	/**
@@ -307,6 +309,7 @@ private:
 	NavModes getModeGroup(uint8_t nav_state) const;
 
 	friend class HealthAndArmingChecks;
+	friend class ExternalChecks;
 	FRIEND_TEST(ReporterTest, basic_no_checks);
 	FRIEND_TEST(ReporterTest, basic_fail_all_modes);
 	FRIEND_TEST(ReporterTest, arming_checks_mode_category);

src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp

@@ -67,6 +67,7 @@
 #include "checks/rcAndDataLinkCheck.hpp"
 #include "checks/vtolCheck.hpp"
 #include "checks/offboardCheck.hpp"
+#include "checks/externalChecks.hpp"
 
 class HealthAndArmingChecks : public ModuleParams
 {
@@ -99,6 +100,10 @@ public:
 
 	const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
 
+#ifndef CONSTRAINED_FLASH
+	ExternalChecks &externalChecks() { return _external_checks; }
+#endif
+
 protected:
 	void updateParams() override;
 private:
@@ -139,8 +144,14 @@ private:
 	RcAndDataLinkChecks _rc_and_data_link_checks;
 	VtolChecks _vtol_checks;
 	OffboardChecks _offboard_checks;
+#ifndef CONSTRAINED_FLASH
+	ExternalChecks _external_checks;
+#endif
 
-	HealthAndArmingCheckBase *_checks[30] = {
+	HealthAndArmingCheckBase *_checks[40] = {
+#ifndef CONSTRAINED_FLASH
+		&_external_checks,
+#endif
 		&_accelerometer_checks,
 		&_airspeed_checks,
 		&_baro_checks,
@@ -156,7 +167,7 @@ private:
 		&_home_position_checks,
 		&_mission_checks,
 		&_offboard_checks, // must be after _estimator_checks
-		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
+		&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks, _external_checks
 		&_parachute_checks,
 		&_power_checks,
 		&_rc_calibration_checks,

src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp

@@ -0,0 +1,333 @@
+/****************************************************************************
+ *
+ *   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);
+			}
+
+			if (!reply.can_arm_and_run) {
+				reporter.clearArmingBits(modes);
+			}
+
+			if (reply.health_component_index > 0) {
+				reporter.setHealth((health_component_t)(1ull << reply.health_component_index),
+						   reply.health_component_is_present, reply.health_component_warning,
+						   reply.health_component_error);
+			}
+
+			for (int i = 0; i < reply.num_events; ++i) {
+				// set the modes, which is the first argument
+				memcpy(reply.events[i].arguments, &modes, sizeof(modes));
+
+				reporter.addExternalEvent(reply.events[i], modes);
+			}
+		}
+	}
+
+	if (unresponsive_modes != NavModes::None) {
+		/* EVENT
+		 * @description
+		 * The application running the mode might have crashed or the CPU load is too high.
+		 */
+		reporter.armingCheckFailure(unresponsive_modes, health_component_t::system,
+					    events::ID("check_external_modes_unresponsive"),
+					    events::Log::Critical, "Mode is unresponsive");
+	}
+
+}
+
+void ExternalChecks::update()
+{
+	if (_active_registrations_mask == 0) {
+		return;
+	}
+
+	const hrt_abstime now = hrt_absolute_time();
+
+	// Check for incoming replies
+	arming_check_reply_s reply;
+	int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;
+
+	while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
+		if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
+		    && _current_request_id == reply.request_id) {
+			_reply_received_mask |= 1u << reply.registration_id;
+			_registrations[reply.registration_id].num_no_response = 0;
+
+			// Prevent toggling between unresponsive & responsive state
+			if (_registrations[reply.registration_id].total_num_unresponsive <= 3) {
+				_registrations[reply.registration_id].unresponsive = false;
+			}
+
+			if (_registrations[reply.registration_id].reply) {
+				*_registrations[reply.registration_id].reply = reply;
+			}
+
+//			PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events);
+		}
+	}
+
+	if (_last_update > 0) {
+		if (_reply_received_mask == _active_registrations_mask) { // Got all responses
+			// Nothing to do
+		} else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout
+			_had_timeout = true;
+			unsigned no_reply = _active_registrations_mask & ~_reply_received_mask;
+
+			for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
+				if ((1u << i) & no_reply) {
+					if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) {
+						// Clear immediately if not a mode
+						if (_registrations[i].nav_mode_id == -1) {
+							removeRegistration(i, -1);
+							PX4_WARN("No response from %i, removing", i);
+
+						} else {
+							_registrations[i].unresponsive = true;
+
+							if (_registrations[i].total_num_unresponsive < 100) {
+								++_registrations[i].total_num_unresponsive;
+							}
+
+							PX4_WARN("No response from %i, flagging unresponsive", i);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// Start a new request?
+	if (now > _last_update + UPDATE_INTERVAL) {
+		_reply_received_mask = 0;
+		_last_update = now;
+		_had_timeout = false;
+
+		// Request the state from all registered components
+		arming_check_request_s request{};
+		request.request_id = ++_current_request_id;
+		request.timestamp = hrt_absolute_time();
+		_arming_check_request_pub.publish(request);
+	}
+}
+
+void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state)
+{
+	_first_external_nav_state = first_external_nav_state;
+	_last_external_nav_state = last_external_nav_state;
+}
+
+void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const
+{
+	// Clear the arming bits for all non-registered external modes.
+	// But only report if one of them is selected, so we don't need to generate the extra event in most cases.
+	bool report_mode_not_available = false;
+
+	for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state;
+	     ++external_nav_state) {
+		bool found = false;
+
+		for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
+			if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			if (external_nav_state == context.status().nav_state_user_intention) {
+				report_mode_not_available = true;
+			}
+
+			reporter.clearArmingBits(reporter.getModeGroup(external_nav_state));
+			setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other);
+		}
+	}
+
+	if (report_mode_not_available) {
+		/* EVENT
+		 * @description
+		 * The application running the mode is not started.
+		 */
+		reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention),
+					    health_component_t::system,
+					    events::ID("check_external_modes_unavailable"),
+					    events::Log::Error, "Mode is not registered");
+	}
+}
+

src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "../Common.hpp"
+#include <uORB/topics/arming_check_request.h>
+#include <uORB/topics/arming_check_reply.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
+static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
+	      health_component_t::avoidance, "enum definition missmatch");
+
+class ExternalChecks : public HealthAndArmingCheckBase
+{
+public:
+	static constexpr int MAX_NUM_REGISTRATIONS = 8;
+
+	ExternalChecks() = default;
+	~ExternalChecks() = default;
+
+	void setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state);
+
+	void checkAndReport(const Context &context, Report &reporter) override;
+
+	bool hasFreeRegistrations() const { return _active_registrations_mask != (1u << MAX_NUM_REGISTRATIONS) - 1; }
+	/**
+	 * Add registration
+	 * @param nav_mode_id associated mode, -1 if none
+	 * @param replaces_nav_state replaced mode, -1 if none
+	 * @return registration id, or -1
+	 */
+	int addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state);
+	bool removeRegistration(int registration_id, int8_t nav_mode_id);
+	void update();
+
+	bool isUnresponsive(int registration_id);
+
+private:
+	static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
+	static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
+	static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval");
+	static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL
+
+	void checkNonRegisteredModes(const Context &context, Report &reporter) const;
+
+	bool registrationValid(int reg_idx) const { return ((1u << reg_idx) & _active_registrations_mask) != 0; }
+
+	struct Registration {
+		~Registration() { delete reply; }
+
+		int8_t nav_mode_id{-1}; ///< associated mode, -1 if none
+		int8_t replaces_nav_state{-1};
+
+		uint8_t num_no_response{0};
+		bool unresponsive{false};
+		uint8_t total_num_unresponsive{0};
+		arming_check_reply_s *reply{nullptr};
+	};
+
+	unsigned _active_registrations_mask{0};
+	Registration _registrations[MAX_NUM_REGISTRATIONS] {};
+
+	uint8_t _first_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
+	uint8_t _last_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
+
+	// Current requests (async updates)
+	hrt_abstime _last_update{0};
+	unsigned _reply_received_mask{0};
+	bool _had_timeout{false};
+
+	uint8_t _current_request_id{0};
+
+	uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
+
+	uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
+};

src/modules/commander/ModeManagement.cpp

@@ -0,0 +1,541 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef CONSTRAINED_FLASH
+
+#include "ModeManagement.hpp"
+
+#include <px4_platform_common/events.h>
+
+bool ModeExecutors::hasFreeExecutors() const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_mode_executors[i].valid) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor)
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_mode_executors[i].valid) {
+			_mode_executors[i] = executor;
+			_mode_executors[i].valid = true;
+			return i + FIRST_EXECUTOR_ID;
+		}
+	}
+
+	PX4_ERR("logic error");
+	return -1;
+}
+
+void ModeExecutors::removeExecutor(int id)
+{
+	if (valid(id)) {
+		_mode_executors[id - FIRST_EXECUTOR_ID].valid = false;
+	}
+}
+
+void ModeExecutors::printStatus(int executor_in_charge) const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (_mode_executors[i].valid) {
+			int executor_id = i + FIRST_EXECUTOR_ID;
+			PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state,
+				 executor_id == executor_in_charge ? "yes" : "no");
+
+		}
+	}
+}
+
+bool Modes::hasFreeExternalModes() const
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_modes[i].valid) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+uint8_t Modes::addExternalMode(const Modes::Mode &mode)
+{
+	for (int i = 0; i < MAX_NUM; ++i) {
+		if (!_modes[i].valid) {
+			_modes[i] = mode;
+			_modes[i].valid = true;
+			return i + FIRST_EXTERNAL_NAV_STATE;
+		}
+	}
+
+	PX4_ERR("logic error");
+	return -1;
+}
+
+bool Modes::removeExternalMode(uint8_t nav_state, const char *name)
+{
+	if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) {
+		_modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false;
+		return true;
+	}
+
+	PX4_ERR("trying to remove invalid mode %s", name);
+	return false;
+}
+
+void Modes::printStatus() const
+{
+	for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
+		if (valid(i)) {
+			const Modes::Mode &cur_mode = mode(i);
+			PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i,
+				 cur_mode.name);
+
+			if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE
+			    && cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
+				PX4_INFO("  Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]);
+			}
+		}
+	}
+}
+
+ModeManagement::ModeManagement(ExternalChecks &external_checks)
+	: _external_checks(external_checks)
+{
+	_external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE);
+}
+
+void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
+{
+	register_ext_component_request_s request;
+	int max_updates = 5;
+
+	while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request)
+	       && --max_updates >= 0) {
+		request.name[sizeof(request.name) - 1] = '\0';
+		PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id,
+			  request.register_arming_check, request.register_mode, request.register_mode_executor);
+		register_ext_component_reply_s reply{};
+		reply.mode_executor_id = -1;
+		reply.mode_id = -1;
+		reply.arming_check_id = -1;
+		static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
+		memcpy(reply.name, request.name, sizeof(request.name));
+		reply.request_id = request.request_id;
+
+		// 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();
+
+	if (armed) {
+		// Drop registration requests (we could send a failure response)
+		register_ext_component_request_s request;
+		_register_ext_component_request_sub.update(&request);
+
+	} 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();
+	const uint32_t selectable_nav_state_mask = mode_util::getSelectableNavStates();
+	can_set_nav_state_mask = valid_nav_state_mask & selectable_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);
+			}
+		}
+	}
+}
+
+#endif /* CONSTRAINED_FLASH */

src/modules/commander/ModeManagement.hpp

@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ *   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 "ModeUtil/ui.hpp"
+#include "UserModeIntention.hpp"
+#include "HealthAndArmingChecks/checks/externalChecks.hpp"
+
+class ModeExecutors
+{
+public:
+	static constexpr int AUTOPILOT_EXECUTOR_ID = 0;
+	static constexpr int FIRST_EXECUTOR_ID = 1;
+	static constexpr int MAX_NUM = 5;
+
+	struct ModeExecutor {
+		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;
+		}
+
+		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();
+		const uint32_t selectable_nav_state_mask = mode_util::getSelectableNavStates();
+		can_set_nav_state_mask = valid_nav_state_mask & selectable_nav_state_mask & ~(1u <<
+					 vehicle_status_s::NAVIGATION_STATE_TERMINATION);
+	}
+
+	void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { }
+
+private:
+};
+
+#endif /* CONSTRAINED_FLASH */

src/modules/commander/ModeUtil/control_mode.cpp

@@ -42,12 +42,10 @@ static bool stabilization_required(uint8_t vehicle_type)
 	return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
 }
 
-void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
+void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 			   const offboard_control_mode_s &offboard_control_mode,
 			   vehicle_control_mode_s &vehicle_control_mode)
 {
-	vehicle_control_mode.flag_armed = armed;
-
 	switch (nav_state) {
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		vehicle_control_mode.flag_control_manual_enabled = true;
@@ -162,17 +160,11 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
 		vehicle_control_mode.flag_control_velocity_enabled = true;
 		break;
 
+	// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
 	default:
 		break;
 	}
 
-	vehicle_control_mode.flag_multicopter_position_control_enabled =
-		(vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
-		&& (vehicle_control_mode.flag_control_altitude_enabled
-		    || vehicle_control_mode.flag_control_climb_rate_enabled
-		    || vehicle_control_mode.flag_control_position_enabled
-		    || vehicle_control_mode.flag_control_velocity_enabled
-		    || vehicle_control_mode.flag_control_acceleration_enabled);
 }
 
 } // namespace mode_util

src/modules/commander/ModeUtil/control_mode.hpp

@@ -41,7 +41,7 @@
 namespace mode_util
 {
 
-void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
+void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
 			   const offboard_control_mode_s &offboard_control_mode,
 			   vehicle_control_mode_s &vehicle_control_mode);
 

src/modules/commander/ModeUtil/conversions.hpp

@@ -75,36 +75,27 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
 	case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
 
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return navigation_mode_t::external1;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return navigation_mode_t::external2;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return navigation_mode_t::external3;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return navigation_mode_t::external4;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return navigation_mode_t::external5;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return navigation_mode_t::external6;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
 	}
 
-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 23, "code requires update");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX  == 31, "code requires update");
 
 	return navigation_mode_t::unknown;
 }
 
-const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
-	"MANUAL",
-	"ALTCTL",
-	"POSCTL",
-	"AUTO_MISSION",
-	"AUTO_LOITER",
-	"AUTO_RTL",
-	"6: unallocated",
-	"7: unallocated",
-	"AUTO_LANDENGFAIL",
-	"9: unallocated",
-	"ACRO",
-	"11: UNUSED",
-	"DESCEND",
-	"TERMINATION",
-	"OFFBOARD",
-	"STAB",
-	"16: UNUSED2",
-	"AUTO_TAKEOFF",
-	"AUTO_LAND",
-	"AUTO_FOLLOW_TARGET",
-	"AUTO_PRECLAND",
-	"ORBIT"
-};
-
 } // namespace mode_util

src/modules/commander/ModeUtil/mode_requirements.cpp

@@ -170,8 +170,9 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position);
 	setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt);
 
+	// NAVIGATION_STATE_EXTERNALx: handled outside
 
-	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements");
+	static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements");
 }
 
 } // namespace mode_util

src/modules/commander/ModeUtil/standard_modes.hpp

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <uORB/topics/vehicle_status.h>
+
+#include <stdint.h>
+
+namespace mode_util
+{
+
+// This matches the definition from MAVLink MAV_STANDARD_MODE
+enum class StandardMode : uint8_t {
+	NON_STANDARD = 0,
+	POSITION_HOLD = 1,
+	ORBIT = 2,
+	CRUISE = 3,
+	ALTITUDE_HOLD = 4,
+	RETURN_HOME = 5,
+	SAFE_RECOVERY = 6,
+	MISSION = 7,
+	LAND = 8,
+	TAKEOFF = 9,
+};
+
+/**
+ * @return Get MAVLink standard mode from nav_state
+ */
+static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
+{
+	switch (nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF;
+		// Note: all other standard modes do not directly map, or are vehicle-type specific
+	}
+
+	return StandardMode::NON_STANDARD;
+}
+
+/**
+ * @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported
+ */
+static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
+{
+	switch (mode) {
+	case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+	case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+
+	case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+	case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
+
+	default: break;
+	}
+
+	return vehicle_status_s::NAVIGATION_STATE_MAX;
+}
+
+
+} // namespace mode_util

src/modules/commander/ModeUtil/ui.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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");
+}
+
+/**
+ * @return Bitmask with all selectable modes
+ */
+static inline uint32_t getSelectableNavStates()
+{
+	return (1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) |
+	       (1u << vehicle_status_s::NAVIGATION_STATE_POSCTL);
+
+	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",
+};
+
+} // namespace mode_util

src/modules/commander/UserModeIntention.cpp

@@ -35,14 +35,22 @@
 #include "UserModeIntention.hpp"
 
 UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
-				     const HealthAndArmingChecks &health_and_arming_checks)
-	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
+				     const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler)
+	: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks),
+	  _handler(handler)
 {
 }
 
-bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
+bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback,
+			       bool force)
 {
 	_ever_had_mode_change = true;
+	_had_mode_change = true;
+
+	if (_handler) {
+		// If a replacement mode is selected, select the internal one instead. The replacement will be selected after.
+		user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state);
+	}
 
 	// Always allow mode change while disarmed
 	bool always_allow = force || !isArmed();
@@ -68,6 +76,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
 		if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
 			_nav_state_after_disarming = user_intended_nav_state;
 		}
+
+		if (_handler) {
+			_handler->onUserIntendedNavStateChange(source, user_intended_nav_state);
+		}
 	}
 
 	return allow_change;

src/modules/commander/UserModeIntention.hpp

@@ -37,21 +37,42 @@
 #include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
 #include <px4_platform_common/module_params.h>
 
+enum class ModeChangeSource {
+	User,           ///< RC or MAVLink
+	ModeExecutor,
+};
+
+class ModeChangeHandler
+{
+public:
+	virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0;
+
+	/**
+	 * Get the replaced (internal) mode for a given (external) mode
+	 * @param nav_state
+	 * @return nav_state or the mode that nav_state replaces
+	 */
+	virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
+};
+
+
 class UserModeIntention : ModuleParams
 {
 public:
 	UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
-			  const HealthAndArmingChecks &health_and_arming_checks);
+			  const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler);
 	~UserModeIntention() = default;
 
 	/**
 	 * Change the user intended mode
 	 * @param user_intended_nav_state new mode
+	 * @param source calling reason
 	 * @param allow_fallback allow to fallback to a lower mode if current mode cannot run
 	 * @param force always set if true
 	 * @return true if successfully set (also if unchanged)
 	 */
-	bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
+	bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User,
+		    bool allow_fallback = false, bool force = false);
 
 	uint8_t get() const { return _user_intented_nav_state; }
 
@@ -72,6 +93,7 @@ private:
 
 	const vehicle_status_s &_vehicle_status;
 	const HealthAndArmingChecks &_health_and_arming_checks;
+	ModeChangeHandler *const _handler{nullptr};
 
 	uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
 	uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming

src/modules/commander/px4_custom_mode.h

@@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
-	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_TERMINATION
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
@@ -63,7 +64,15 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
 	PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
 	PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
 	PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
-	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
+	PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL1,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL2,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL3,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL4,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL5,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL6,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL7,
+	PX4_CUSTOM_SUB_MODE_EXTERNAL8,
 };
 
 enum PX4_CUSTOM_SUB_MODE_POSCTL {
@@ -131,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_TERMINATION;
 		break;
 
 	case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
@@ -171,6 +180,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
 		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL2;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL3;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL4;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL5;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL6;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL7;
+		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
+		break;
 	}
 
 	return custom_mode;

src/modules/ekf2/EKF/ekf.h

@@ -1155,6 +1155,8 @@ private:
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
+	AlphaFilter<AxisAnglef> _ev_q_error_filt{0.001f};
+	bool _ev_q_error_init{false};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator

src/modules/ekf2/EKF/ev_pos_control.cpp

@@ -95,8 +95,16 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 
 		} else {
 			// rotate EV to the EKF reference frame
-			const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
-			const Dcmf R_ev_to_ekf = Dcmf(q_error);
+			const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
+			if (_ev_q_error_init) {
+				_ev_q_error_filt.update(q_error);
+
+			} else {
+				_ev_q_error_filt.reset(q_error);
+				_ev_q_error_init = true;
+			}
+
+			const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
 
 			pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
 			pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
@@ -230,8 +238,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 			if (!_control_status.flags.gps) {
 				ECL_INFO("reset to %s", EV_AID_SRC_NAME);
 				_information_events.flags.reset_pos_to_vision = true;
-				resetHorizontalPositionTo(measurement, measurement_var);
-				_ev_pos_b_est.reset();
+				resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var);
 
 			} else {
 				_ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement);

src/modules/ekf2/EKF2.cpp

@@ -46,11 +46,12 @@ static px4::atomic<EKF2 *> _objects[EKF2_MAX_INSTANCES] {};
 static px4::atomic<EKF2Selector *> _ekf2_selector {nullptr};
 #endif // CONFIG_EKF2_MULTI_INSTANCE
 
-EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
+EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, config),
 	_replay_mode(replay_mode && !multi_mode),
 	_multi_mode(multi_mode),
+	_config_param(config_param),
 	_instance(multi_mode ? -1 : 0),
 	_attitude_pub(multi_mode ? ORB_ID(estimator_attitude) : ORB_ID(vehicle_attitude)),
 	_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
@@ -112,7 +113,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_req_vdrift(_params->req_vdrift),
 	_param_ekf2_hgt_ref(_params->height_sensor_ref),
 	_param_ekf2_baro_ctrl(_params->baro_ctrl),
-	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
 	_param_ekf2_noaid_tout(_params->valid_timeout_max),
 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	_param_ekf2_rng_ctrl(_params->rng_ctrl),
@@ -136,7 +136,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #endif // CONFIG_EKF2_RANGE_FINDER
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_param_ekf2_ev_delay(_params->ev_delay_ms),
-	_param_ekf2_ev_ctrl(_params->ev_ctrl),
 	_param_ekf2_ev_qmin(_params->ev_quality_minimum),
 	_param_ekf2_evp_noise(_params->ev_pos_noise),
 	_param_ekf2_evv_noise(_params->ev_vel_noise),
@@ -440,6 +439,22 @@ void EKF2::Run()
 				}
 			}
 		}
+
+		if (_config_param == 1) {
+			_params->gnss_ctrl = _param_ekf2_1_gps_ctrl.get();
+
+		} else {
+			_params->gnss_ctrl = _param_ekf2_gps_ctrl.get();
+		}
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_config_param == 1) {
+			_params->ev_ctrl = _param_ekf2_1_ev_ctrl.get();
+
+		} else {
+			_params->ev_ctrl = _param_ekf2_ev_ctrl.get();
+		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}
 
 	if (!_callback_registered) {
@@ -2560,6 +2575,8 @@ int EKF2::task_spawn(int argc, char *argv[])
 	bool multi_mode = false;
 	int32_t imu_instances = 0;
 	int32_t mag_instances = 0;
+	int32_t conf_instances = 0;
+	param_get(param_find("EKF2_MULTI_CONF"), &conf_instances);
 
 	int32_t sens_imu_mode = 1;
 	param_get(param_find("SENS_IMU_MODE"), &sens_imu_mode);
@@ -2624,13 +2641,13 @@ int EKF2::task_spawn(int argc, char *argv[])
 		}
 
 		const hrt_abstime time_started = hrt_absolute_time();
-		const int multi_instances = math::min(imu_instances * mag_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
+		const int multi_instances = math::min(imu_instances * mag_instances * conf_instances, static_cast<int32_t>(EKF2_MAX_INSTANCES));
 		int multi_instances_allocated = 0;
 
 		// allocate EKF2 instances until all found or arming
 		uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
 
-		bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS] {}; // IMUs * mags
+		bool ekf2_instance_created[MAX_NUM_IMUS][MAX_NUM_MAGS][MAX_NUM_CONFS] {}; // IMUs * mags * configs
 
 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
@@ -2650,34 +2667,36 @@ int EKF2::task_spawn(int argc, char *argv[])
 					// Mag & IMU data must be valid, first mag can be ignored initially
 					if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
 
-						if (!ekf2_instance_created[imu][mag]) {
-							EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
+						for (uint8_t conf = 0; conf < conf_instances; conf++) {
+							if (!ekf2_instance_created[imu][mag][conf]) {
+								EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false, conf);
 
-							if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
-								int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
+								if (ekf2_inst && ekf2_inst->multi_init(imu, mag)) {
+									int actual_instance = ekf2_inst->instance(); // match uORB instance numbering
 
-								if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
-									_objects[actual_instance].store(ekf2_inst);
-									success = true;
-									multi_instances_allocated++;
-									ekf2_instance_created[imu][mag] = true;
+									if ((actual_instance >= 0) && (_objects[actual_instance].load() == nullptr)) {
+										_objects[actual_instance].store(ekf2_inst);
+										success = true;
+										multi_instances_allocated++;
+										ekf2_instance_created[imu][mag][conf] = true;
 
-									PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
-										  imu, vehicle_imu_sub.get().accel_device_id,
-										  mag, vehicle_mag_sub.get().device_id);
+										PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
+											  imu, vehicle_imu_sub.get().accel_device_id,
+											  mag, vehicle_mag_sub.get().device_id);
 
-									_ekf2_selector.load()->ScheduleNow();
+										_ekf2_selector.load()->ScheduleNow();
+
+									} else {
+										PX4_ERR("instance numbering problem instance: %d", actual_instance);
+										delete ekf2_inst;
+										break;
+									}
 
 								} else {
-									PX4_ERR("instance numbering problem instance: %d", actual_instance);
-									delete ekf2_inst;
+									PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
+									px4_usleep(100000);
 									break;
 								}
-
-							} else {
-								PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
-								px4_usleep(100000);
-								break;
 							}
 						}
 
@@ -2699,7 +2718,7 @@ int EKF2::task_spawn(int argc, char *argv[])
 
 	{
 		// otherwise launch regular
-		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode);
+		EKF2 *ekf2_inst = new EKF2(false, px4::wq_configurations::INS0, replay_mode, 0);
 
 		if (ekf2_inst) {
 			_objects[0].store(ekf2_inst);

src/modules/ekf2/EKF2.hpp

@@ -115,7 +115,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
 	EKF2() = delete;
-	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode);
+	EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode, int config_param);
 	~EKF2() override;
 
 	/** @see ModuleBase */
@@ -160,6 +160,7 @@ private:
 
 	static constexpr uint8_t MAX_NUM_IMUS = 4;
 	static constexpr uint8_t MAX_NUM_MAGS = 4;
+	static constexpr uint8_t MAX_NUM_CONFS = 2;
 
 	void Run() override;
 
@@ -257,6 +258,7 @@ private:
 
 	const bool _replay_mode{false};			///< true when we use replay data from a log
 	const bool _multi_mode;
+	const int _config_param{0};
 	int _instance{0};
 
 	px4::atomic_bool _task_should_exit{false};
@@ -578,7 +580,8 @@ private:
 		_param_ekf2_aid_mask,		///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used
 		(ParamExtInt<px4::params::EKF2_HGT_REF>) _param_ekf2_hgt_ref,    ///< selects the primary source for height data
 		(ParamExtInt<px4::params::EKF2_BARO_CTRL>) _param_ekf2_baro_ctrl,///< barometer control selection
-		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,  ///< GPS control selection
+		(ParamInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,  ///< GPS control selection
+		(ParamInt<px4::params::EKF2_1_GPS_CTRL>) _param_ekf2_1_gps_ctrl,  ///< Alternate GPS control selection
 
 		(ParamExtInt<px4::params::EKF2_NOAID_TOUT>)
 		_param_ekf2_noaid_tout,	///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec)
@@ -626,7 +629,8 @@ private:
 		(ParamExtFloat<px4::params::EKF2_EV_DELAY>)
 		_param_ekf2_ev_delay, ///< off-board vision measurement delay relative to the IMU (mSec)
 
-		(ParamExtInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl,	 ///< external vision (EV) control selection
+		(ParamInt<px4::params::EKF2_EV_CTRL>) _param_ekf2_ev_ctrl,	 ///< external vision (EV) control selection
+		(ParamInt<px4::params::EKF2_1_EV_CTRL>) _param_ekf2_1_ev_ctrl,	 ///< Alternate external vision (EV) control selection
 		(ParamInt<px4::params::EKF2_EV_NOISE_MD>) _param_ekf2_ev_noise_md, ///< determine source of vision observation noise
 		(ParamExtInt<px4::params::EKF2_EV_QMIN>) _param_ekf2_ev_qmin,
 		(ParamExtFloat<px4::params::EKF2_EVP_NOISE>)

src/modules/ekf2/EKF2Selector.cpp

@@ -754,7 +754,16 @@ void EKF2Selector::Run()
 			}
 		}
 
-		if (!_instance[_selected_instance].healthy.get_state()) {
+		if (_param_ekf2_sel_rc_map.get() > 0) {
+			// Manual instance selection through RC
+			if (isRcOverride()) {
+				SelectInstance(_param_ekf2_sel_rc_inst.get());
+
+			} else {
+				SelectInstance(0);
+			}
+
+		} else if (!_instance[_selected_instance].healthy.get_state()) {
 			// prefer the best healthy instance using a different IMU
 			if (!SelectInstance(best_ekf_different_imu)) {
 				// otherwise switch to the healthy instance with best overall test ratio
@@ -807,6 +816,48 @@ void EKF2Selector::Run()
 	ScheduleDelayed(FILTER_UPDATE_PERIOD);
 }
 
+bool EKF2Selector::isRcOverride()
+{
+	manual_control_setpoint_s manual_control_setpoint{};
+	_manual_control_setpoint_sub.copy(&manual_control_setpoint);
+
+	float channel = 0;
+
+	switch (_param_ekf2_sel_rc_map.get()) {
+	case 0:
+		return false;
+
+	case 1:
+		channel = manual_control_setpoint.aux1;
+		break;
+
+	case 2:
+		channel = manual_control_setpoint.aux2;
+		break;
+
+	case 3:
+		channel = manual_control_setpoint.aux3;
+		break;
+
+	case 4:
+		channel = manual_control_setpoint.aux4;
+		break;
+
+	case 5:
+		channel = manual_control_setpoint.aux5;
+		break;
+
+	case 6:
+		channel = manual_control_setpoint.aux6;
+		break;
+
+	default:
+		return false;
+	}
+
+	return channel > .5f;
+}
+
 void EKF2Selector::PublishEstimatorSelectorStatus()
 {
 	estimator_selector_status_s selector_status{};

src/modules/ekf2/EKF2Selector.hpp

@@ -47,6 +47,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/topics/estimator_selector_status.h>
 #include <uORB/topics/estimator_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/sensors_status_imu.h>
@@ -82,6 +83,8 @@ private:
 
 	void Run() override;
 
+	bool isRcOverride();
+
 	void PrintInstanceChange(const uint8_t old_instance, uint8_t new_instance);
 
 	void PublishEstimatorSelectorStatus();
@@ -228,6 +231,7 @@ private:
 	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
 	uint8_t _odometry_instance_prev{INVALID_INSTANCE};
 
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 
@@ -245,7 +249,9 @@ private:
 		(ParamFloat<px4::params::EKF2_SEL_IMU_RAT>) _param_ekf2_sel_imu_angle_rate,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ANG>) _param_ekf2_sel_imu_angle,
 		(ParamFloat<px4::params::EKF2_SEL_IMU_ACC>) _param_ekf2_sel_imu_accel,
-		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity
+		(ParamFloat<px4::params::EKF2_SEL_IMU_VEL>) _param_ekf2_sel_imu_velocity,
+		(ParamInt<px4::params::EKF2_SEL_RC_MAP>) _param_ekf2_sel_rc_map,
+		(ParamInt<px4::params::EKF2_SEL_RC_INST>) _param_ekf2_sel_rc_inst
 	)
 };
 #endif // !EKF2SELECTOR_HPP

src/modules/ekf2/ekf2_params.c

@@ -686,6 +686,25 @@ PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1);
  */
 PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
 
+/**
+ * (Alt)External vision (EV) sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Horizontal position fusion
+ * 1 : Vertical position fusion
+ * 2 : 3D velocity fusion
+ * 3 : Yaw
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Horizontal position
+ * @bit 1 Vertical position
+ * @bit 2 3D velocity
+ * @bit 3 Yaw
+ */
+PARAM_DEFINE_INT32(EKF2_1_EV_CTRL, 15);
+
 /**
  * GNSS sensor aiding
  *
@@ -705,6 +724,25 @@ PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15);
  */
 PARAM_DEFINE_INT32(EKF2_GPS_CTRL, 7);
 
+/**
+ * (Alt)GNSS sensor aiding
+ *
+ * Set bits in the following positions to enable:
+ * 0 : Longitude and latitude fusion
+ * 1 : Altitude fusion
+ * 2 : 3D velocity fusion
+ * 3 : Dual antenna heading fusion
+ *
+ * @group EKF2
+ * @min 0
+ * @max 15
+ * @bit 0 Lon/lat
+ * @bit 1 Altitude
+ * @bit 2 3D velocity
+ * @bit 3 Dual antenna heading
+ */
+PARAM_DEFINE_INT32(EKF2_1_GPS_CTRL, 7);
+
 /**
  * Range sensor height aiding
  *

src/modules/ekf2/ekf2_params_multi.c

@@ -56,3 +56,17 @@ PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0);
  * @max 4
  */
 PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0);
+
+/**
+ * Multi-EKF Configurations.
+ *
+ * Select the number of configurations to use for Multi-EKF.
+ * Multiple configurations can be used with the same IMU/mag
+ * Requires SENS_IMU_MODE > 0
+ *
+ * @group EKF2
+ * @reboot_required true
+ * @min 1
+ * @max 2
+ */
+PARAM_DEFINE_INT32(EKF2_MULTI_CONF, 1);

src/modules/ekf2/ekf2_params_selector.c

@@ -79,3 +79,39 @@ PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f);
  * @unit m/s
  */
 PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f);
+
+/**
+ * Manual instance selection channel
+ *
+ * Defines which RC_MAP_AUXn parameter maps the RC channel used to select a desired
+ * EKF2 instance.
+ * Use EKF2_SEL_RC_INST to define which instance is selected when this switch is active.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @value 0 Disable
+ * @value 1 Aux1
+ * @value 2 Aux2
+ * @value 3 Aux3
+ * @value 4 Aux4
+ * @value 5 Aux5
+ * @value 6 Aux6
+ * @min 0
+ * @max 6
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_MAP, 0);
+
+/**
+ * Manual instance selection
+ *
+ * Defines which estimator instance is used when manual switch is active.
+ * Use EKF2_SEL_RC_MAP to define which aux switch is used to trigger the switch.
+ * When manual selection is active and the switch is off, instance 0 is selected.
+ * Manual selection overrides automatic instance switch.
+ *
+ * @min 0
+ * @max 10
+ * @group EKF2
+ */
+PARAM_DEFINE_INT32(EKF2_SEL_RC_INST, 0);

src/modules/fake_ev/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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_module(
+	MODULE modules__fake_ev
+	MAIN fake_ev
+	COMPILE_FLAGS
+		${MAX_CUSTOM_OPT_LEVEL}
+	SRCS
+		fake_ev.cpp
+		fake_ev.hpp
+	DEPENDS
+		px4_work_queue
+		mathlib
+)

src/modules/fake_ev/Kconfig

@@ -0,0 +1,12 @@
+menuconfig MODULES_FAKE_EV
+	bool "fake_ev"
+	default y
+	---help---
+		Enable fake external vision publisher
+
+menuconfig USER_FAKE_EV
+	bool "fake_ev running as userspace module"
+	default y
+	depends on BOARD_PROTECTED && MODULES_FAKE_EV
+	---help---
+		Put fake_ev in userspace memory

src/modules/fake_ev/fake_ev.cpp

@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev.cpp
+ */
+
+#include "fake_ev.hpp"
+
+#include <mathlib/mathlib.h>
+
+FakeEV::FakeEV() :
+	ModuleParams(nullptr),
+	WorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+	updateParams();
+}
+
+FakeEV::~FakeEV()
+{
+	perf_free(_cycle_perf);
+}
+
+bool FakeEV::init()
+{
+	if (!_vehicle_gps_position_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	return true;
+}
+
+void FakeEV::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void FakeEV::Run()
+{
+	if (should_exit()) {
+		_vehicle_gps_position_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	if (!_vehicle_gps_position_sub.updated()) {
+		return;
+	}
+
+	perf_begin(_cycle_perf);
+
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s att;
+
+		if (_vehicle_attitude_sub.copy(&att)) {
+			_q_att = matrix::Quatf(att.q);
+		}
+	}
+
+	sensor_gps_s gps{};
+
+	if (_vehicle_gps_position_sub.copy(&gps)) {
+		vehicle_odometry_s odom = gpsToOdom(gps);
+		_vehicle_visual_odometry_pub.publish(odom);
+	}
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		updateParams();
+	}
+
+	perf_end(_cycle_perf);
+}
+
+vehicle_odometry_s FakeEV::gpsToOdom(const sensor_gps_s &gps)
+{
+	vehicle_odometry_s odom{vehicle_odometry_empty};
+
+	const matrix::Quatf q_error(matrix::Eulerf(0.f, 0.f, M_PI_F / 8.f));
+
+	/* odom.timestamp_sample = gps.timestamp_sample; */ // gps timestamp_sample isn't set in SITL
+	odom.timestamp_sample = gps.timestamp;
+
+	const hrt_abstime now = odom.timestamp_sample;
+	const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.5f);
+	_last_run = now;
+
+	if (gps.fix_type >= 3) {
+		if (PX4_ISFINITE(_alt_ref)) {
+			matrix::Vector3f pos = _pos_prev;
+
+			if (!_param_fev_stale.get()) {
+				matrix::Vector2f hpos = _pos_ref.project(gps.lat / 1.0e7, gps.lon / 1.0e7);
+				const float vpos = -((float)gps.alt * 1e-3f - _alt_ref);
+				pos = matrix::Vector3f(hpos(0), hpos(1), vpos);
+				_pos_prev = pos;
+			}
+
+			pos = q_error.rotateVector(pos);
+			_h_drift += _param_fev_h_drift_rate.get() * dt;
+			pos += matrix::Vector3f(_h_drift, _h_drift, 0.f);
+			pos.copyTo(odom.position);
+
+			const float hvar = std::pow(gps.eph, 2.f);
+			const float vvar = std::pow(gps.epv, 2.f);
+			matrix::Vector3f(hvar, hvar, vvar).copyTo(odom.position_variance);
+
+			odom.pose_frame = vehicle_odometry_s::POSE_FRAME_FRD;
+
+		} else {
+			_pos_ref.initReference(gps.lat / 1.0e7, gps.lon / 1.0e7, gps.timestamp);
+			_alt_ref = (float)gps.alt * 1e-3f;
+		}
+	}
+
+	if (gps.vel_ned_valid) {
+		matrix::Vector3f gnss_vel(gps.vel_n_m_s, gps.vel_e_m_s, gps.vel_d_m_s);
+		matrix::Vector3f ev_vel = q_error.rotateVector(gnss_vel);
+
+		ev_vel.copyTo(odom.velocity);
+
+		matrix::Vector3f vel_var;
+		vel_var.setAll(std::pow(gps.s_variance_m_s, 2.f));
+		vel_var.copyTo(odom.velocity_variance);
+		odom.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_NED;
+	}
+
+	(_q_att * q_error).copyTo(odom.q);
+
+	odom.timestamp = hrt_absolute_time();
+
+	return odom;
+}
+
+int FakeEV::task_spawn(int argc, char *argv[])
+{
+	FakeEV *instance = new FakeEV();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int FakeEV::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int FakeEV::print_status()
+{
+	perf_print_counter(_cycle_perf);
+
+	return 0;
+}
+
+int FakeEV::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("fake_ev", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int fake_ev_main(int argc, char *argv[])
+{
+	return FakeEV::main(argc, argv);
+}

src/modules/fake_ev/fake_ev.hpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev.hpp
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_odometry.h>
+
+using namespace time_literals;
+
+class FakeEV : public ModuleBase<FakeEV>, public ModuleParams,
+	public px4::WorkItem
+{
+public:
+	FakeEV();
+	~FakeEV() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+private:
+	static constexpr vehicle_odometry_s vehicle_odometry_empty {
+		.timestamp = 0,
+		.timestamp_sample = 0,
+		.position = {NAN, NAN, NAN},
+		.q = {NAN, NAN, NAN, NAN},
+		.velocity = {NAN, NAN, NAN},
+		.angular_velocity = {NAN, NAN, NAN},
+		.position_variance = {NAN, NAN, NAN},
+		.orientation_variance = {NAN, NAN, NAN},
+		.velocity_variance = {NAN, NAN, NAN},
+		.pose_frame = vehicle_odometry_s::POSE_FRAME_UNKNOWN,
+		.velocity_frame = vehicle_odometry_s::VELOCITY_FRAME_UNKNOWN,
+		.reset_counter = 0,
+		.quality = 0
+	};
+
+	void Run() override;
+	void updateParams() override;
+
+	vehicle_odometry_s gpsToOdom(const sensor_gps_s &gps);
+
+	uORB::Publication<vehicle_odometry_s> _vehicle_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
+
+	uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	uORB::SubscriptionInterval _vehicle_attitude_sub{ORB_ID(vehicle_attitude), 1_s};
+
+	hrt_abstime _timestamp_last{0};
+
+	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude
+	float _alt_ref{NAN};
+
+	hrt_abstime _last_run{0};
+	float _h_drift{0.f};
+	float _h_drift_rate{0.05f};
+
+	bool _is_stale{false};
+	matrix::Vector3f _pos_prev;
+
+	matrix::Quatf _q_att;
+
+	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::FEV_EN>) _param_fev_en,
+		(ParamBool<px4::params::FEV_STALE>) _param_fev_stale,
+		(ParamFloat<px4::params::FEV_H_DRIFT_RATE>) _param_fev_h_drift_rate
+	)
+};

src/modules/fake_ev/fake_ev_params.c

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fake_ev_params.c
+ */
+
+
+/**
+ * Enable fake EV
+ *
+ * Generates fake vision output based on GNSS data
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_EN, 1);
+
+/**
+ * Stale sensor
+ *
+ * Enable to freeze the current output
+ *
+ * @boolean
+ * @group Fake EV
+ */
+PARAM_DEFINE_INT32(FEV_STALE, 0);
+
+/**
+ * Drift rate
+ *
+ * Set the horizontal drift rate
+ *
+ * @min 0
+ * @max 5
+ * @decimal 2
+ * @unit m/s
+ * @group Fake EV
+ */
+PARAM_DEFINE_FLOAT(FEV_H_DRIFT_RATE, 0);

src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp

@@ -321,12 +321,12 @@ void FlightTaskAuto::_limitYawRate()
 		if (!PX4_ISFINITE(_yawspeed_setpoint) && (_deltatime > FLT_EPSILON)) {
 			// Create a feedforward using the filtered derivative
 			_yawspeed_filter.setParameters(_deltatime, .2f);
-			_yawspeed_filter.update(dyaw);
-			_yawspeed_setpoint = _yawspeed_filter.getState() / _deltatime;
+			_yawspeed_filter.update(dyaw / _deltatime);
+			_yawspeed_setpoint = _yawspeed_filter.getState();
 		}
 	}
 
-	_yaw_sp_prev = _yaw_setpoint;
+	_yaw_sp_prev = PX4_ISFINITE(_yaw_setpoint) ? _yaw_setpoint : _yaw;
 
 	if (PX4_ISFINITE(_yawspeed_setpoint)) {
 		// The yaw setpoint is aligned when its rate is not saturated

src/modules/fw_att_control/fw_att_control_params.c

@@ -244,3 +244,31 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_FLOAT(FW_MAN_YR_MAX, 30.f);
+
+/**
+ * Maximum manual roll angle
+ *
+ * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
+
+/**
+ * Maximum manual pitch angle
+ *
+ * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);

src/modules/fw_pos_control/FixedwingPositionControl.cpp

@@ -1466,7 +1466,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			tecs_update_pitch_throttle(control_interval,
 						   altitude_setpoint_amsl,
 						   target_airspeed,
-						   radians(_param_fw_p_lim_min.get()),
+						   radians(_takeoff_pitch_min.get()),
 						   radians(_param_fw_p_lim_max.get()),
 						   _param_fw_thr_min.get(),
 						   max_takeoff_throttle,

src/modules/fw_pos_control/fw_path_navigation_params.c

@@ -603,7 +603,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f);
  * @increment 0.1
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f);
+PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f);
 
 /**
  * Process noise standard deviation for the airspeed rate in the airspeed filter.

src/modules/fw_rate_control/FixedwingRateControl.hpp

@@ -166,9 +166,7 @@ private:
 		(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
 		(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
 
-		(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
 		(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
-		(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
 		(ParamFloat<px4::params::FW_MAN_R_SC>) _param_fw_man_r_sc,
 		(ParamFloat<px4::params::FW_MAN_Y_SC>) _param_fw_man_y_sc,
 

src/modules/fw_rate_control/fw_rate_control_params.c

@@ -484,20 +484,6 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
  */
 PARAM_DEFINE_FLOAT(FW_MAN_R_SC, 1.0f);
 
-/**
- * Maximum manual pitch angle
- *
- * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @decimal 1
- * @increment 0.5
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);
-
 /**
  * Manual pitch scale
  *
@@ -512,20 +498,6 @@ PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);
  */
 PARAM_DEFINE_FLOAT(FW_MAN_P_SC, 1.0f);
 
-/**
- * Maximum manual roll angle
- *
- * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode
- *
- * @unit deg
- * @min 0.0
- * @max 90.0
- * @decimal 1
- * @increment 0.5
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
-
 /**
  * Manual yaw scale
  *

src/modules/logger/logged_topics.cpp

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

src/modules/mavlink/mavlink

@@ -1 +1 @@
-Subproject commit 3ee5382d0c96134b0e1c250d8c2d54bfed0166fa
+Subproject commit 6431ba72c77dc25d4b1b4d9084c9dd9efd9bfea9

src/modules/mavlink/mavlink_command_sender.cpp

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

src/modules/mavlink/mavlink_main.cpp

@@ -1493,9 +1493,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE", 15.0f);
 		configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("DISTANCE_SENSOR", 0.5f);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESC_INFO", 1.0f);
@@ -1560,9 +1562,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream_local("ATTITUDE_TARGET", 10.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
@@ -1631,9 +1635,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
@@ -1710,9 +1716,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE", 50.0f);
 		configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
 		configure_stream_local("ATTITUDE_TARGET", 8.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);
 		configure_stream_local("ESC_STATUS", 10.0f);
@@ -1802,8 +1810,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 
 		configure_stream_local("ADSB_VEHICLE", unlimited_rate);
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
+		configure_stream_local("AVAILABLE_MODES", 0.3f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
@@ -2434,7 +2444,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 					if (!command_ack.from_external
 					    && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
-					    && is_target_known) {
+					    && is_target_known
+					    && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
 
 						mavlink_command_ack_t msg{};
 						msg.result = command_ack.result;

src/modules/mavlink/mavlink_messages.cpp

@@ -121,6 +121,11 @@
 #include "streams/VIBRATION.hpp"
 #include "streams/WIND_COV.hpp"
 
+#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"
@@ -228,7 +233,7 @@ static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast<MAV_SE
 static_assert(MAV_SENSOR_ROTATION_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_PITCH_315), "Pitch: 315");
 static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_ROLL_90_PITCH_315),
 	      "Roll: 90, Pitch: 315");
-static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync");
+static_assert(41 + 3 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync");
 
 
 static const StreamListItem streams_list[] = {
@@ -481,8 +486,14 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
 #endif // UAVIONIX_ADSB_OUT_CFG_HPP
 #if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
-	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
+	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
 #endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+#if defined(AVAILABLE_MODES_HPP)
+	create_stream_list_item<MavlinkStreamAvailableModes>(),
+#endif // AVAILABLE_MODES_HPP
+#if defined(CURRENT_MODE_HPP)
+	create_stream_list_item<MavlinkStreamCurrentMode>(),
+#endif // CURRENT_MODE_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/mavlink_messages.h

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

src/modules/mavlink/streams/AVAILABLE_MODES.hpp

@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   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 <commander/ModeUtil/ui.hpp>
+#include <commander/ModeUtil/standard_modes.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};
+
+	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;
+
+		// Set the mode name if not a standard mode
+		available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state);
+
+		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 (_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 ((vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0) {
+			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 (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 (dynamic_update) {
+			_had_dynamic_update = true;
+			++_dynamic_update_seq;
+		}
+	}
+
+	bool send() override
+	{
+		if (_had_dynamic_update) {
+			mavlink_available_modes_monitor_t monitor{};
+			monitor.seq = _dynamic_update_seq;
+			mavlink_msg_available_modes_monitor_send_struct(_mavlink->get_channel(), &monitor);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // AVAILABLE_MODES_HPP

src/modules/mavlink/streams/CURRENT_MODE.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef CURRENT_MODE_HPP
+#define CURRENT_MODE_HPP
+
+#include <uORB/topics/vehicle_status.h>
+#include <commander/ModeUtil/standard_modes.hpp>
+
+class MavlinkStreamCurrentMode : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCurrentMode(mavlink); }
+
+	static constexpr const char *get_name_static() { return "CURRENT_MODE"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CURRENT_MODE; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_CURRENT_MODE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamCurrentMode(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	bool send() override
+	{
+		vehicle_status_s vehicle_status;
+
+		if (_vehicle_status_sub.update(&vehicle_status)) {
+			mavlink_current_mode_t current_mode{};
+			current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data;
+			current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data;
+			current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state);
+			mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), &current_mode);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // CURRENT_MODE_HPP

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -5,6 +5,15 @@
 #####
 publications:
 
+  - topic: /fmu/out/register_ext_component_reply
+    type: px4_msgs::msg::RegisterExtComponentReply
+
+  - topic: /fmu/out/arming_check_request
+    type: px4_msgs::msg::ArmingCheckRequest
+
+  - topic: /fmu/out/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
   - topic: /fmu/out/collision_constraints
     type: px4_msgs::msg::CollisionConstraints
 
@@ -29,6 +38,9 @@ publications:
   - topic: /fmu/out/vehicle_control_mode
     type: px4_msgs::msg::VehicleControlMode
 
+  - topic: /fmu/out/vehicle_command_ack
+    type: px4_msgs::msg::VehicleCommandAck
+
   - topic: /fmu/out/vehicle_global_position
     type: px4_msgs::msg::VehicleGlobalPosition
 
@@ -41,6 +53,9 @@ publications:
   - topic: /fmu/out/vehicle_odometry
     type: px4_msgs::msg::VehicleOdometry
 
+  - topic: /fmu/out/vehicle_fake_visual_odometry
+    type: px4_msgs::msg::VehicleOdometry
+
   - topic: /fmu/out/vehicle_status
     type: px4_msgs::msg::VehicleStatus
 
@@ -48,6 +63,23 @@ publications:
     type: px4_msgs::msg::VehicleTrajectoryWaypoint
 
 subscriptions:
+  - topic: /fmu/in/register_ext_component_request
+    type: px4_msgs::msg::RegisterExtComponentRequest
+
+  - topic: /fmu/in/unregister_ext_component
+    type: px4_msgs::msg::UnregisterExtComponent
+
+  - topic: /fmu/in/config_overrides_request
+    type: px4_msgs::msg::ConfigOverrides
+
+  - topic: /fmu/in/arming_check_reply
+    type: px4_msgs::msg::ArmingCheckReply
+
+  - topic: /fmu/in/mode_completed
+    type: px4_msgs::msg::ModeCompleted
+
+  - topic: /fmu/in/config_control_setpoints
+    type: px4_msgs::msg::VehicleControlMode
 
   - topic: /fmu/in/offboard_control_mode
     type: px4_msgs::msg::OffboardControlMode
@@ -82,6 +114,9 @@ subscriptions:
   - topic: /fmu/in/vehicle_command
     type: px4_msgs::msg::VehicleCommand
 
+  - topic: /fmu/in/vehicle_command_mode_executor
+    type: px4_msgs::msg::VehicleCommand
+
   - topic: /fmu/in/vehicle_trajectory_bezier
     type: px4_msgs::msg::VehicleTrajectoryBezier
 

src/modules/vtol_att_control/tailsitter.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,9 +42,6 @@
 #include "tailsitter.h"
 #include "vtol_att_control_main.h"
 
-#define PITCH_TRANSITION_FRONT_P1 -1.1f	// pitch angle to switch to TRANSITION_P2
-#define PITCH_TRANSITION_BACK -0.25f	// pitch angle to switch to MC
-
 using namespace matrix;
 
 Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
@@ -91,8 +88,15 @@ void Tailsitter::update_vtol_state()
 		case vtol_mode::TRANSITION_BACK:
 			const float pitch = Eulerf(Quatf(_v_att->q)).theta();
 
+			float pitch_threshold_mc = PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC;
+
+			// if doing transition in Stabilized mode set threshold to max angle plus 5° margin
+			if (!_v_control_mode->flag_control_altitude_enabled) {
+				pitch_threshold_mc = math::radians(-_param_mpc_tilt_max.get() - 5.f);
+			}
+
 			// check if we have reached pitch angle to switch to MC mode
-			if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
+			if (pitch >= pitch_threshold_mc || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
 				_vtol_mode = vtol_mode::MC_MODE;
 			}
 
@@ -115,6 +119,7 @@ void Tailsitter::update_vtol_state()
 
 				if (isFrontTransitionCompleted()) {
 					_vtol_mode = vtol_mode::FW_MODE;
+					_trans_finished_ts = hrt_absolute_time();
 				}
 
 				break;
@@ -288,7 +293,7 @@ void Tailsitter::fill_actuator_outputs()
 
 		/* allow differential thrust if enabled */
 		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
-			_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get();
+			_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_y.get();
 		}
 
 		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
@@ -296,7 +301,7 @@ void Tailsitter::fill_actuator_outputs()
 		}
 
 		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
-			_torque_setpoint_0->xyz[2] = -_vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_r.get();
+			_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get();
 		}
 
 	} else {
@@ -324,7 +329,14 @@ bool Tailsitter::isFrontTransitionCompletedBase()
 	bool transition_to_fw = false;
 	const float pitch = Eulerf(Quatf(_v_att->q)).theta();
 
-	if (pitch <= PITCH_TRANSITION_FRONT_P1) {
+	float pitch_threshold_fw = PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW;
+
+	// if doing transition in Stabilized mode set threshold to max angle minus 5° margin
+	if (!_v_control_mode->flag_control_altitude_enabled) {
+		pitch_threshold_fw = math::radians(-_param_mpc_tilt_max.get() + 5.f);
+	}
+
+	if (pitch <= pitch_threshold_fw) {
 		if (airspeed_triggers_transition) {
 			transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
 

src/modules/vtol_att_control/tailsitter.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -48,6 +48,12 @@
 #include <drivers/drv_hrt.h>
 #include <matrix/matrix/math.hpp>
 
+// [rad] Pitch threshold required for completing transition to fixed-wing in automatic transitions
+static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW = -1.05f; // -60°
+
+// [rad] Pitch threshold required for completing transition to hover in automatic transitions
+static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC = -0.26f; // -15°
+
 class Tailsitter : public VtolType
 {
 
@@ -82,7 +88,8 @@ private:
 	bool isFrontTransitionCompletedBase() override;
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
-					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
+					(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
+					(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_tilt_max
 				       )
 
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -134,6 +134,7 @@ void Tiltrotor::update_vtol_state()
 		case vtol_mode::TRANSITION_FRONT_P1: {
 				if (isFrontTransitionCompleted()) {
 					_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
+					_trans_finished_ts = hrt_absolute_time();
 					resetTransitionStates();
 				}
 

src/modules/vtol_att_control/vtol_att_control_params.c

@@ -217,6 +217,8 @@ PARAM_DEFINE_FLOAT(VT_QC_HR_ERROR_I, 0.0f);
  * Quad-chute transition altitude loss threshold
  *
  * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight.
+ * Active until 5s after completing transition to fixed-wing.
+ * Only active if altitude estimate is valid and in altitude-controlled mode.
  * If the current altitude is more than this value below the altitude at the beginning of the
  * transition, it will instantly switch back to MC mode and execute behavior defined in COM_QC_ACT.
  *

src/modules/vtol_att_control/vtol_type.cpp

@@ -301,16 +301,11 @@ bool VtolType::isFrontTransitionAltitudeLoss()
 {
 	bool result = false;
 
-	if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW && _local_pos->z_valid) {
+	// only run if param set, altitude valid and controlled, and in transition to FW or within 5s of finishing it.
+	if (_param_vt_qc_t_alt_loss.get() > FLT_EPSILON && _local_pos->z_valid && _v_control_mode->flag_control_altitude_enabled
+	    && (_common_vtol_mode == mode::TRANSITION_TO_FW || hrt_elapsed_time(&_trans_finished_ts) < 5_s)) {
 
-		if (_local_pos->z <= FLT_EPSILON) {
-			// vehilce is above home
-			result = _local_pos->z - _local_position_z_start_of_transition > _param_vt_qc_t_alt_loss.get();
-
-		} else {
-			// vehilce is below home
-			result = _local_position_z_start_of_transition - _local_pos->z > _param_vt_qc_t_alt_loss.get();
-		}
+		result = _local_pos->z - _local_position_z_start_of_transition > _param_vt_qc_t_alt_loss.get();
 	}
 
 	return result;

src/ros2/cpp/px4_sdk_lib/CMakeLists.txt

@@ -0,0 +1,99 @@
+cmake_minimum_required(VERSION 3.5)
+project(px4_sdk_lib)
+
+# Default to C++17
+if(NOT CMAKE_CXX_STANDARD)
+	set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+	add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wno-unused-parameter)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(px4_msgs REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+include_directories(include ${Eigen3_INCLUDE_DIRS})
+set(HEADER_FILES
+		include/px4_sdk/components/events.h
+		include/px4_sdk/components/health_and_arming_checks.h
+		include/px4_sdk/components/mode.h
+		include/px4_sdk/components/mode_executor.h
+		include/px4_sdk/components/overrides.h
+		include/px4_sdk/components/setpoints.h
+		include/px4_sdk/components/wait_for_fmu.h
+	)
+
+add_library(px4_sdk_lib
+		src/components/health_and_arming_checks.cpp
+		src/components/mode.cpp
+		src/components/mode_executor.cpp
+		src/components/overrides.cpp
+		src/components/registration.cpp
+		src/components/setpoints.cpp
+		src/components/wait_for_fmu.cpp
+		${HEADER_FILES}
+	)
+ament_target_dependencies(px4_sdk_lib Eigen3 rclcpp px4_msgs)
+
+ament_export_targets(px4_sdk_lib HAS_LIBRARY_TARGET)
+ament_export_dependencies(
+    px4_msgs
+    rclcpp
+	eigen3_cmake_module
+	Eigen3
+)
+install(
+	DIRECTORY include/px4_sdk
+	DESTINATION include
+)
+install(
+	TARGETS px4_sdk_lib
+	EXPORT px4_sdk_lib
+	LIBRARY DESTINATION lib
+	ARCHIVE DESTINATION lib
+	RUNTIME DESTINATION bin
+	INCLUDES DESTINATION include
+)
+
+if(BUILD_TESTING)
+	find_package(ament_lint_auto REQUIRED)
+	find_package(ament_cmake_gtest REQUIRED)
+	ament_lint_auto_find_test_dependencies()
+
+	add_library(integration_utils
+			test/integration/util.cpp
+			)
+	include_directories(include)
+	ament_target_dependencies(integration_utils rclcpp px4_msgs)
+
+	ament_add_gtest(test_arming_check test/integration/arming_check.cpp)
+	if(TARGET test_arming_check)
+		target_link_libraries(test_arming_check ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_arming_check PRIVATE include)
+	endif()
+
+	ament_add_gtest(test_mode test/integration/mode.cpp)
+	if(TARGET test_mode)
+		target_link_libraries(test_mode ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_mode PRIVATE include)
+	endif()
+
+	ament_add_gtest(test_mode_executor test/integration/mode_executor.cpp)
+	if(TARGET test_mode_executor)
+		target_link_libraries(test_mode_executor ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_mode_executor PRIVATE include)
+	endif()
+
+	ament_add_gtest(test_overrides test/integration/overrides.cpp)
+	if(TARGET test_overrides)
+		target_link_libraries(test_overrides ${PROJECT_NAME} integration_utils)
+		target_include_directories(test_overrides PRIVATE include)
+	endif()
+
+endif()
+
+ament_package()

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/events.h

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   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 <px4_msgs/msg/arming_check_reply.hpp>
+
+namespace events
+{
+
+using EventType = px4_msgs::msg::Event;
+
+enum class LogLevel : uint8_t {
+	Emergency = 0,
+	Alert = 1,
+	Critical = 2,
+	Error = 3,
+	Warning = 4,
+	Notice = 5,
+	Info = 6,
+	Debug = 7,
+	Protocol = 8,
+	Disabled = 9,
+
+	Count
+};
+
+enum class LogLevelInternal : uint8_t {
+	Emergency = 0,
+	Alert = 1,
+	Critical = 2,
+	Error = 3,
+	Warning = 4,
+	Notice = 5,
+	Info = 6,
+	Debug = 7,
+	Protocol = 8,
+	Disabled = 9,
+
+	Count
+};
+
+using Log = LogLevel;
+using LogInternal = LogLevelInternal;
+
+struct LogLevels {
+	LogLevels() {}
+	LogLevels(Log external_level) : external(external_level), internal((LogInternal)external_level) {}
+	LogLevels(Log external_level, LogInternal internal_level)
+		: external(external_level), internal(internal_level) {}
+
+	Log external{Log::Info};
+	LogInternal internal{LogInternal::Info};
+};
+
+namespace util
+{
+
+// source: https://gist.github.com/ruby0x1/81308642d0325fd386237cfa3b44785c
+constexpr uint32_t val_32_const = 0x811c9dc5;
+constexpr uint32_t prime_32_const = 0x1000193;
+constexpr uint64_t val_64_const = 0xcbf29ce484222325;
+constexpr uint64_t prime_64_const = 0x100000001b3;
+inline constexpr uint32_t hash_32_fnv1a_const(const char *const str, const uint32_t value = val_32_const) noexcept
+{
+	return (str[0] == '\0') ? value : hash_32_fnv1a_const(&str[1], (value ^ uint32_t(str[0])) * prime_32_const);
+}
+
+template<typename T>
+inline constexpr void fillEventArguments(uint8_t *buf, T arg)
+{
+	// This assumes we're on little-endian
+	memcpy(buf, &arg, sizeof(T));
+}
+
+template<typename T, typename... Args>
+inline constexpr void fillEventArguments(uint8_t *buf, T arg, Args... args)
+{
+	fillEventArguments(buf, arg);
+	fillEventArguments(buf + sizeof(T), args...);
+}
+
+constexpr unsigned sizeofArguments() { return 0; }
+
+template <typename T, typename... Args>
+constexpr unsigned sizeofArguments(const T &t, const Args &... args)
+{
+	return sizeof(T) + sizeofArguments(args...);
+}
+
+} // namespace util
+
+/**
+ * Generate event ID from an event name
+ */
+template<size_t N>
+constexpr uint32_t ID(const char (&name)[N])
+{
+	// Note: the generated ID must match with the python generator under Tools/px4events
+	uint32_t component_id = 1u << 24; // autopilot component
+	return (0xffffff & util::hash_32_fnv1a_const(name)) | component_id;
+}
+
+
+} // namespace events
+

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/health_and_arming_checks.h

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/arming_check_reply.hpp>
+#include <px4_msgs/msg/arming_check_request.hpp>
+
+#include "events.h"
+
+#include <memory>
+#include <functional>
+
+class Registration;
+
+namespace px4_sdk
+{
+
+struct ModeRequirements {
+
+	static ModeRequirements none()
+	{
+		// None is the default already
+		return ModeRequirements{};
+	}
+	static ModeRequirements autonomous()
+	{
+		ModeRequirements ret{};
+		ret.angular_velocity = true;
+		ret.attitude = true;
+		ret.local_alt = true;
+		ret.local_position = true;
+		ret.global_position = true;
+		ret.home_position = true;
+		return ret;
+	}
+	static ModeRequirements manualControlledPosition()
+	{
+		ModeRequirements ret{};
+		ret.angular_velocity = true;
+		ret.attitude = true;
+		ret.local_alt = true;
+		ret.local_position_relaxed = true;
+		return ret;
+	}
+
+	bool angular_velocity{false};
+	bool attitude{false};
+	bool local_alt{false};
+	bool local_position{false};
+	bool local_position_relaxed{false};
+	bool global_position{false};
+	bool mission{false};
+	bool home_position{false};
+	bool prevent_arming{false};
+};
+
+
+class HealthAndArmingCheckReporter
+{
+public:
+	HealthAndArmingCheckReporter(px4_msgs::msg::ArmingCheckReply &arming_check_reply)
+		: _arming_check_reply(arming_check_reply) {}
+
+	template<typename... Args>
+	void armingCheckFailureExt(uint32_t event_id,
+				   events::Log log_level, const char *message, Args... args)
+	{
+		uint16_t navigation_mode_groups{};
+		uint8_t health_component_index{};
+
+		_arming_check_reply.can_arm_and_run = false;
+
+		if (!addEvent(event_id, log_level, message, navigation_mode_groups,
+			      health_component_index, args...)) {
+			printf("Error: too many events\n");
+		}
+	}
+
+	void setHealth(uint8_t health_component_index, bool is_present, bool warning, bool error);
+
+private:
+	template<typename... Args>
+	bool addEvent(uint32_t event_id, const events::LogLevels &log_levels, const char *message, Args... args);
+
+	px4_msgs::msg::ArmingCheckReply &_arming_check_reply;
+};
+
+
+template<typename... Args>
+bool HealthAndArmingCheckReporter::addEvent(uint32_t event_id, const events::LogLevels &log_levels,
+		const char *message, Args... args)
+{
+	if (_arming_check_reply.num_events >= _arming_check_reply.events.size()) {
+		return false;
+	}
+
+	events::EventType &e = _arming_check_reply.events[_arming_check_reply.num_events];
+	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
+	e.id = event_id;
+	static_assert(events::util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments");
+	events::util::fillEventArguments((uint8_t *)e.arguments.data(), args...);
+	++_arming_check_reply.num_events;
+	return true;
+}
+
+
+inline void HealthAndArmingCheckReporter::setHealth(uint8_t health_component_index, bool is_present, bool warning,
+		bool error)
+{
+	_arming_check_reply.health_component_index = health_component_index;
+	_arming_check_reply.health_component_is_present = is_present;
+	_arming_check_reply.health_component_warning = warning;
+	_arming_check_reply.health_component_error = error;
+}
+
+
+class HealthAndArmingChecks
+{
+public:
+	using CheckCallback = std::function<void(HealthAndArmingCheckReporter &)>;
+
+	HealthAndArmingChecks(rclcpp::Node &node, const CheckCallback &check_callback,
+			      const std::string &topic_namespace_prefix = "");
+	HealthAndArmingChecks(const HealthAndArmingChecks &) = delete;
+
+	/**
+	 * Register the checks. Call this once on startup. This is a blocking method.
+	 * @param name registration name. Should uniquely identify the component with length < 25 characters
+	 * @return true on success
+	 */
+	bool doRegister(const std::string &name);
+
+	void setModeRequirements(const ModeRequirements &mode_requirements) { _mode_requirements = mode_requirements; }
+
+private:
+	friend class ModeBase;
+	friend class ModeExecutorBase;
+	void overrideRegistration(const std::shared_ptr<Registration> &registration);
+
+	void watchdogTimerUpdate();
+
+	rclcpp::Node &_node;
+	std::shared_ptr<Registration> _registration;
+	CheckCallback _check_callback;
+	bool _check_triggered{true};
+
+	rclcpp::Subscription<px4_msgs::msg::ArmingCheckRequest>::SharedPtr _arming_check_request_sub;
+	rclcpp::Publisher<px4_msgs::msg::ArmingCheckReply>::SharedPtr _arming_check_reply_pub;
+
+	ModeRequirements _mode_requirements{};
+	rclcpp::TimerBase::SharedPtr _watchdog_timer;
+	bool _shutdown_on_timeout{true};
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode.h

@@ -0,0 +1,194 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_msgs/msg/mode_completed.hpp>
+#include "health_and_arming_checks.h"
+#include "setpoints.h"
+#include "overrides.h"
+
+class Registration;
+struct RegistrationSettings;
+
+namespace px4_sdk
+{
+
+enum class Result {
+	Success = 0,
+	Rejected, ///< The request was rejected
+	Interrupted, ///< Ctrl-C or another error (from ROS)
+	Timeout,
+	Deactivated, ///< Mode or executor got deactivated
+
+	// Mode-specific results
+	ModeFailureOther = 100,
+};
+
+static_assert((int)Result::ModeFailureOther == (int)px4_msgs::msg::ModeCompleted::RESULT_FAILURE_OTHER,
+	      "definition mismatch");
+
+constexpr inline const char *resultToString(Result result) noexcept
+{
+	switch (result) {
+	case Result::Success: return "Success";
+
+	case Result::Rejected: return "Rejected";
+
+	case Result::Interrupted: return "Interrupted";
+
+	case Result::Timeout: return "Timeout";
+
+	case Result::Deactivated: return "Deactivated";
+
+	case Result::ModeFailureOther: return "Mode Failure (generic)";
+	}
+
+	return "Unknown";
+}
+
+class ModeBase
+{
+public:
+	using ID_t = uint8_t; ///< Mode ID, corresponds to nav_state
+	static constexpr ID_t ID_INVALID = 0xff;
+
+	static constexpr ID_t ID_NAVIGATION_STATE_POSCTL = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_POSCTL;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_TAKEOFF = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_TAKEOFF;
+	static constexpr ID_t ID_NAVIGATION_STATE_DESCEND = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_DESCEND;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_LAND = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_LAND;
+	static constexpr ID_t ID_NAVIGATION_STATE_AUTO_RTL = px4_msgs::msg::VehicleStatus::NAVIGATION_STATE_AUTO_RTL;
+
+	struct Settings {
+		std::string name; ///< Name of the mode with length < 25 characters
+		bool activate_even_while_disarmed{true}; ///< If true, the mode is also activated while disarmed if selected
+		ID_t replace_internal_mode{ID_INVALID}; ///< Can be used to replace an fmu-internal mode
+	};
+
+	ModeBase(rclcpp::Node &node, const Settings &settings, const ModeRequirements &requirements,
+		 const std::string &topic_namespace_prefix = "");
+	ModeBase(const ModeBase &) = delete;
+	virtual ~ModeBase() = default;
+
+	/**
+	 * Register the mode. Call this once on startup, unless there's an associated executor. This is a blocking method.
+	 * @return true on success
+	 */
+	bool doRegister();
+
+
+	/**
+	 * Report any custom mode requirements. This is called regularly, also while the mode is active.
+	 */
+	virtual void checkArmingAndRunConditions(HealthAndArmingCheckReporter &reporter) {}
+
+	/**
+	 * Called whenever the mode is activated, also if the vehicle is disarmed
+	 */
+	virtual void onActivate() = 0;
+
+	/**
+	 * Called whenever the mode is deactivated, also if the vehicle is disarmed
+	 */
+	virtual void onDeactivate() = 0;
+
+	/**
+	 * Set the update rate when the mode is active. This is disabled by default.
+	 * @param rate_hz set to 0 to disable
+	 */
+	void setSetpointUpdateRate(float rate_hz);
+
+	virtual void updateSetpoint() {}
+
+	/**
+	 * Mode completed signal. Call this when the mode is finished. A mode might never call this, but modes like
+	 * RTL, Land or Takeoff are expected to signal their completion.
+	 * @param result
+	 */
+	void completed(Result result);
+
+
+	// Properties & state
+
+	ID_t id() const;
+
+	bool isArmed() const { return _is_armed; }
+
+	bool isActive() const { return _is_active; }
+
+	rclcpp::Node &node() { return _node; }
+
+	SetpointSender &setpoints() { return _setpoint_sender; }
+
+	ConfigOverrides &configOverrides() { return _config_overrides; }
+
+private:
+	friend class ModeExecutorBase;
+	void overrideRegistration(const std::shared_ptr<Registration> &registration);
+	RegistrationSettings getRegistrationSettings() const;
+	void onRegistered();
+
+	void unsubscribeVehicleStatus();
+	void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate = false);
+
+	void callOnActivate();
+	void callOnDeactivate();
+
+	void updateSetpointUpdateTimer();
+
+	rclcpp::Node &_node;
+	std::shared_ptr<Registration> _registration;
+
+	const Settings _settings;
+
+	HealthAndArmingChecks _health_and_arming_checks;
+
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr _vehicle_status_sub;
+	rclcpp::Publisher<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_pub;
+
+	bool _is_active{false}; ///< Mode is currently selected
+	bool _is_armed{false}; ///< Is vehicle armed?
+	bool _completed{false}; ///< Is mode completed?
+
+	float _setpoint_update_rate_hz{0.f};
+	rclcpp::TimerBase::SharedPtr _setpoint_update_timer;
+	SetpointSender _setpoint_sender;
+
+	ConfigOverrides _config_overrides;
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/mode_executor.h

@@ -0,0 +1,208 @@
+/****************************************************************************
+ *
+ *   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 "mode.h"
+#include "overrides.h"
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_status.hpp>
+#include <px4_msgs/msg/vehicle_command.hpp>
+#include <px4_msgs/msg/vehicle_command_ack.hpp>
+#include <px4_msgs/msg/mode_completed.hpp>
+
+#include <functional>
+
+class Registration;
+
+namespace px4_sdk
+{
+
+class ModeExecutorBase
+{
+public:
+	using CompletedCallback = std::function<void(Result)>;
+
+	struct Settings {
+		bool activate_immediately{false}; ///< If set activate the mode (and executor) immediately. Only use this for fully autonomous executors that also arm the vehicle
+	};
+
+	enum class DeactivateReason {
+		FailsafeActivated,
+		Other
+	};
+
+	ModeExecutorBase(rclcpp::Node &node, const Settings &settings, ModeBase &owned_mode,
+			 const std::string &topic_namespace_prefix = "");
+	ModeExecutorBase(const ModeExecutorBase &) = delete;
+	virtual ~ModeExecutorBase() = default;
+
+	/**
+	 * Register the mode executor. Call this once on startup. This is a blocking method.
+	 * @return true on success
+	 */
+	bool doRegister();
+
+
+	/**
+	 * Called whenever the mode is activated, also if the vehicle is disarmed
+	 */
+	virtual void onActivate() = 0;
+
+	/**
+	 * Called whenever the mode is deactivated, also if the vehicle is disarmed
+	 */
+	virtual void onDeactivate(DeactivateReason reason) = 0;
+
+	/**
+	 * Called when failsafes are currently being deferred (see @deferFailsafes), and the FMU wants to trigger
+	 * a failsafe.
+	 */
+	virtual void onFailsafeDeferred() {}
+
+	/**
+	* Send command and wait for ack/nack
+	*/
+	Result sendCommandSync(uint32_t command, float param1 = NAN, float param2 = NAN, float param3 = NAN, float param4 = NAN,
+			       float param5 = NAN, float param6 = NAN, float param7 = NAN);
+
+	/**
+	 * Switch to a mode with a callback when it is finished.
+	 * The callback is also executed when the mode is deactivated.
+	 * If there's already a mode scheduling active, the previous one is cancelled.
+	 */
+	void scheduleMode(ModeBase::ID_t mode_id, const CompletedCallback &on_completed);
+
+	void takeoff(const CompletedCallback &on_completed, float altitude = NAN, float heading = NAN);
+	void land(const CompletedCallback &on_completed);
+	void rtl(const CompletedCallback &on_completed);
+
+	void arm(const CompletedCallback &on_completed);
+	void waitReadyToArm(const CompletedCallback &on_completed);
+	void waitUntilDisarmed(const CompletedCallback &on_completed);
+
+	bool isInCharge() const { return _is_in_charge; }
+
+	bool isArmed() const { return _is_armed; }
+
+	ModeBase &ownedMode() { return _owned_mode; }
+
+	int id() const;
+
+	rclcpp::Node &node() { return _node; }
+
+	ConfigOverrides &configOverrides() { return _config_overrides; }
+
+	/**
+	 * Enable/disable deferring failsafes. While enabled (and the executor is in charge),
+	 * most failsafes are prevented from being triggered until the given timeout is exceeded.
+	 * Some failsafes that cannot be prevented:
+	 * - vehicle exceeds attitude limits (can be disabled via parameters)
+	 * - the mode cannot run (some mode requirements are not met, such as no position estimate)
+	 *
+	 * If the executor is in charge, this method will wait for the FMU for acknowledgement (to avoid race conditions)
+	 * @param enabled
+	 * @param timeout_s 0=system default, -1=no timeout
+	 * @return true on success
+	 */
+	bool deferFailsafesSync(bool enabled, int timeout_s = 0);
+
+private:
+
+	class ScheduledMode
+	{
+	public:
+		ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix);
+
+		bool active() const { return _mode_id != ModeBase::ID_INVALID; }
+		void activate(ModeBase::ID_t mode_id, const CompletedCallback &on_completed);
+		void cancel();
+		ModeBase::ID_t modeId() const { return _mode_id; }
+	private:
+		void reset() { _mode_id = ModeBase::ID_INVALID; }
+
+		ModeBase::ID_t _mode_id{ModeBase::ID_INVALID};
+		CompletedCallback _on_completed_callback;
+		rclcpp::Subscription<px4_msgs::msg::ModeCompleted>::SharedPtr _mode_completed_sub;
+	};
+
+	class WaitForVehicleStatusCondition
+	{
+	public:
+		using RunCheckCallback = std::function<bool(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)>;
+		WaitForVehicleStatusCondition() = default;
+
+		bool active() const { return _on_completed_callback != nullptr; }
+		void update(const px4_msgs::msg::VehicleStatus::UniquePtr &msg);
+
+		void activate(const RunCheckCallback &run_check_callback, const CompletedCallback &on_completed);
+		void cancel();
+	private:
+		CompletedCallback _on_completed_callback;
+		RunCheckCallback _run_check_callback;
+	};
+
+	void onRegistered();
+
+	void callOnActivate();
+	void callOnDeactivate(DeactivateReason reason);
+
+	void vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg);
+
+	void scheduleMode(ModeBase::ID_t mode_id, const px4_msgs::msg::VehicleCommand &cmd,
+			  const ModeExecutorBase::CompletedCallback &on_completed);
+
+	rclcpp::Node &_node;
+	const Settings _settings;
+	ModeBase &_owned_mode;
+
+	std::shared_ptr<Registration> _registration;
+
+	rclcpp::Subscription<px4_msgs::msg::VehicleStatus>::SharedPtr _vehicle_status_sub;
+	rclcpp::Publisher<px4_msgs::msg::VehicleCommand>::SharedPtr _vehicle_command_pub;
+	rclcpp::Subscription<px4_msgs::msg::VehicleCommandAck>::SharedPtr _vehicle_command_ack_sub;
+
+	ScheduledMode _current_scheduled_mode;
+	WaitForVehicleStatusCondition _current_wait_vehicle_status;
+
+	bool _is_in_charge{false};
+	bool _is_armed{false};
+	bool _was_never_activated{true};
+	ModeBase::ID_t _prev_nav_state{ModeBase::ID_INVALID};
+	uint8_t _prev_failsafe_defer_state{px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED};
+
+	ConfigOverrides _config_overrides;
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/overrides.h

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+
+#include <px4_msgs/msg/config_overrides.hpp>
+
+namespace px4_sdk
+{
+
+class ModeBase;
+class ModeExecutorBase;
+
+class ConfigOverrides
+{
+public:
+	ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix = "");
+
+	void controlAutoDisarm(bool enabled);
+
+
+private:
+	void update();
+
+	friend class ModeBase;
+	friend class ModeExecutorBase;
+	void deferFailsafes(bool enabled, int timeout_s = 0);
+	void setup(uint8_t type, uint8_t id);
+
+	rclcpp::Node &_node;
+
+	px4_msgs::msg::ConfigOverrides _current_overrides{};
+	rclcpp::Publisher<px4_msgs::msg::ConfigOverrides>::SharedPtr _config_overrides_pub;
+	bool _is_setup{false};
+	bool _require_update_after_setup{false};
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/setpoints.h

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/vehicle_control_mode.hpp>
+#include <px4_msgs/msg/trajectory_setpoint.hpp>
+
+#include <Eigen/Core>
+
+namespace px4_sdk
+{
+
+class ModeBase;
+
+class SetpointSender
+{
+public:
+	enum class SetpointConfigurationResult {
+		Success = 0,
+		Timeout,
+		FailureOther
+	};
+
+	struct SetpointConfiguration {
+		// TODO...
+		bool manual_enabled{false};
+		bool auto_enabled{false};
+		bool rates_enabled{true};
+		bool attitude_enabled{true};
+		bool acceleration_enabled{true};
+		bool velocity_enabled{true};
+		bool position_enabled{true};
+		bool altitude_enabled{true};
+		bool climb_rate_enabled{false};
+	};
+
+	SetpointSender(rclcpp::Node &node, const ModeBase &mode, const std::string &topic_namespace_prefix = "");
+
+	SetpointConfigurationResult configureSetpointsSync(const SetpointConfiguration &config);
+
+	void sendTrajectorySetpoint(const Eigen::Vector3f &velocity);
+
+	// TODO: goto, stop, ...
+
+private:
+	rclcpp::Node &_node;
+	const ModeBase &_mode;
+	SetpointConfiguration _setpoint_configuration{};
+	rclcpp::Publisher<px4_msgs::msg::VehicleControlMode>::SharedPtr _config_control_setpoints_pub;
+	rclcpp::Publisher<px4_msgs::msg::TrajectorySetpoint>::SharedPtr _trajectory_setpoint_pub;
+};
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/include/px4_sdk/components/wait_for_fmu.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   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 <rclcpp/rclcpp.hpp>
+using namespace std::chrono_literals;
+
+namespace px4_sdk
+{
+
+/**
+ * Wait for a heartbeat/status message from the FMU
+ * @return true on success
+ */
+bool waitForFMU(rclcpp::Node &node, rclcpp::Duration timeout = 30s, const std::string &topic_namespace_prefix = "");
+
+} /* namespace px4_sdk */

src/ros2/cpp/px4_sdk_lib/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>px4_sdk_lib</name>
+  <version>0.0.1</version>
+  <description>Library to interface with PX4 from ROS2</description>
+  <maintainer email="beat-kueng@gmx.net">beat</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <buildtool_depend>eigen3_cmake_module</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_export_depend>eigen3_cmake_module</buildtool_export_depend>
+
+  <build_depend>eigen</build_depend>
+  <build_depend>rclcpp</build_depend>
+  <build_depend>px4_msgs</build_depend>
+  <build_export_depend>eigen</build_export_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ros2/cpp/px4_sdk_lib/src/components/health_and_arming_checks.cpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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 "registration.h"
+#include "px4_sdk/components/health_and_arming_checks.h"
+
+#include <cassert>
+
+using namespace std::chrono_literals;
+using namespace px4_sdk;
+
+HealthAndArmingChecks::HealthAndArmingChecks(rclcpp::Node &node, const CheckCallback &check_callback,
+		const std::string &topic_namespace_prefix)
+	: _node(node), _registration(std::make_shared<Registration>(node, topic_namespace_prefix)),
+	  _check_callback(check_callback)
+{
+	_arming_check_reply_pub = _node.create_publisher<px4_msgs::msg::ArmingCheckReply>(
+					  topic_namespace_prefix + "/fmu/in/arming_check_reply", 1);
+
+	_arming_check_request_sub = _node.create_subscription<px4_msgs::msg::ArmingCheckRequest>(
+					    topic_namespace_prefix + "/fmu/out/arming_check_request",
+					    rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::ArmingCheckRequest::UniquePtr msg) {
+
+		RCLCPP_DEBUG_ONCE(_node.get_logger(), "Arming check request (id=%i, only printed once)", msg->request_id);
+
+		if (_registration->registered()) {
+			px4_msgs::msg::ArmingCheckReply reply{};
+			reply.registration_id = _registration->armingCheckId();
+			reply.request_id = msg->request_id;
+			reply.can_arm_and_run = true;
+
+			HealthAndArmingCheckReporter reporter(reply);
+			_check_callback(reporter);
+
+			reply.mode_req_angular_velocity = _mode_requirements.angular_velocity;
+			reply.mode_req_attitude = _mode_requirements.attitude;
+			reply.mode_req_local_alt = _mode_requirements.local_alt;
+			reply.mode_req_local_position = _mode_requirements.local_position;
+			reply.mode_req_local_position_relaxed = _mode_requirements.local_position_relaxed;
+			reply.mode_req_global_position = _mode_requirements.global_position;
+			reply.mode_req_mission = _mode_requirements.mission;
+			reply.mode_req_home_position = _mode_requirements.home_position;
+			reply.mode_req_prevent_arming = _mode_requirements.prevent_arming;
+
+			reply.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+			_arming_check_reply_pub->publish(reply);
+			_check_triggered = true;
+
+		} else {
+			RCLCPP_DEBUG(_node.get_logger(), "...not registered yet");
+		}
+	});
+
+	_watchdog_timer = _node.create_wall_timer(4s, std::bind(&HealthAndArmingChecks::watchdogTimerUpdate, this));
+}
+
+void HealthAndArmingChecks::overrideRegistration(const std::shared_ptr<Registration> &registration)
+{
+	assert(!_registration->registered());
+	_registration = registration;
+}
+
+bool HealthAndArmingChecks::doRegister(const std::string &name)
+{
+	assert(!_registration->registered());
+	RegistrationSettings settings{};
+	settings.name = name;
+	settings.register_arming_check = true;
+	return _registration->doRegister(settings);
+}
+
+void HealthAndArmingChecks::watchdogTimerUpdate()
+{
+	if (_registration->registered()) {
+		if (!_check_triggered && _shutdown_on_timeout) {
+			RCLCPP_FATAL(_node.get_logger(), "Timeout, no request received from FMU, exiting (this can happen on FMU reboots)");
+			rclcpp::shutdown();
+		}
+
+		_check_triggered = false;
+
+	} else {
+		// avoid false positives while unregistered
+		_check_triggered = true;
+	}
+}
+

src/ros2/cpp/px4_sdk_lib/src/components/mode.cpp

@@ -0,0 +1,191 @@
+/****************************************************************************
+ *
+ *   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 "px4_sdk/components/mode.h"
+
+#include "registration.h"
+
+#include <cassert>
+#include <cfloat>
+
+using namespace px4_sdk;
+
+ModeBase::ModeBase(rclcpp::Node &node, const ModeBase::Settings &settings,
+		   const ModeRequirements &requirements, const std::string &topic_namespace_prefix)
+	: _node(node), _registration(std::make_shared<Registration>(node, topic_namespace_prefix)), _settings(settings),
+	  _health_and_arming_checks(node, std::bind(&ModeBase::checkArmingAndRunConditions, this, std::placeholders::_1),
+				    topic_namespace_prefix),
+	  _setpoint_sender(node, *this, topic_namespace_prefix), _config_overrides(node, topic_namespace_prefix)
+{
+	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
+				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
+		if (_registration->registered()) {
+			vehicleStatusUpdated(msg);
+		}
+	});
+	_mode_completed_pub = _node.create_publisher<px4_msgs::msg::ModeCompleted>(
+				      topic_namespace_prefix + "/fmu/in/mode_completed", 1);
+
+	_health_and_arming_checks.setModeRequirements(requirements);
+}
+
+ModeBase::ID_t ModeBase::id() const
+{
+	return _registration->modeId();
+}
+
+void ModeBase::overrideRegistration(const std::shared_ptr<Registration> &registration)
+{
+	assert(!_registration->registered());
+	_health_and_arming_checks.overrideRegistration(registration);
+	_registration = registration;
+}
+
+bool ModeBase::doRegister()
+{
+	assert(!_registration->registered());
+	_health_and_arming_checks.overrideRegistration(_registration);
+	RegistrationSettings settings = getRegistrationSettings();
+	bool ret = _registration->doRegister(settings);
+
+	if (ret) {
+		onRegistered();
+	}
+
+	return ret;
+}
+
+RegistrationSettings ModeBase::getRegistrationSettings() const
+{
+	RegistrationSettings settings{};
+	settings.name = _settings.name;
+	settings.register_arming_check = true;
+	settings.register_mode = true;
+
+	if (_settings.replace_internal_mode != ID_INVALID) {
+		settings.enable_replace_internal_mode = true;
+		settings.replace_internal_mode = _settings.replace_internal_mode;
+	}
+
+	return settings;
+}
+
+void ModeBase::callOnActivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode '%s' activated", _registration->name().c_str());
+	_is_active = true;
+	_completed = false;
+	onActivate();
+
+	if (_setpoint_update_rate_hz > FLT_EPSILON) {
+		updateSetpoint(); // Immediately update
+	}
+
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::callOnDeactivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode '%s' deactivated", _registration->name().c_str());
+	_is_active = false;
+	onDeactivate();
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::updateSetpointUpdateTimer()
+{
+	bool activate = _is_active && _setpoint_update_rate_hz > FLT_EPSILON;
+
+	if (activate) {
+		if (!_setpoint_update_timer) {
+			_setpoint_update_timer = _node.create_wall_timer(std::chrono::milliseconds((long)(1000.f /
+			_setpoint_update_rate_hz)), [this]() { updateSetpoint(); });
+		}
+
+	} else {
+		if (_setpoint_update_timer) {
+			_setpoint_update_timer.reset();
+		}
+	}
+}
+
+void ModeBase::setSetpointUpdateRate(float rate_hz)
+{
+	_setpoint_update_timer.reset();
+	_setpoint_update_rate_hz = rate_hz;
+	updateSetpointUpdateTimer();
+}
+
+void ModeBase::unsubscribeVehicleStatus()
+{
+	_vehicle_status_sub.reset();
+}
+
+void ModeBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg, bool do_not_activate)
+{
+	// Update state
+	_is_armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+	bool is_active = id() == msg->nav_state && (_is_armed || _settings.activate_even_while_disarmed);
+
+	if (_is_active != is_active) {
+		if (is_active) {
+			if (!do_not_activate) {
+				callOnActivate();
+			}
+
+		} else {
+			callOnDeactivate();
+		}
+	}
+}
+
+void ModeBase::completed(Result result)
+{
+	if (_completed) {
+		RCLCPP_DEBUG_ONCE(_node.get_logger(), "Mode '%s': completed was already called", _registration->name().c_str());
+		return;
+	}
+
+	px4_msgs::msg::ModeCompleted mode_completed{};
+	mode_completed.nav_state = (uint8_t)id();
+	mode_completed.result = (uint8_t)result;
+	mode_completed.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_mode_completed_pub->publish(mode_completed);
+	_completed = true;
+}
+
+void ModeBase::onRegistered()
+{
+	_config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE, _registration->modeId());
+}

src/ros2/cpp/px4_sdk_lib/src/components/mode_executor.cpp

@@ -0,0 +1,439 @@
+/****************************************************************************
+ *
+ *   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 "px4_sdk/components/mode_executor.h"
+
+#include "registration.h"
+
+#include <cassert>
+#include <future>
+using namespace std::chrono_literals;
+
+using namespace px4_sdk;
+
+ModeExecutorBase::ModeExecutorBase(rclcpp::Node &node, const ModeExecutorBase::Settings &settings,
+				   ModeBase &owned_mode, const std::string &topic_namespace_prefix)
+	: _node(node), _settings(settings), _owned_mode(owned_mode),
+	  _registration(std::make_shared<Registration>(node, topic_namespace_prefix)),
+	  _current_scheduled_mode(node, topic_namespace_prefix),
+	  _config_overrides(node, topic_namespace_prefix)
+{
+	_vehicle_status_sub = _node.create_subscription<px4_msgs::msg::VehicleStatus>(
+				      topic_namespace_prefix + "/fmu/out/vehicle_status", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleStatus::UniquePtr msg) {
+		if (_registration->registered()) {
+			vehicleStatusUpdated(msg);
+		}
+	});
+
+	_vehicle_command_pub = _node.create_publisher<px4_msgs::msg::VehicleCommand>(
+				       topic_namespace_prefix + "/fmu/in/vehicle_command_mode_executor", 1);
+
+	_vehicle_command_ack_sub = _node.create_subscription<px4_msgs::msg::VehicleCommandAck>(
+					   topic_namespace_prefix + "/fmu/out/vehicle_command_ack", rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::VehicleCommandAck::UniquePtr msg) {});
+}
+
+bool ModeExecutorBase::doRegister()
+{
+	if (_owned_mode._registration->registered()) {
+		RCLCPP_FATAL(_node.get_logger(), "Mode executor %s: mode already registered", _registration->name().c_str());
+	}
+
+	assert(!_registration->registered());
+	_owned_mode.overrideRegistration(_registration);
+	_owned_mode.unsubscribeVehicleStatus();
+	RegistrationSettings settings = _owned_mode.getRegistrationSettings();
+	settings.register_mode_executor = true;
+	settings.activate_mode_immediately = _settings.activate_immediately;
+	bool ret = _registration->doRegister(settings);
+
+	if (ret) {
+		_owned_mode.onRegistered();
+		onRegistered();
+	}
+
+	return ret;
+}
+
+void ModeExecutorBase::onRegistered()
+{
+	_config_overrides.setup(px4_msgs::msg::ConfigOverrides::SOURCE_TYPE_MODE_EXECUTOR, _registration->modeExecutorId());
+}
+
+void ModeExecutorBase::callOnActivate()
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode executor '%s' activated", _registration->name().c_str());
+	_is_in_charge = true;
+	onActivate();
+}
+
+void ModeExecutorBase::callOnDeactivate(DeactivateReason reason)
+{
+	RCLCPP_DEBUG(_node.get_logger(), "Mode executor '%s' deactivated (%i)", _registration->name().c_str(), (int)reason);
+	_current_scheduled_mode.cancel();
+	_current_wait_vehicle_status.cancel();
+	_is_in_charge = false;
+	_was_never_activated = false; // Set on deactivation, so we stay activated for the first time (while disarmed)
+	onDeactivate(reason);
+}
+
+int ModeExecutorBase::id() const
+{
+	return _registration->modeExecutorId();
+}
+
+Result ModeExecutorBase::sendCommandSync(uint32_t command, float param1, float param2, float param3, float param4,
+		float param5, float param6, float param7)
+{
+	// Send command and wait for ack
+	Result result{Result::Rejected};
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = command;
+	cmd.param1 = param1;
+	cmd.param2 = param2;
+	cmd.param3 = param3;
+	cmd.param4 = param4;
+	cmd.param5 = param5;
+	cmd.param6 = param6;
+	cmd.param7 = param7;
+	cmd.source_component = px4_msgs::msg::VehicleCommand::COMPONENT_MODE_EXECUTOR_START + id();
+	cmd.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+
+
+	rclcpp::WaitSet wait_set;
+	wait_set.add_subscription(_vehicle_command_ack_sub);
+
+	bool got_reply = false;
+	auto start_time = std::chrono::steady_clock::now();
+	auto timeout = 200ms;
+	_vehicle_command_pub->publish(cmd);
+
+	while (!got_reply) {
+		auto now = std::chrono::steady_clock::now();
+
+		if (now >= start_time + timeout) {
+			break;
+		}
+
+		auto wait_ret = wait_set.wait(timeout - (now - start_time));
+
+		if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+			px4_msgs::msg::VehicleCommandAck ack;
+			rclcpp::MessageInfo info;
+
+			if (_vehicle_command_ack_sub->take(ack, info)) {
+				if (ack.command == cmd.command && ack.target_component == cmd.source_component) {
+					if (ack.result == px4_msgs::msg::VehicleCommandAck::VEHICLE_CMD_RESULT_ACCEPTED) {
+						result = Result::Success;
+					}
+
+					got_reply = true;
+				}
+
+			} else {
+				RCLCPP_DEBUG(_node.get_logger(), "no message received");
+			}
+
+		} else {
+			RCLCPP_DEBUG(_node.get_logger(), "timeout");
+		}
+	}
+
+	wait_set.remove_subscription(_vehicle_command_ack_sub);
+
+	if (!got_reply) {
+		// We don't expect to run into an ack timeout
+		result = Result::Timeout;
+		RCLCPP_WARN(_node.get_logger(), "Cmd %i: timeout, no ack received", cmd.command);
+	}
+
+	return result;
+}
+
+void ModeExecutorBase::scheduleMode(ModeBase::ID_t mode_id, const CompletedCallback &on_completed)
+{
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_SET_NAV_STATE;
+	cmd.param1 = mode_id;
+	scheduleMode(mode_id, cmd, on_completed);
+}
+
+void ModeExecutorBase::scheduleMode(ModeBase::ID_t mode_id, const px4_msgs::msg::VehicleCommand &cmd,
+				    const CompletedCallback &on_completed)
+{
+	if (!_is_armed) {
+		on_completed(Result::Rejected);
+		return;
+	}
+
+	// If there's already an active mode, cancel it (it will call the callback with a failure result)
+	_current_scheduled_mode.cancel();
+
+	Result result = sendCommandSync(cmd.command, cmd.param1, cmd.param2, cmd.param3, cmd.param4, cmd.param5, cmd.param6,
+					cmd.param7);
+
+	if (result != Result::Success) {
+		on_completed(result);
+		return;
+	}
+
+	// Store the callback and ensure it's called eventually. There's a number of outcomes:
+	// - The mode finishes and publishes the completion result.
+	// - Failsafe is entered or the user switches out. In that case the executor gets deactivated.
+	// - The user switches into the owned mode. In that case the fmu does not deactivate the executor.
+	_current_scheduled_mode.activate(mode_id, on_completed);
+}
+
+void ModeExecutorBase::takeoff(const CompletedCallback &on_completed, float altitude, float heading)
+{
+	px4_msgs::msg::VehicleCommand cmd{};
+	cmd.command = px4_msgs::msg::VehicleCommand::VEHICLE_CMD_NAV_TAKEOFF;
+	cmd.param1 = NAN;
+	cmd.param2 = NAN;
+	cmd.param3 = NAN;
+	cmd.param4 = heading;
+	cmd.param5 = NAN;
+	cmd.param6 = NAN;
+	cmd.param7 = altitude; // TODO: this is AMSL, local (relative to home) would be better
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_TAKEOFF, cmd, on_completed);
+}
+
+void ModeExecutorBase::land(const CompletedCallback &on_completed)
+{
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_LAND, on_completed);
+}
+
+void ModeExecutorBase::rtl(const CompletedCallback &on_completed)
+{
+	scheduleMode(ModeBase::ID_NAVIGATION_STATE_AUTO_RTL, on_completed);
+}
+
+void ModeExecutorBase::arm(const CompletedCallback &on_completed)
+{
+	if (_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	Result result = sendCommandSync(px4_msgs::msg::VehicleCommand::VEHICLE_CMD_COMPONENT_ARM_DISARM, 1.f);
+
+	if (result != Result::Success) {
+		on_completed(result);
+		return;
+	}
+
+	// Wait until our internal state changes to armed
+	_current_wait_vehicle_status.activate(
+	[this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return _is_armed; }, on_completed);
+}
+
+void ModeExecutorBase::waitReadyToArm(const CompletedCallback &on_completed)
+{
+	if (_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Waiting until ready to arm...");
+	_current_wait_vehicle_status.activate([](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return msg->pre_flight_checks_pass; },
+	on_completed);
+}
+
+void ModeExecutorBase::waitUntilDisarmed(const CompletedCallback &on_completed)
+{
+	if (!_is_armed) {
+		on_completed(Result::Success);
+		return;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Waiting until disarmed...");
+	_current_wait_vehicle_status.activate([this](const px4_msgs::msg::VehicleStatus::UniquePtr & msg) { return !_is_armed; },
+	on_completed);
+}
+
+void ModeExecutorBase::vehicleStatusUpdated(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)
+{
+	// Update state
+	const bool was_armed = _is_armed;
+	const ModeBase::ID_t current_mode = (ModeBase::ID_t)msg->nav_state;
+	_is_armed = msg->arming_state == px4_msgs::msg::VehicleStatus::ARMING_STATE_ARMED;
+	const bool wants_to_activate_immediately = _settings.activate_immediately && _was_never_activated;
+	const bool is_in_charge = id() == msg->executor_in_charge && (_is_armed || wants_to_activate_immediately);
+	const bool changed_to_armed = !was_armed && _is_armed;
+
+	if (_is_in_charge != is_in_charge) {
+		if (is_in_charge) {
+			callOnActivate();
+
+		} else {
+			callOnDeactivate(msg->failsafe ? DeactivateReason::FailsafeActivated : DeactivateReason::Other);
+		}
+	}
+
+	if (_is_in_charge && _current_scheduled_mode.active() && _current_scheduled_mode.modeId() == _prev_nav_state
+	    && current_mode == _owned_mode.id() && _prev_nav_state != current_mode) {
+		// If the user switched from the currently scheduled mode to the owned mode, the executor stays in charge.
+		// In order for the executor to restore the correct state, we re-activate it (which also cancels the scheduled
+		// mode)
+		callOnDeactivate(DeactivateReason::Other);
+		callOnActivate();
+	}
+
+
+	if (_is_in_charge && _prev_failsafe_defer_state != msg->failsafe_defer_state
+	    && msg->failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE) {
+		// FMU wants to failsafe, but got deferred -> notify the executor
+		onFailsafeDeferred();
+	}
+
+	_prev_failsafe_defer_state = msg->failsafe_defer_state;
+
+	// Do not activate the mode if we're scheduling another mode. This is only expected to happen for a brief moment,
+	// e.g. when the executor gets activated or right after arming. It thus prevents unnecessary mode activation toggling.
+	bool do_not_activate_mode = (_current_scheduled_mode.active() && _owned_mode.id() != _current_scheduled_mode.modeId())
+				    || (changed_to_armed && _current_wait_vehicle_status.active());
+	// To avoid race conditions and ensure consistent ordering we update vehicle status of the mode directly.
+	_owned_mode.vehicleStatusUpdated(msg, do_not_activate_mode && _is_in_charge);
+
+	_prev_nav_state = current_mode;
+
+	_current_wait_vehicle_status.update(msg);
+}
+
+bool ModeExecutorBase::deferFailsafesSync(bool enabled, int timeout_s)
+{
+	_config_overrides.deferFailsafes(enabled, timeout_s);
+
+	// To avoid race conditions we wait until the FMU sets it if the executor is in charge
+	if (enabled && _is_in_charge && _registration->registered()
+	    && _prev_failsafe_defer_state == px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) {
+		rclcpp::WaitSet wait_set;
+		wait_set.add_subscription(_vehicle_status_sub);
+
+		bool got_message = false;
+		auto start_time = _node.now();
+		const rclcpp::Duration timeout = 1s;
+
+		while (!got_message) {
+			auto now = _node.now();
+
+			if (now >= start_time + timeout) {
+				break;
+			}
+
+			auto wait_ret = wait_set.wait((timeout - (now - start_time)).to_chrono<std::chrono::microseconds>());
+
+			if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+				px4_msgs::msg::VehicleStatus msg;
+				rclcpp::MessageInfo info;
+
+				if (_vehicle_status_sub->take(msg, info)) {
+					if (msg.failsafe_defer_state != px4_msgs::msg::VehicleStatus::FAILSAFE_DEFER_STATE_DISABLED) {
+						got_message = true;
+					}
+
+				} else {
+					RCLCPP_DEBUG(_node.get_logger(), "no message received");
+				}
+
+			} else {
+				RCLCPP_DEBUG(_node.get_logger(), "timeout");
+			}
+		}
+
+		wait_set.remove_subscription(_vehicle_status_sub);
+
+		return got_message;
+	}
+
+	return true;
+}
+
+ModeExecutorBase::ScheduledMode::ScheduledMode(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+{
+	_mode_completed_sub = node.create_subscription<px4_msgs::msg::ModeCompleted>(
+				      topic_namespace_prefix + "/fmu/out/mode_completed", rclcpp::QoS(1).best_effort(),
+	[this, &node](px4_msgs::msg::ModeCompleted::UniquePtr msg) {
+		if (active() && msg->nav_state == (uint8_t)_mode_id) {
+			RCLCPP_DEBUG(node.get_logger(), "Got matching ModeCompleted message, result: %i", msg->result);
+			CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+			reset();
+			on_completed_callback((Result)msg->result); // Call after, as it might trigger new requests
+		}
+	});
+}
+
+void ModeExecutorBase::ScheduledMode::activate(ModeBase::ID_t mode_id, const CompletedCallback &on_completed)
+{
+	assert(!active());
+	_mode_id = mode_id;
+	_on_completed_callback = on_completed;
+}
+
+void ModeExecutorBase::ScheduledMode::cancel()
+{
+	if (active()) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		reset();
+		on_completed_callback(Result::Deactivated); // Call after, as it might trigger new requests
+	}
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::update(const px4_msgs::msg::VehicleStatus::UniquePtr &msg)
+{
+	if (_on_completed_callback && _run_check_callback(msg)) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		_on_completed_callback = nullptr;
+		_run_check_callback = nullptr;
+		on_completed_callback(Result::Success); // Call after, as it might trigger new requests
+	}
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::activate(const RunCheckCallback &run_check_callback,
+		const CompletedCallback &on_completed)
+{
+	assert(!_on_completed_callback);
+	_on_completed_callback = on_completed;
+	_run_check_callback = run_check_callback;
+}
+
+void ModeExecutorBase::WaitForVehicleStatusCondition::cancel()
+{
+	if (_on_completed_callback) {
+		CompletedCallback on_completed_callback(std::move(_on_completed_callback));
+		_on_completed_callback = nullptr;
+		_run_check_callback = nullptr;
+		on_completed_callback(Result::Deactivated); // Call after, as it might trigger new requests
+	}
+}

src/ros2/cpp/px4_sdk_lib/src/components/overrides.cpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   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 "px4_sdk/components/overrides.h"
+
+#include <cassert>
+
+using namespace px4_sdk;
+
+ConfigOverrides::ConfigOverrides(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+	: _node(node)
+{
+	_config_overrides_pub = _node.create_publisher<px4_msgs::msg::ConfigOverrides>(
+					topic_namespace_prefix + "/fmu/in/config_overrides_request", 1);
+}
+
+void ConfigOverrides::controlAutoDisarm(bool enabled)
+{
+	_current_overrides.disable_auto_disarm = !enabled;
+	update();
+}
+
+void ConfigOverrides::deferFailsafes(bool enabled, int timeout_s)
+{
+	_current_overrides.defer_failsafes = enabled;
+	_current_overrides.defer_failsafes_timeout_s = timeout_s;
+	update();
+}
+
+void ConfigOverrides::update()
+{
+	if (_is_setup) {
+		_current_overrides.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_config_overrides_pub->publish(_current_overrides);
+
+	} else {
+		_require_update_after_setup = true;
+	}
+}
+
+void ConfigOverrides::setup(uint8_t type, uint8_t id)
+{
+	assert(!_is_setup);
+	_current_overrides.source_type = type;
+	_current_overrides.source_id = id;
+	_is_setup = true;
+
+	if (_require_update_after_setup) {
+		update();
+		_require_update_after_setup = false;
+	}
+}

src/ros2/cpp/px4_sdk_lib/src/components/registration.cpp

@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   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 "registration.h"
+
+#include <cassert>
+#include <random>
+
+using namespace std::chrono_literals;
+
+Registration::Registration(rclcpp::Node &node, const std::string &topic_namespace_prefix)
+	: _node(node)
+{
+	_register_ext_component_reply_sub = node.create_subscription<px4_msgs::msg::RegisterExtComponentReply>(
+			topic_namespace_prefix + "/fmu/out/register_ext_component_reply",
+			rclcpp::QoS(1).best_effort(),
+	[this](px4_msgs::msg::RegisterExtComponentReply::UniquePtr msg) {
+	});
+
+	_register_ext_component_request_pub = node.create_publisher<px4_msgs::msg::RegisterExtComponentRequest>(
+			topic_namespace_prefix + "/fmu/in/register_ext_component_request", 1);
+
+	_unregister_ext_component_pub = node.create_publisher<px4_msgs::msg::UnregisterExtComponent>(
+						topic_namespace_prefix + "/fmu/in/unregister_ext_component", 1);
+
+	_unregister_ext_component.mode_id = px4_sdk::ModeBase::ID_INVALID;
+}
+
+bool Registration::doRegister(const RegistrationSettings &settings)
+{
+	assert(!_registered);
+	px4_msgs::msg::RegisterExtComponentRequest request{};
+
+	if (settings.name.length() >= request.name.size() || settings.name.length() >= _unregister_ext_component.name.size()) {
+		RCLCPP_ERROR(_node.get_logger(), "Name too long (%i >= %i)", (int)settings.name.length(), (int)request.name.size());
+		return false;
+	}
+
+	RCLCPP_DEBUG(_node.get_logger(), "Registering '%s' (arming check: %i, mode: %i, mode executor: %i)",
+		     settings.name.c_str(),
+		     settings.register_arming_check, settings.register_mode, settings.register_mode_executor);
+
+	strcpy((char *)request.name.data(), settings.name.c_str());
+	request.register_arming_check = settings.register_arming_check;
+	request.register_mode = settings.register_mode;
+	request.register_mode_executor = settings.register_mode_executor;
+	request.enable_replace_internal_mode = settings.enable_replace_internal_mode;
+	request.replace_internal_mode = settings.replace_internal_mode;
+	request.activate_mode_immediately = settings.activate_mode_immediately;
+
+	std::random_device rd;
+	std::mt19937 gen(rd());
+	std::uniform_int_distribution<uint64_t> distrib{};
+	request.request_id = distrib(gen);
+
+	// wait for subscription, it might take a while initially...
+	for (int i = 0; i < 100; ++i) {
+		if (_register_ext_component_request_pub->get_subscription_count() > 0) {
+			RCLCPP_DEBUG(_node.get_logger(), "Subscriber found, continuing");
+			break;
+		}
+
+		usleep(100000);
+	}
+
+	// send request and wait for response
+	rclcpp::WaitSet wait_set;
+	wait_set.add_subscription(_register_ext_component_reply_sub);
+
+	bool got_reply = false;
+
+	for (int retries = 0; retries < 5 && !got_reply; ++retries) {
+		request.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_register_ext_component_request_pub->publish(request);
+
+		// wait for publisher, it might take a while initially...
+		for (int i = 0; i < 100 && retries == 0; ++i) {
+			if (_register_ext_component_reply_sub->get_publisher_count() > 0) {
+				RCLCPP_DEBUG(_node.get_logger(), "Publisher found, continuing");
+				break;
+			}
+
+			usleep(100000);
+		}
+
+		auto start_time = std::chrono::steady_clock::now();
+		auto timeout = 300ms;
+
+		while (!got_reply) {
+			auto now = std::chrono::steady_clock::now();
+
+			if (now >= start_time + timeout) {
+				break;
+			}
+
+			auto wait_ret = wait_set.wait(timeout - (now - start_time));
+
+			if (wait_ret.kind() == rclcpp::WaitResultKind::Ready) {
+				px4_msgs::msg::RegisterExtComponentReply reply;
+				rclcpp::MessageInfo info;
+
+				if (_register_ext_component_reply_sub->take(reply, info)) {
+					reply.name.back() = '\0';
+
+					if (strcmp((const char *) reply.name.data(), settings.name.c_str()) == 0 &&
+					    request.request_id == reply.request_id) {
+						RCLCPP_DEBUG(_node.get_logger(), "Got RegisterExtComponentReply");
+
+						if (reply.success) {
+							_unregister_ext_component.arming_check_id = reply.arming_check_id;
+							_unregister_ext_component.mode_id = reply.mode_id;
+							_unregister_ext_component.mode_executor_id = reply.mode_executor_id;
+							strcpy((char *) _unregister_ext_component.name.data(), settings.name.c_str());
+							_registered = true;
+
+						} else {
+							RCLCPP_ERROR(_node.get_logger(), "Registration failed");
+						}
+
+						got_reply = true;
+					}
+
+				} else {
+					RCLCPP_INFO(_node.get_logger(), "no message received");
+				}
+
+			} else {
+				RCLCPP_INFO(_node.get_logger(), "timeout");
+			}
+		}
+	}
+
+	wait_set.remove_subscription(_register_ext_component_reply_sub);
+
+	return _registered;
+}
+
+void Registration::doUnregister()
+{
+	if (_registered) {
+		RCLCPP_DEBUG(_node.get_logger(), "Unregistering");
+		_unregister_ext_component.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+		_unregister_ext_component_pub->publish(_unregister_ext_component);
+		_registered = false;
+	}
+}

src/ros2/cpp/px4_sdk_lib/src/components/registration.h

@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 <cstdint>
+#include <string>
+
+#include <rclcpp/rclcpp.hpp>
+#include <px4_msgs/msg/register_ext_component_request.hpp>
+#include <px4_msgs/msg/register_ext_component_reply.hpp>
+#include <px4_msgs/msg/unregister_ext_component.hpp>
+
+#include <px4_sdk/components/mode.h>
+
+struct RegistrationSettings {
+	std::string name;
+	bool register_arming_check{false};
+	bool register_mode{false};
+	bool register_mode_executor{false};
+
+	bool enable_replace_internal_mode{false};
+	px4_sdk::ModeBase::ID_t replace_internal_mode{};
+	bool activate_mode_immediately{false};
+};
+
+class Registration
+{
+public:
+
+	Registration(rclcpp::Node &node, const std::string &topic_namespace_prefix = "");
+	~Registration()
+	{
+		doUnregister();
+	}
+
+	bool doRegister(const RegistrationSettings &settings);
+	void doUnregister();
+
+	bool registered() const { return _registered; }
+
+	int armingCheckId() const { return _unregister_ext_component.arming_check_id; }
+	px4_sdk::ModeBase::ID_t modeId() const { return _unregister_ext_component.mode_id; }
+	int modeExecutorId() const { return _unregister_ext_component.mode_executor_id; }
+
+	std::string name() const { return (const char *)_unregister_ext_component.name.data(); }
+private:
+	rclcpp::Subscription<px4_msgs::msg::RegisterExtComponentReply>::SharedPtr _register_ext_component_reply_sub;
+	rclcpp::Publisher<px4_msgs::msg::RegisterExtComponentRequest>::SharedPtr _register_ext_component_request_pub;
+	rclcpp::Publisher<px4_msgs::msg::UnregisterExtComponent>::SharedPtr _unregister_ext_component_pub;
+
+	bool _registered{false};
+	px4_msgs::msg::UnregisterExtComponent _unregister_ext_component{};
+	rclcpp::Node &_node;
+};
+

src/ros2/cpp/px4_sdk_lib/src/components/setpoints.cpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   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 "px4_sdk/components/setpoints.h"
+#include "px4_sdk/components/mode.h"
+
+#include <cassert>
+
+using namespace px4_sdk;
+
+SetpointSender::SetpointSender(rclcpp::Node &node, const ModeBase &mode, const std::string &topic_namespace_prefix)
+	: _node(node), _mode(mode)
+{
+	_config_control_setpoints_pub = node.create_publisher<px4_msgs::msg::VehicleControlMode>(
+						topic_namespace_prefix + "/fmu/in/config_control_setpoints", 1);
+
+	_trajectory_setpoint_pub = node.create_publisher<px4_msgs::msg::TrajectorySetpoint>(
+					   topic_namespace_prefix + "/fmu/in/trajectory_setpoint", 1);
+}
+
+SetpointSender::SetpointConfigurationResult SetpointSender::configureSetpointsSync(const SetpointConfiguration &config)
+{
+	assert(_mode.id() != ModeBase::ID_INVALID); // Ensure mode is registered
+
+	px4_msgs::msg::VehicleControlMode control_mode{};
+	control_mode.source_id = (uint8_t)_mode.id();
+	control_mode.flag_control_manual_enabled = config.manual_enabled;
+	control_mode.flag_control_auto_enabled = config.auto_enabled;
+	control_mode.flag_control_rates_enabled = config.rates_enabled;
+	control_mode.flag_control_attitude_enabled = config.attitude_enabled;
+	control_mode.flag_control_acceleration_enabled = config.acceleration_enabled;
+	control_mode.flag_control_velocity_enabled = config.velocity_enabled;
+	control_mode.flag_control_position_enabled = config.position_enabled;
+	control_mode.flag_control_altitude_enabled = config.altitude_enabled;
+	control_mode.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_config_control_setpoints_pub->publish(control_mode);
+	// TODO: wait for feedback from FMU
+
+	_setpoint_configuration = config;
+	return SetpointConfigurationResult::Success;
+}
+
+void SetpointSender::sendTrajectorySetpoint(const Eigen::Vector3f &velocity)
+{
+	// TODO: check if configured setpoints match
+
+	// TODO: add mode id to setpoint
+
+	px4_msgs::msg::TrajectorySetpoint sp{};
+	sp.yaw = NAN;
+	sp.yawspeed = NAN;
+	sp.position[0] = sp.position[1] = sp.position[2] = NAN; // TODO ...
+	sp.acceleration[0] = sp.acceleration[1] = sp.acceleration[2] = NAN;
+	sp.velocity[0] = velocity(0);
+	sp.velocity[1] = velocity(1);
+	sp.velocity[2] = velocity(2);
+	sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000;
+	_trajectory_setpoint_pub->publish(sp);
+}