Merge branch 'main' of github.com:PX4/PX4-Autopilot into pr-fw_ctrl_api

.github/workflows/dev_container.yml

@@ -17,7 +17,7 @@ on:
 jobs:
   build:
     name: Build and Push Container
-    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
     steps:
       - uses: actions/checkout@v4
         with:
@@ -52,12 +52,7 @@ jobs:
             ghcr.io/PX4/px4-dev
             ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
           tags: |
-            type=semver,pattern={{version}}
+            type=raw,enable=true,value=${{ steps.px4-tag.outputs.tag }},priority=1000
-            type=semver,pattern={{major}}.{{minor}}
-            type=semver,pattern={{major}}
-            type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
-            type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
-            type=ref,event=branch,suffix=,priority=500
 
       - name: Set up Docker Buildx
         uses: docker/setup-buildx-action@v3

ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx

@@ -16,10 +16,6 @@ param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 
-# disable some checks to allow to fly:
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 # Square quadrotor X PX4 numbering
 param set-default CA_ROTOR_COUNT 4
 param set-default CA_ROTOR0_PX 1

ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane

@@ -16,12 +16,6 @@ param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
 param set-default SENS_EN_ARSPDSIM 1
 
-# disable some checks to allow to fly:
-# - with usb
-param set-default CBRK_USB_CHK 197848
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 param set-default SIH_T_MAX 6
 param set-default SIH_MASS 0.3
 param set-default SIH_IXX 0.00402
@@ -33,18 +27,19 @@ param set-default SIH_KDV 0.2
 param set-default SIH_VEHICLE_TYPE 1 	# sih as fixed wing
 param set-default RWTO_TKOFF 1  # enable takeoff from runway (as opposed to launched)
 
-
 param set-default CA_AIRFRAME 1
-
 param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
 
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
 param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TRQ_R 1
-param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
 param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS1_TYPE 3  # elevator
 param set-default CA_SV_CS2_TRQ_Y 1
-param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
 param set-default PWM_MAIN_FUNC1 201
 param set-default PWM_MAIN_FUNC2 202
 param set-default PWM_MAIN_FUNC3 203

ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert

@@ -28,10 +28,6 @@ param set-default MC_PITCH_P 5
 
 param set-default MAV_TYPE 19
 
-# disable some checks to allow to fly:
-# - without real battery
-param set-default CBRK_SUPPLY_CHK 894281
-
 param set-default SIH_T_MAX 2
 param set-default SIH_Q_MAX 0.0165
 param set-default SIH_MASS 0.2

ROMFS/px4fmu_common/init.d-posix/airframes/10043_sihsim_standard_vtol

@@ -0,0 +1,96 @@
+#!/bin/sh
+#
+# @name SIH Standard VTOL
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Ailerons (single channel)
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol}
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+param set-default EKF2_GPS_DELAY 0
+
+param set-default VT_TYPE 2
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR0_PX 0.2
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PX -0.2
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR2_PX 0.2
+param set-default CA_ROTOR2_PY -0.2
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.2
+param set-default CA_ROTOR3_PY 0.2
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_PX -0.3
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 1
+param set-default CA_ROTOR4_AZ 0
+
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3  # elevator
+param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+param set-default FW_AIRSPD_MAX 12
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 201
+param set-default PWM_MAIN_FUNC6 202
+param set-default PWM_MAIN_FUNC7 203
+param set-default PWM_MAIN_FUNC8 105
+
+param set-default MAV_TYPE 22
+
+# set SYS_HITL to 2 to start the SIH and avoid sensors startup
+# param set-default SYS_HITL 2
+
+param set-default SENS_DPRES_OFF 0.001
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.2
+
+# sih as standard vtol
+param set SIH_VEHICLE_TYPE 3

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -109,6 +109,7 @@ px4_add_romfs_files(
 	10040_sihsim_quadx
 	10041_sihsim_airplane
 	10042_sihsim_xvert
+	10043_sihsim_standard_vtol
 
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

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

@@ -4,6 +4,9 @@
 # Simulator IMU data provided at 250 Hz
 param set-default IMU_INTEG_RATE 250
 
+# For simulation, allow registering modes while armed for developer convenience
+param set-default COM_MODE_ARM_CHK 1
+
 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
 
 	echo "INFO  [init] SIH simulator"

ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil

@@ -5,6 +5,15 @@
 # @type Standard VTOL
 # @class VTOL
 #
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Aileron
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
 # @maintainer Roman Bapst <roman@auterion.com>
 #
 # @board px4_fmu-v2 exclude

ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil

@@ -17,15 +17,16 @@ param set UAVCAN_ENABLE 0
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
-param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_CS0_TRQ_R -0.5
-param set-default CA_SV_CS0_TYPE 1
-param set-default CA_SV_CS1_TRQ_P 1
-param set-default CA_SV_CS1_TYPE 3
-param set-default CA_SV_CS2_TRQ_Y 1
-param set-default CA_SV_CS2_TYPE 4
 
-param set HIL_ACT_REV 1
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3  # elevator
+param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
 param set HIL_ACT_FUNC1 201
 param set HIL_ACT_FUNC2 202
 param set HIL_ACT_FUNC3 203

ROMFS/px4fmu_common/init.d/airframes/1103_standard_vtol_sih.hil

@@ -0,0 +1,92 @@
+#!/bin/sh
+#
+# @name SIH Standard VTOL QuadPlane
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 MC motor front right
+# @output Motor2 MC motor back left
+# @output Motor3 MC motor front left
+# @output Motor4 MC motor back right
+# @output Motor5 Forward thrust motor
+# @output Servo1 Ailerons (single channel)
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set UAVCAN_ENABLE 0
+
+param set-default VT_TYPE 2
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR0_PX 0.2
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PX -0.2
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR2_PX 0.2
+param set-default CA_ROTOR2_PY -0.2
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.2
+param set-default CA_ROTOR3_PY 0.2
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_PX -0.3
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 1
+param set-default CA_ROTOR4_AZ 0
+
+# SIH for now hardcodes this configuration which we need to match in the airframe files.
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 1
+param set-default CA_SV_CS0_TYPE 15  # single channel aileron
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3  # elevator
+param set-default CA_SV_CS2_TRQ_Y 1
+param set-default CA_SV_CS2_TYPE 4  # rudder
+
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+param set-default FW_AIRSPD_MAX 12
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 201
+param set-default HIL_ACT_FUNC6 202
+param set-default HIL_ACT_FUNC7 203
+param set-default HIL_ACT_FUNC8 105
+
+param set-default MAV_TYPE 22
+
+# set SYS_HITL to 2 to start the SIH and avoid sensors startup
+param set-default SYS_HITL 2
+
+# disable some checks to allow to fly:
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+
+param set-default SENS_DPRES_OFF 0.001
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.2
+
+# sih as standard vtol
+param set SIH_VEHICLE_TYPE 3

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
 		1100_rc_quad_x_sih.hil
 		1101_rc_plane_sih.hil
 		1102_tailsitter_duo_sih.hil
+		1103_standard_vtol_sih.hil
 	)
 endif()
 

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -24,7 +24,6 @@ param set-default COM_DISARM_PRFLT -1
 
 param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
-param set-default EKF2_GPS_CHECK 21
 param set-default EKF2_MAG_ACCLIM 0
 param set-default EKF2_REQ_EPH 10
 param set-default EKF2_REQ_EPV 10

ROMFS/px4fmu_common/init.d/rc.heli_defaults

@@ -12,6 +12,7 @@ param set-default MAV_TYPE 4
 
 param set-default COM_PREARM_MODE 2
 param set-default COM_SPOOLUP_TIME 10
+param set-default COM_DISARM_PRFLT 60
 
 # No need for minimum collective pitch (or airmode) to keep torque authority
 param set-default MPC_MANTHR_MIN 0

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -21,3 +21,6 @@ param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10
 
 param set-default GPS_UBX_DYNMODEL 6
+
+# lower RNG_FOG since MC are expected to fly closer over obstacles
+param set-default EKF2_RNG_FOG 1.0

ROMFS/px4fmu_common/init.d/rcS

@@ -302,6 +302,75 @@ else
 		. $FCONFIG
 	fi
 
+
+	#
+	# Sensors System (start before Commander so Preflight checks are properly run).
+	#
+	if param greater SYS_HITL 0
+	then
+		sensors start -h
+
+		# disable GPS
+		param set GPS_1_CONFIG 0
+
+		# start the simulator in hardware if needed
+		if param compare SYS_HITL 2
+		then
+			simulator_sih start
+			sensor_baro_sim start
+			sensor_mag_sim start
+			sensor_gps_sim start
+			sensor_agp_sim start
+		fi
+
+	else
+		#
+		# board sensors: rc.sensors
+		#
+		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
+		if [ -f $BOARD_RC_SENSORS ]
+		then
+			echo "Board sensors: ${BOARD_RC_SENSORS}"
+			. $BOARD_RC_SENSORS
+		fi
+		unset BOARD_RC_SENSORS
+
+		. ${R}etc/init.d/rc.sensors
+
+		if param compare -s BAT1_SOURCE 2
+		then
+			esc_battery start
+		fi
+
+		if ! param compare BAT1_SOURCE 1
+		then
+			battery_status start
+		fi
+
+		sensors start
+	fi
+
+	#
+	# state estimator selection
+	#
+	if param compare -s EKF2_EN 1
+	then
+		ekf2 start &
+	fi
+
+	if param compare -s LPE_EN 1
+	then
+		local_position_estimator start
+	fi
+
+	if param compare -s ATT_EN 1
+	then
+		attitude_estimator_q start
+	fi
+
+	#
+	# px4io
+	#
 	if px4io supported
 	then
 	# Check if PX4IO present and update firmware if needed.
@@ -369,79 +438,24 @@ else
 	fi
 
 	#
-	# Sensors System (start before Commander so Preflight checks are properly run).
+	# Commander
-	# Commander needs to be this early for in-air-restarts.
 	#
 	if param greater SYS_HITL 0
 	then
+		commander start -h
+
 		if ! pwm_out_sim start -m hil
 		then
 			tune_control play error
 		fi
 
-		sensors start -h
-		commander start -h
-		# disable GPS
-		param set GPS_1_CONFIG 0
-
-		# start the simulator in hardware if needed
-		if param compare SYS_HITL 2
-		then
-			simulator_sih start
-			sensor_baro_sim start
-			sensor_mag_sim start
-			sensor_gps_sim start
-			sensor_agp_sim start
-		fi
-
 	else
-		#
-		# board sensors: rc.sensors
-		#
-		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
-		if [ -f $BOARD_RC_SENSORS ]
-		then
-			echo "Board sensors: ${BOARD_RC_SENSORS}"
-			. $BOARD_RC_SENSORS
-		fi
-		unset BOARD_RC_SENSORS
-
-		. ${R}etc/init.d/rc.sensors
-
-		if param compare -s BAT1_SOURCE 2
-		then
-			esc_battery start
-		fi
-
-		if ! param compare BAT1_SOURCE 1
-		then
-			battery_status start
-		fi
-
-		sensors start
 		commander start
 
 		dshot start
 		pwm_out start
 	fi
 
-	#
-	# state estimator selection
-	if param compare -s EKF2_EN 1
-	then
-		ekf2 start &
-	fi
-
-	if param compare -s LPE_EN 1
-	then
-		local_position_estimator start
-	fi
-
-	if param compare -s ATT_EN 1
-	then
-		attitude_estimator_q start
-	fi
-
 	#
 	# Configure vehicle type specific parameters.
 	# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

Tools/px_uploader.py

@@ -189,7 +189,7 @@ class uploader:
     GET_CHIP        = b'\x2c'     # rev5+  , get chip version
     SET_BOOT_DELAY  = b'\x2d'     # rev5+  , set boot delay
     GET_CHIP_DES    = b'\x2e'     # rev5+  , get chip description in ASCII
-    GET_VERSION     = b'\x2f'     # rev5+  , get chip description in ASCII
+    GET_VERSION     = b'\x2f'     # rev5+  , get bootloader version in ASCII
     CHIP_FULL_ERASE = b'\x40'     # full erase of flash, rev6+
     MAX_DES_LENGTH  = 20
 
@@ -201,7 +201,6 @@ class uploader:
     INFO_BOARD_ID   = b'\x02'        # board type
     INFO_BOARD_REV  = b'\x03'        # board revision
     INFO_FLASH_SIZE = b'\x04'        # max firmware size in bytes
-    BL_VERSION      = b'\x07'        # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars)
 
     PROG_MULTI_MAX  = 252            # protocol max is 255, must be multiple of 4
     READ_MULTI_MAX  = 252            # protocol max is 255

boards/ark/fmu-v6x/init/rc.board_defaults

@@ -3,6 +3,8 @@
 # board specific defaults
 #------------------------------------------------------------------------------
 
+param set-default EKF2_MULTI_IMU 0
+
 # Mavlink ethernet (CFG 1000)
 param set-default MAV_2_CONFIG 1000
 param set-default MAV_2_BROADCAST 1
@@ -17,6 +19,7 @@ param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1
 
 param set-default SENS_EN_THERMAL 1
+param set-default SENS_IMU_MODE 1
 param set-default SENS_IMU_TEMP 10.0
 #param set-default SENS_IMU_TEMP_FF 0.0
 #param set-default SENS_IMU_TEMP_I 0.025

boards/ark/fmu-v6x/nuttx-config/nsh/defconfig

@@ -145,10 +145,11 @@ CONFIG_NETDB_DNSCLIENT=y
 CONFIG_NETDB_DNSCLIENT_ENTRIES=8
 CONFIG_NETDB_DNSSERVER_NOADDR=y
 CONFIG_NETDEV_PHY_IOCTL=y
-CONFIG_NETINIT_DHCPC=y
+CONFIG_NETMAN_FALLBACK_IPADDR=0xC0A80004
 CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0xA290AFE
+CONFIG_NETINIT_IPADDR=0xC0A80004
-CONFIG_NETINIT_DRIPADDR=0xA290AFE
+CONFIG_NETINIT_DNSIPADDR=0xC0A800FE
+CONFIG_NETINIT_DRIPADDR=0xC0A80001
 CONFIG_NETINIT_MONITOR=y
 CONFIG_NETINIT_THREAD=y
 CONFIG_NETINIT_THREAD_PRIORITY=49

boards/ark/fpv/default.px4board

@@ -16,9 +16,9 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_DRIVERS_IMU_MURATA_SCH16T=y
-CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
+CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -37,6 +37,7 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y

boards/ark/pi6x/init/rc.board_defaults

@@ -32,10 +32,11 @@ then
 	param set-default SENS_TEMP_ID 2490378
 fi
 
-param set-default EKF2_MULTI_IMU 2
+param set-default EKF2_MULTI_IMU 0
 param set-default EKF2_OF_CTRL 1
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
 param set-default EKF2_RNG_QLTY_T 0.1
 
 param set-default SENS_FLOW_RATE 150
+param set-default SENS_IMU_MODE 1

boards/bitcraze/crazyflie/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/bitcraze/crazyflie21/default.px4board

@@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/cuav/7-nano/src/board_config.h

@@ -352,6 +352,7 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT,	        \
 		GPIO_SAFETY_SWITCH_IN,			\
 		GPIO_nARMED,				\
+		GPIO_PWM_LEVEL_CONTROL,			\
 	}
 
 #define BOARD_ENABLE_CONSOLE_BUFFER

boards/diatone/mamba-f405-mk2/default.px4board

@@ -36,7 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

boards/flywoo/gn-f405/default.px4board

@@ -27,7 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

boards/holybro/kakutef7/default.px4board

@@ -23,6 +23,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set
@@ -40,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/holybro/kakuteh7/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/holybro/kakuteh7mini/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/holybro/kakuteh7v2/default.px4board

@@ -34,6 +34,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set

boards/micoair/h743-aio/default.px4board

@@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
-CONFIG_COMMON_INS=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PPS_CAPTURE=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/micoair/h743-v2/default.px4board

@@ -11,21 +11,25 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_GOERTEK_SPL06=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
-CONFIG_COMMON_INS=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PPS_CAPTURE=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TAP_ESC=y
-CONFIG_DRIVERS_UAVCAN=y
-CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
+CONFIG_DRIVERS_UAVCAN=y
+CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y

boards/micoair/h743-v2/nuttx-config/bootloader/defconfig

@@ -35,7 +35,6 @@ CONFIG_CDCACM_PRODUCTID=0x004b
 CONFIG_CDCACM_PRODUCTSTR="MicoAir743v2"
 CONFIG_CDCACM_RXBUFSIZE=6000
 CONFIG_CDCACM_TXBUFSIZE=12000
-#TODO：ally for VENDOR ID in the future
 CONFIG_CDCACM_VENDORID=0x3162
 CONFIG_CDCACM_VENDORSTR="MicoAir"
 CONFIG_DEBUG_FULLOPT=y

boards/micoair/h743-v2/nuttx-config/nsh/defconfig

@@ -196,17 +196,17 @@ CONFIG_STM32H7_SDMMC1=y
 CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
 CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
 CONFIG_STM32H7_SPI2=y
-CONFIG_STM32H7_SPI3=y
 CONFIG_STM32H7_SPI2_DMA=y
 CONFIG_STM32H7_SPI2_DMA_BUFFER=2048
+CONFIG_STM32H7_SPI3=y
 CONFIG_STM32H7_SPI3_DMA=y
 CONFIG_STM32H7_SPI3_DMA_BUFFER=2048
 CONFIG_STM32H7_SPI_DMATHRESHOLD=8
+CONFIG_STM32H7_TIM15=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
 CONFIG_STM32H7_TIM8=y
-CONFIG_STM32H7_TIM15=y
 CONFIG_STM32H7_UART4=y
 CONFIG_STM32H7_UART5=y
 CONFIG_STM32H7_UART7=y
@@ -222,25 +222,18 @@ CONFIG_STM32H7_USART_SWAP=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
-CONFIG_UART4_BAUD=115200
 CONFIG_UART4_RXBUFSIZE=600
 CONFIG_UART4_TXBUFSIZE=800
-CONFIG_UART5_BAUD=115200
 CONFIG_UART5_RXBUFSIZE=600
 CONFIG_UART5_TXBUFSIZE=800
-CONFIG_UART7_BAUD=115200
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_TXBUFSIZE=800
-CONFIG_UART8_BAUD=115200
 CONFIG_UART8_RXBUFSIZE=600
 CONFIG_UART8_TXBUFSIZE=800
-CONFIG_USART1_BAUD=115200
 CONFIG_USART1_RXBUFSIZE=1200
 CONFIG_USART1_TXBUFSIZE=1500
-CONFIG_USART2_BAUD=115200
 CONFIG_USART2_RXBUFSIZE=600
 CONFIG_USART2_TXBUFSIZE=800
-CONFIG_USART3_BAUD=115200
 CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_TXBUFSIZE=800
 CONFIG_USART6_BAUD=57600

boards/micoair/h743/default.px4board

@@ -10,11 +10,15 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_AUAV=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ETS=y
+CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_MS4515=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI270=y
-CONFIG_COMMON_INS=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_PPS_CAPTURE=y
 CONFIG_DRIVERS_PWM_OUT=y

boards/modalai/fc-v1/default.px4board

@@ -43,6 +43,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y

boards/nxp/tropic-community/default.px4board

@@ -4,7 +4,7 @@ CONFIG_BOARD_ETHERNET=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
-CONFIG_BOARD_PARAM_FILE="/fs/microsd/mtd_params"
+CONFIG_BOARD_PARAM_FILE="/fs/nor/mtd_params"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y

boards/nxp/tropic-community/nuttx-config/nsh/defconfig

@@ -60,6 +60,8 @@ CONFIG_FS_BINFS=y
 CONFIG_FS_CROMFS=y
 CONFIG_FS_FAT=y
 CONFIG_FS_FATTIME=y
+CONFIG_FS_LITTLEFS=y
+CONFIG_FS_LITTLEFS_PROGRAM_SIZE_FACTOR=2
 CONFIG_FS_PROCFS=y
 CONFIG_FS_PROCFS_MAX_TASKS=64
 CONFIG_FS_PROCFS_REGISTER=y
@@ -80,6 +82,8 @@ CONFIG_IMXRT_ENET_NTXBUFFERS=8
 CONFIG_IMXRT_ENET_PHYINIT=y
 CONFIG_IMXRT_FLEXCAN3=y
 CONFIG_IMXRT_FLEXCAN_TXMB=1
+CONFIG_IMXRT_FLEXSPI1=y
+CONFIG_IMXRT_FLEXSPI1_XIP=y
 CONFIG_IMXRT_GPIO1_0_15_IRQ=y
 CONFIG_IMXRT_GPIO1_16_31_IRQ=y
 CONFIG_IMXRT_GPIO2_0_15_IRQ=y
@@ -116,7 +120,7 @@ CONFIG_INTELHEX_BINARY=y
 CONFIG_IOB_NBUFFERS=24
 CONFIG_IOB_THROTTLE=0
 CONFIG_IPCFG_BINARY=y
-CONFIG_IPCFG_PATH="/fs/microsd"
+CONFIG_IPCFG_PATH="/fs/nor"
 CONFIG_LIBC_MAX_EXITFUNS=1
 CONFIG_LIBC_STRERROR=y
 CONFIG_LPI2C1_DMA=y

boards/nxp/tropic-community/nuttx-config/scripts/itcm_static_functions.ld

@@ -117,3 +117,19 @@
 *(.text.ipv4_input)
 *(.text.work_thread)
 *(.text.work_queue)
+
+/* Flash Storage */
+*(.text.imxrt_flexspi_transfer_blocking)
+*(.text.imxrt_flexspi_transfer_blocking_private)
+*(.text.imxrt_flexspi_write_blocking)
+*(.text.imxrt_flexspi_read_blocking)
+*(.text.imxrt_flexspi_check_and_clear_error)
+*(.text.imxrt_flexspi_get_bus_idle_status)
+*(.text.imxrt_flexspi_configure_prefetch)
+*(.text.imxrt_flexspi_configure_prefetch_private)
+*(.text.imxrt_flexspi_storage_write_enable)
+*(.text.imxrt_flexspi_storage_wait_bus_busy)
+*(.text.imxrt_flexspi_storage_read_status)
+*(.text.imxrt_flexspi_storage_erase)
+*(.text.imxrt_flexspi_storage_bwrite)
+*(.text.imxrt_flexspi_storage_page_program)

boards/nxp/tropic-community/nuttx-config/scripts/script.ld

@@ -22,7 +22,7 @@
 
 MEMORY
 {
-  flash (rx)  : ORIGIN = 0x60000000, LENGTH = 7936K
+  flash (rx)  : ORIGIN = 0x60000000, LENGTH = 7936K - 128K
   sram  (rwx) : ORIGIN = 0x20200000, LENGTH = 512K
   itcm  (rwx) : ORIGIN = 0x00000000, LENGTH = 384K
   dtcm  (rwx) : ORIGIN = 0x20000000, LENGTH = 128K

boards/nxp/tropic-community/src/CMakeLists.txt

@@ -41,8 +41,13 @@ px4_add_library(drivers_board
 	usb.c
 	imxrt_flexspi_nor_boot.c
 	imxrt_flexspi_nor_flash.c
+	imxrt_flexspi_storage.c
+	imxrt_ocram_initialize.c
 )
 
+# Force compiler not to use builtin functions (like memcpy)
+# to optimize for loops in init.c (imxrt_ocram_initialize)
+set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
 
 target_link_libraries(drivers_board
 	PRIVATE

boards/nxp/tropic-community/src/board_config.h

@@ -326,6 +326,8 @@ extern void fmurt1062_timer_initialize(void);
 
 #include <px4_platform_common/board_common.h>
 
+int imxrt_flexspi_storage_initialize(void);
+
 #endif /* __ASSEMBLY__ */
 
 __END_DECLS

boards/nxp/tropic-community/src/imxrt_flexspi_storage.c

@@ -0,0 +1,614 @@
+/****************************************************************************
+ * boards/px4/fmu-v6xrt/src/imxrt_flexspi_storage.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/kmalloc.h>
+#include <nuttx/signal.h>
+#include <nuttx/fs/fs.h>
+#include <nuttx/mtd/mtd.h>
+
+#include <px4_platform_common/px4_mtd.h>
+#include "px4_log.h"
+
+#include "imxrt_flexspi.h"
+#include "board_config.h"
+#include "hardware/imxrt_pinmux.h"
+
+#ifdef CONFIG_IMXRT_FLEXSPI
+
+/* Used sectors must be multiple of the flash block size
+ * i.e. W25Q32JV has a block size of 64KB
+*/
+
+#define NOR_USED_SECTORS  (0x20U)   /* 32 * 4KB = 128KB */
+#define NOR_TOTAL_SECTORS (0x0800U)
+#define NOR_PAGE_SIZE     (0x0100U) /* 256 bytes */
+#define NOR_SECTOR_SIZE   (0x1000U) /* 4KB */
+#define NOR_START_SECTOR  (NOR_TOTAL_SECTORS - NOR_USED_SECTORS)
+#define NOR_START_PAGE    ((NOR_START_SECTOR * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE)
+#define NOR_STORAGE_ADDR  (IMXRT_FLEXCIPHER_BASE + NOR_START_SECTOR * NOR_SECTOR_SIZE)
+
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+
+enum {
+	/* SPI instructions */
+
+	READ_FAST = 0,
+	READ_STATUS_REG = 1,
+	WRITE_STATUS_REG = 3,
+	WRITE_ENABLE = 4,
+	SECTOR_ERASE_4K = 5,
+	READ_FAST_QUAD_OUTPUT = 6,
+	PAGE_PROGRAM_QUAD_INPUT = 7,
+	PAGE_PROGRAM = 9,
+	CHIP_ERASE = 11,
+};
+
+static const uint32_t g_flexspi_nor_lut[][4] = {
+	[READ_FAST] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0xeb,
+				FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_4PAD, 0x18),
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x06,
+				FLEXSPI_COMMAND_READ_SDR,  FLEXSPI_4PAD, 0x04),
+	},
+
+	[READ_STATUS_REG] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x05,
+				FLEXSPI_COMMAND_READ_SDR,  FLEXSPI_1PAD, 0x04),
+	},
+
+	[WRITE_STATUS_REG] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x01,
+				FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04),
+	},
+
+	[WRITE_ENABLE] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x06,
+				FLEXSPI_COMMAND_STOP,      FLEXSPI_1PAD, 0),
+	},
+
+	[SECTOR_ERASE_4K] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x20,
+				FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
+	},
+
+	[CHIP_ERASE] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0xc7,
+				FLEXSPI_COMMAND_STOP,      FLEXSPI_1PAD, 0),
+	},
+
+	[PAGE_PROGRAM] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x02,
+				FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_1PAD, 0x04,
+				FLEXSPI_COMMAND_STOP,  FLEXSPI_1PAD, 0x0),
+	},
+
+	[READ_FAST_QUAD_OUTPUT] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x6b,
+				FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_DUMMY_SDR, FLEXSPI_4PAD, 0x08,
+				FLEXSPI_COMMAND_READ_SDR,  FLEXSPI_4PAD, 0x04),
+	},
+
+	[PAGE_PROGRAM_QUAD_INPUT] =
+	{
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_SDR,       FLEXSPI_1PAD, 0x32,
+				FLEXSPI_COMMAND_RADDR_SDR, FLEXSPI_1PAD, 0x18),
+		FLEXSPI_LUT_SEQ(FLEXSPI_COMMAND_WRITE_SDR, FLEXSPI_4PAD, 0x04,
+				FLEXSPI_COMMAND_STOP,      FLEXSPI_1PAD, 0),
+	},
+
+};
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/* FlexSPI NOR device private data */
+
+struct imxrt_flexspi_storage_dev_s {
+	struct mtd_dev_s mtd;
+	struct flexspi_dev_s *flexspi;   /* Saved FlexSPI interface instance */
+	uint8_t *ahb_base;
+	enum flexspi_port_e port;
+	struct flexspi_device_config_s *config;
+};
+
+/****************************************************************************
+ * Private Functions Prototypes
+ ****************************************************************************/
+
+/* MTD driver methods */
+
+static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev,
+				       off_t startblock,
+				       size_t nblocks);
+static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev,
+		off_t offset,
+		size_t nbytes,
+		uint8_t *buffer);
+static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev,
+		off_t startblock,
+		size_t nblocks,
+		uint8_t *buffer);
+static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev,
+		off_t startblock,
+		size_t nblocks,
+		const uint8_t *buffer);
+static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev,
+				       int cmd,
+				       unsigned long arg);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static struct flexspi_device_config_s g_flexspi_device_config = {
+	.flexspi_root_clk = 4000000,
+	.is_sck2_enabled = 0,
+	.flash_size = NOR_USED_SECTORS * NOR_SECTOR_SIZE / 4,
+	.cs_interval_unit = FLEXSPI_CS_INTERVAL_UNIT1_SCK_CYCLE,
+	.cs_interval = 0,
+	.cs_hold_time = 3,
+	.cs_setup_time = 3,
+	.data_valid_time = 0,
+	.columnspace = 0,
+	.enable_word_address = 0,
+	.awr_seq_index = 0,
+	.awr_seq_number = 0,
+	.ard_seq_index = READ_FAST,
+	.ard_seq_number = 1,
+	.ahb_write_wait_unit = FLEXSPI_AHB_WRITE_WAIT_UNIT2_AHB_CYCLE,
+	.ahb_write_wait_interval = 0,
+	.rx_sample_clock = FLEXSPI_READ_SAMPLE_CLK_LOOPBACK_FROM_DQS_PAD,
+};
+
+static struct imxrt_flexspi_storage_dev_s g_flexspi_nor = {
+	.mtd =
+	{
+		.erase  = imxrt_flexspi_storage_erase,
+		.bread  = imxrt_flexspi_storage_bread,
+		.bwrite = imxrt_flexspi_storage_bwrite,
+		.read   = imxrt_flexspi_storage_read,
+		.ioctl  = imxrt_flexspi_storage_ioctl,
+#ifdef CONFIG_MTD_BYTE_WRITE
+		.write  = NULL,
+#endif
+		.name   = "imxrt_flexspi_storage"
+	},
+	.flexspi = (void *)0,
+	.ahb_base = (uint8_t *) NOR_STORAGE_ADDR,
+	.port = FLEXSPI_PORT_A1,
+	.config = &g_flexspi_device_config
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int imxrt_flexspi_storage_read_status(
+	const struct imxrt_flexspi_storage_dev_s *dev,
+	uint32_t *status)
+{
+	int stat;
+
+	struct flexspi_transfer_s transfer = {
+		.device_address = 0,
+		.port = dev->port,
+		.cmd_type = FLEXSPI_READ,
+		.seq_number = 1,
+		.seq_index = READ_STATUS_REG,
+		.data = status,
+		.data_size = 1,
+	};
+
+	stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
+
+	if (stat != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int imxrt_flexspi_storage_write_enable(
+	const struct imxrt_flexspi_storage_dev_s *dev)
+{
+	int stat;
+
+	struct flexspi_transfer_s transfer = {
+		.device_address = 0,
+		.port = dev->port,
+		.cmd_type = FLEXSPI_COMMAND,
+		.seq_number = 1,
+		.seq_index = WRITE_ENABLE,
+		.data = NULL,
+		.data_size = 0,
+	};
+
+	stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
+
+	if (stat != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int imxrt_flexspi_storage_erase_sector(
+	const struct imxrt_flexspi_storage_dev_s *dev,
+	off_t offset)
+{
+	int stat;
+	struct flexspi_transfer_s transfer = {
+		.device_address = offset,
+		.port = dev->port,
+		.cmd_type = FLEXSPI_COMMAND,
+		.seq_number = 1,
+		.seq_index = SECTOR_ERASE_4K,
+		.data = NULL,
+		.data_size = 0,
+	};
+
+	stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
+
+	if (stat != 0) {
+		return -EIO;
+	}
+
+
+	return 0;
+}
+
+static int imxrt_flexspi_storage_erase_chip(
+	const struct imxrt_flexspi_storage_dev_s *dev)
+{
+	/* We can't erase the chip we're executing from */
+	return -EINVAL;
+}
+
+static int imxrt_flexspi_storage_page_program(
+	const struct imxrt_flexspi_storage_dev_s *dev,
+	off_t offset,
+	const void *buffer,
+	size_t len)
+{
+	int stat;
+
+	struct flexspi_transfer_s transfer = {
+		.device_address = offset,
+		.port = dev->port,
+		.cmd_type = FLEXSPI_WRITE,
+		.seq_number = 1,
+		.seq_index = PAGE_PROGRAM_QUAD_INPUT,
+		.data = (uint32_t *) buffer,
+		.data_size = len,
+	};
+
+	stat = FLEXSPI_TRANSFER(dev->flexspi, &transfer);
+
+	if (stat != 0) {
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int imxrt_flexspi_storage_wait_bus_busy(
+	const struct imxrt_flexspi_storage_dev_s *dev)
+{
+	uint32_t status = 0;
+	int ret;
+
+	do {
+		ret = imxrt_flexspi_storage_read_status(dev, &status);
+
+		if (ret) {
+			return ret;
+		}
+	} while (status & 1);
+
+	return 0;
+}
+
+static ssize_t imxrt_flexspi_storage_read(struct mtd_dev_s *dev,
+		off_t offset,
+		size_t nbytes,
+		uint8_t *buffer)
+{
+	struct imxrt_flexspi_storage_dev_s *priv =
+		(struct imxrt_flexspi_storage_dev_s *)dev;
+	uint8_t *src;
+
+	finfo("offset: %08lx nbytes: %d\n", (long)offset, (int)nbytes);
+
+	if (priv->port >= FLEXSPI_PORT_COUNT) {
+		return -EIO;
+	}
+
+	src = priv->ahb_base + offset;
+
+	memcpy(buffer, src, nbytes);
+
+	finfo("return nbytes: %d\n", (int)nbytes);
+	return (ssize_t)nbytes;
+}
+
+static ssize_t imxrt_flexspi_storage_bread(struct mtd_dev_s *dev,
+		off_t startblock,
+		size_t nblocks,
+		uint8_t *buffer)
+{
+	ssize_t nbytes;
+
+	finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	/* On this device, we can handle the block read just like the byte-oriented
+	 * read
+	 */
+
+	nbytes = imxrt_flexspi_storage_read(dev, startblock * NOR_PAGE_SIZE,
+					    nblocks * NOR_PAGE_SIZE, buffer);
+
+	if (nbytes > 0) {
+		nbytes /= NOR_PAGE_SIZE;
+	}
+
+	return nbytes;
+}
+
+static ssize_t imxrt_flexspi_storage_bwrite(struct mtd_dev_s *dev,
+		off_t startblock,
+		size_t nblocks,
+		const uint8_t *buffer)
+{
+	struct imxrt_flexspi_storage_dev_s *priv =
+		(struct imxrt_flexspi_storage_dev_s *)dev;
+	size_t len = nblocks * NOR_PAGE_SIZE;
+	off_t offset = (startblock + NOR_START_PAGE) * NOR_PAGE_SIZE;
+	uint8_t *src = (uint8_t *) buffer;
+#ifdef CONFIG_ARMV7M_DCACHE
+	uint8_t *dst = priv->ahb_base + startblock * NOR_PAGE_SIZE;
+#endif
+	int i;
+
+	finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false);
+
+	irqstate_t flags = enter_critical_section();
+
+	while (len) {
+		i = MIN(NOR_PAGE_SIZE, len);
+		imxrt_flexspi_storage_write_enable(priv);
+		imxrt_flexspi_storage_page_program(priv, offset, src, i);
+		imxrt_flexspi_storage_wait_bus_busy(priv);
+		offset += i;
+		src += i;
+		len -= i;
+	}
+
+	FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true);
+
+	leave_critical_section(flags);
+
+#ifdef CONFIG_ARMV7M_DCACHE
+	up_invalidate_dcache((uintptr_t)dst,
+			     (uintptr_t)dst + nblocks * NOR_PAGE_SIZE);
+#endif
+
+	return nblocks;
+}
+
+static int imxrt_flexspi_storage_erase(struct mtd_dev_s *dev,
+				       off_t startblock,
+				       size_t nblocks)
+{
+	struct imxrt_flexspi_storage_dev_s *priv =
+		(struct imxrt_flexspi_storage_dev_s *)dev;
+	size_t blocksleft = nblocks;
+#ifdef CONFIG_ARMV7M_DCACHE
+	uint8_t *dst = priv->ahb_base + startblock * NOR_SECTOR_SIZE;
+#endif
+
+	startblock += NOR_START_SECTOR;
+
+	finfo("startblock: %08lx nblocks: %d\n", (long)startblock, (int)nblocks);
+
+	FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, false);
+
+	irqstate_t flags = enter_critical_section();
+
+	while (blocksleft-- > 0) {
+		/* Erase each sector */
+
+		imxrt_flexspi_storage_write_enable(priv);
+		imxrt_flexspi_storage_erase_sector(priv, startblock * NOR_SECTOR_SIZE);
+		imxrt_flexspi_storage_wait_bus_busy(priv);
+		startblock++;
+	}
+
+	FLEXSPI_CONFIGURE_PREFETCH(priv->flexspi, true);
+
+	leave_critical_section(flags);
+
+#ifdef CONFIG_ARMV7M_DCACHE
+	up_invalidate_dcache((uintptr_t)dst,
+			     (uintptr_t)dst + nblocks * NOR_SECTOR_SIZE);
+#endif
+
+	return (int)nblocks;
+}
+
+static int imxrt_flexspi_storage_ioctl(struct mtd_dev_s *dev,
+				       int cmd,
+				       unsigned long arg)
+{
+	struct imxrt_flexspi_storage_dev_s *priv =
+		(struct imxrt_flexspi_storage_dev_s *)dev;
+	int ret = -EINVAL; /* Assume good command with bad parameters */
+
+	finfo("cmd: %d\n", cmd);
+
+	switch (cmd) {
+	case MTDIOC_GEOMETRY: {
+			struct mtd_geometry_s *geo =
+				(struct mtd_geometry_s *)((uintptr_t)arg);
+
+			if (geo) {
+				/* Populate the geometry structure with information need to
+				 * know the capacity and how to access the device.
+				 *
+				 * NOTE:
+				 * that the device is treated as though it where just an array
+				 * of fixed size blocks.  That is most likely not true, but the
+				 * client will expect the device logic to do whatever is
+				 * necessary to make it appear so.
+				 */
+
+				geo->blocksize    = (NOR_PAGE_SIZE);
+				geo->erasesize    = (NOR_SECTOR_SIZE);
+				geo->neraseblocks = (NOR_USED_SECTORS);
+
+				ret               = OK;
+
+				finfo("blocksize: %lu erasesize: %lu neraseblocks: %lu\n",
+				      geo->blocksize, geo->erasesize, geo->neraseblocks);
+			}
+		}
+		break;
+
+	case BIOC_PARTINFO: {
+			struct partition_info_s *info =
+				(struct partition_info_s *)arg;
+
+			if (info != NULL) {
+				info->numsectors  = (NOR_USED_SECTORS * NOR_SECTOR_SIZE) / NOR_PAGE_SIZE;
+				info->sectorsize  = NOR_PAGE_SIZE;
+				info->startsector = 0;
+				info->parent[0]   = '\0';
+				ret               = OK;
+			}
+		}
+		break;
+
+	case MTDIOC_BULKERASE: {
+			/* Erase the entire device */
+
+			imxrt_flexspi_storage_write_enable(priv);
+			imxrt_flexspi_storage_erase_chip(priv);
+			imxrt_flexspi_storage_wait_bus_busy(priv);
+			ret               = OK;
+		}
+		break;
+
+	default:
+		ret = -ENOTTY; /* Bad/unsupported command */
+		break;
+	}
+
+	finfo("return %d\n", ret);
+	return ret;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Name: imxrt_flexspi_storage_initialize
+ *
+ * Description:
+ *  This function is called by board-bringup logic to configure the
+ *  flash device.
+ *
+ * Returned Value:
+ *   Zero is returned on success.  Otherwise, a negated errno value is
+ *   returned to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+int imxrt_flexspi_storage_initialize(void)
+{
+	struct mtd_dev_s *mtd_dev = &g_flexspi_nor.mtd;
+	int ret = -ENODEV;
+
+	/* Select FlexSPI1 */
+
+	g_flexspi_nor.flexspi = imxrt_flexspi_initialize(0);
+
+	if (g_flexspi_nor.flexspi) {
+		ret = OK;
+
+
+		FLEXSPI_UPDATE_LUT(g_flexspi_nor.flexspi,
+				   0,
+				   (const uint32_t *)g_flexspi_nor_lut,
+				   sizeof(g_flexspi_nor_lut) / 4);
+	}
+
+	/* Register the MTD driver so that it can be accessed from the
+	 * VFS.
+	 */
+
+	ret = register_mtddriver("/dev/nor", mtd_dev, 0755, NULL);
+
+	if (ret < 0) {
+		syslog(LOG_ERR, "ERROR: Failed to register MTD driver: %d\n",
+		       ret);
+	}
+
+#ifdef CONFIG_FS_LITTLEFS
+
+	/* Mount the LittleFS file system */
+
+	ret = nx_mount("/dev/nor", "/fs/nor", "littlefs", 0,
+		       "autoformat");
+
+	if (ret < 0) {
+		syslog(LOG_ERR,
+		       "ERROR: Failed to mount LittleFS at /mnt/lfs: %d\n",
+		       ret);
+	}
+
+#endif
+
+	return ret;
+}
+#endif /* CONFIG_IMXRT_FLEXSPI */

boards/nxp/tropic-community/src/imxrt_ocram_initialize.c

@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2019, 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 imxrt_ocram_initialize.c
+ *
+ * PX4 fmu-v6xrt RAM startup early startup code.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "arm_internal.h"
+#include "imxrt_iomuxc.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+__BEGIN_DECLS
+extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
+extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
+extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
+extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
+extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
+__END_DECLS
+
+/****************************************************************************
+ * Name: imxrt_ocram_initialize
+ *
+ * Description:
+ *   Called off reset vector to reconfigure the flexRAM
+ *   and finish the FLASH to RAM Copy.
+ *   CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
+ *
+ ****************************************************************************/
+
+__EXPORT void imxrt_ocram_initialize(void)
+{
+	uint32_t regval;
+	register uint64_t *src;
+	register uint64_t *dest;
+
+	/* FlexRAM Configuration
+	 *    F = 64K ITCM
+	 *    A = 64K DTCM
+	 *    5 = 64K OCRAM
+	 *    So 0xFFFFFFAA is
+	 *    384K FlexRAM ITCM
+	 *    128K FlexRAM DTCM
+	 * */
+
+	putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
+	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
+	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
+
+	/* Copy any necessary code sections from FLASH to ITCM. The process is the
+	* same as the code copying from FLASH to RAM above. */
+	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
+	     dest < (uint64_t *)&_eitcmfuncs;) {
+		*dest++ = *src++;
+	}
+
+	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
+	* certain that there are no issues with the state of global variables.
+	*/
+
+	for (dest = &_sdtcm; dest < &_edtcm;) {
+		*dest++ = 0;
+	}
+}

boards/nxp/tropic-community/src/init.c

@@ -104,11 +104,6 @@ extern void led_off(int led);
 
 extern uint32_t _srodata;            /* Start of .rodata */
 extern uint32_t _erodata;            /* End of .rodata */
-extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
-extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
-extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
-extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
-extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
 __END_DECLS
 
 /************************************************************************************
@@ -192,50 +187,6 @@ void imxrt_flash_setup_prefetch_partition(void)
 	ARM_DMB();
 }
 
-/****************************************************************************
- * Name: imxrt_ocram_initialize
- *
- * Description:
- *   Called off reset vector to reconfigure the flexRAM
- *   and finish the FLASH to RAM Copy.
- *
- ****************************************************************************/
-
-__EXPORT void imxrt_ocram_initialize(void)
-{
-	uint32_t regval;
-	register uint64_t *src;
-	register uint64_t *dest;
-
-	/* FlexRAM Configuration
-	 *    F = 64K ITCM
-	 *    A = 64K DTCM
-	 *    5 = 64K OCRAM
-	 *    So 0xFFFFFFAA is
-	 *    384K FlexRAM ITCM
-	 *    128K FlexRAM DTCM
-	 * */
-
-	putreg32(0xFFFFFFAA, IMXRT_IOMUXC_GPR_GPR17);
-	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
-	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL, IMXRT_IOMUXC_GPR_GPR16);
-
-	/* Copy any necessary code sections from FLASH to ITCM. The process is the
-	* same as the code copying from FLASH to RAM above. */
-	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
-	     dest < (uint64_t *)&_eitcmfuncs;) {
-		*dest++ = *src++;
-	}
-
-	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
-	* certain that there are no issues with the state of global variables.
-	*/
-
-	for (dest = &_sdtcm; dest < &_edtcm;) {
-		*dest++ = 0;
-	}
-}
-
 /****************************************************************************
  * Name: imxrt_boardinitialize
  *
@@ -411,5 +362,15 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	imxrt_caninitialize(3);
 #endif
 
+#ifdef CONFIG_IMXRT_FLEXSPI
+	ret = imxrt_flexspi_storage_initialize();
+
+	if (ret < 0) {
+		syslog(LOG_ERR,
+		       "ERROR: imxrt_flexspi_nor_initialize failed: %d\n", ret);
+	}
+
+#endif
+
 	return ret;
 }

boards/omnibus/f4sd/default.px4board

@@ -31,7 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v2/default.px4board

@@ -25,6 +25,7 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 # CONFIG_EKF2_AUXVEL is not set
 # CONFIG_EKF2_BARO_COMPENSATION is not set
 # CONFIG_EKF2_DRAG_FUSION is not set
@@ -40,7 +41,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v5/cryptotest.px4board

@@ -4,5 +4,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_BOARD_CRYPTO=y
 CONFIG_DRIVERS_STUB_KEYSTORE=y
 CONFIG_DRIVERS_SW_CRYPTO=y
+# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
 CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
 CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"

boards/px4/fmu-v5/rover.px4board

@@ -17,7 +17,6 @@ CONFIG_MODULES_UUV_ATT_CONTROL=n
 CONFIG_MODULES_UUV_POS_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v5/stackcheck.px4board

@@ -46,4 +46,3 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y

boards/px4/fmu-v5x/rover.px4board

@@ -14,7 +14,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6c/rover.px4board

@@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6u/rover.px4board

@@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6x/multicopter.px4board

@@ -8,7 +8,6 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_COMMON_RC=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set

boards/px4/fmu-v6x/rover.px4board

@@ -13,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6xrt/rover.px4board

@@ -14,7 +14,6 @@ CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_DRIVERS_ROBOCLAW=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 # CONFIG_EKF2_WIND is not set
 CONFIG_MODULES_ROVER_ACKERMANN=y
 CONFIG_MODULES_ROVER_DIFFERENTIAL=y

boards/px4/fmu-v6xrt/src/CMakeLists.txt

@@ -75,9 +75,14 @@ else()
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
 		imxrt_clockconfig.c
+		imxrt_ocram_initialize.c
 		${SRCS}
 	)
 
+	# Force compiler not to use builtin functions (like memcpy)
+	# to optimize for loops in init.c (imxrt_ocram_initialize)
+	set_source_files_properties(imxrt_ocram_initialize.c PROPERTIES COMPILE_FLAGS -fno-builtin)
+
 	target_link_libraries(drivers_board
 		PRIVATE
 			arch_board_hw_info

boards/px4/fmu-v6xrt/src/imxrt_ocram_initialize.c

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2019, 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 imxrt_ocram_initialize.c
+ *
+ * PX4 fmu-v6xrt RAM startup early startup code.
+ */
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "arm_internal.h"
+#include "imxrt_iomuxc.h"
+
+/****************************************************************************
+ * Pre-Processor Definitions
+ ****************************************************************************/
+
+__BEGIN_DECLS
+extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
+extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
+extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
+extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
+extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
+__END_DECLS
+
+/****************************************************************************
+ * Name: imxrt_ocram_initialize
+ *
+ * Description:
+ *   Called off reset vector to reconfigure the flexRAM
+ *   and finish the FLASH to RAM Copy.
+ *   CMakeLists.txt Forces compiler not to use builtin functions using -fno-builtin
+ *
+ ****************************************************************************/
+
+__EXPORT void imxrt_ocram_initialize(void)
+{
+	uint32_t regval;
+	register uint64_t *src;
+	register uint64_t *dest;
+
+	/* Reallocate
+	 * Final Configuration is
+	 *    No DTCM
+	 *    512k  OCRAM M7 (FlexRAM)          (2038:0000-203f:ffff)
+	 *    128k  OCRAMM7 FlexRAM ECC         (2036:0000-2037:ffff)
+	 *    64k   OCRAM2 ECC parity           (2035:0000-2035:ffff)
+	 *    64k   OCRAM1 ECC parity           (2034:0000-2034:ffff)
+	 *    512k  FlexRAM OCRAM2              (202C:0000-2033:ffff)
+	 *    512k  FlexRAM OCRAM1              (2024:0000-202B:ffff)
+	 *    256k  System  OCRAM M4            (2020:0000-2023:ffff)
+	 */
+
+	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
+	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
+	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
+	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
+
+	/* Copy any necessary code sections from FLASH to ITCM. The process is the
+	* same as the code copying from FLASH to RAM above. */
+	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
+	     dest < (uint64_t *)&_eitcmfuncs;) {
+		*dest++ = *src++;
+	}
+
+	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
+	* certain that there are no issues with the state of global variables.
+	*/
+
+	for (dest = &_sdtcm; dest < &_edtcm;) {
+		*dest++ = 0;
+	}
+
+#if defined(CONFIG_BOOT_RUNFROMISRAM)
+	const uint32_t *src;
+	uint32_t *dest;
+
+	for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
+	     dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
+	     dest < (uint32_t *) &_etext;) {
+		*dest++ = *src++;
+	}
+
+#endif
+}

boards/px4/fmu-v6xrt/src/init.c

@@ -109,11 +109,6 @@ extern void led_off(int led);
 
 extern uint32_t _srodata;            /* Start of .rodata */
 extern uint32_t _erodata;            /* End of .rodata */
-extern const uint64_t _fitcmfuncs;   /* Copy source address in FLASH */
-extern uint64_t _sitcmfuncs;         /* Copy destination start address in ITCM */
-extern uint64_t _eitcmfuncs;         /* Copy destination end address in ITCM */
-extern uint64_t _sdtcm;              /* Copy destination start address in DTCM */
-extern uint64_t _edtcm;              /* Copy destination end address in DTCM */
 __END_DECLS
 
 /************************************************************************************
@@ -225,65 +220,7 @@ void imxrt_flash_setup_prefetch_partition(void)
 	ARM_ISB();
 	ARM_DMB();
 }
-/****************************************************************************
- * Name: imxrt_ocram_initialize
- *
- * Description:
- *   Called off reset vector to reconfigure the flexRAM
- *   and finish the FLASH to RAM Copy.
- *
- ****************************************************************************/
 
-__EXPORT void imxrt_ocram_initialize(void)
-{
-	uint32_t regval;
-	register uint64_t *src;
-	register uint64_t *dest;
-
-	/* Reallocate
-	 * Final Configuration is
-	 *    No DTCM
-	 *    512k  OCRAM M7 (FlexRAM)          (2038:0000-203f:ffff)
-	*    128k  OCRAMM7 FlexRAM ECC         (2036:0000-2037:ffff)
-	*    64k   OCRAM2 ECC parity           (2035:0000-2035:ffff)
-	*    64k   OCRAM1 ECC parity           (2034:0000-2034:ffff)
-	*    512k  FlexRAM OCRAM2              (202C:0000-2033:ffff)
-	 *    512k  FlexRAM OCRAM1              (2024:0000-202B:ffff)
-	 *    256k  System  OCRAM M4            (2020:0000-2023:ffff)
-	 */
-
-	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR17);
-	putreg32(0x0000FFAA, IMXRT_IOMUXC_GPR_GPR18);
-	regval = getreg32(IMXRT_IOMUXC_GPR_GPR16);
-	putreg32(regval | GPR_GPR16_FLEXRAM_BANK_CFG_SEL_REG, IMXRT_IOMUXC_GPR_GPR16);
-
-	/* Copy any necessary code sections from FLASH to ITCM. The process is the
-	* same as the code copying from FLASH to RAM above. */
-	for (src = (uint64_t *)&_fitcmfuncs, dest = (uint64_t *)&_sitcmfuncs;
-	     dest < (uint64_t *)&_eitcmfuncs;) {
-		*dest++ = *src++;
-	}
-
-	/* Clear .dtcm.  We'll do this inline (vs. calling memset) just to be
-	* certain that there are no issues with the state of global variables.
-	*/
-
-	for (dest = &_sdtcm; dest < &_edtcm;) {
-		*dest++ = 0;
-	}
-
-#if defined(CONFIG_BOOT_RUNFROMISRAM)
-	const uint32_t *src;
-	uint32_t *dest;
-
-	for (src = (uint32_t *)(LOCATE_IN_SRC(g_boot_data.start) + g_boot_data.size),
-	     dest = (uint32_t *)(g_boot_data.start + g_boot_data.size);
-	     dest < (uint32_t *) &_etext;) {
-		*dest++ = *src++;
-	}
-
-#endif
-}
 
 /****************************************************************************
  * Name: imxrt_boardinitialize

boards/px4/sitl/default.px4board

@@ -15,7 +15,6 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_VERBOSE_STATUS=y
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y

boards/raspberrypi/pico/default.px4board

@@ -32,7 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
-CONFIG_NAVIGATOR_ADSB=n
+# CONFIG_NAVIGATOR_ADSB is not set
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_SYSTEMCMDS_DMESG=y

msg/Ekf2Timestamps.msg

@@ -14,6 +14,7 @@ int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative times
 # difference of +-3.2s to the sensor_combined topic.
 
 int16 airspeed_timestamp_rel
+int16 airspeed_validated_timestamp_rel
 int16 distance_sensor_timestamp_rel
 int16 optical_flow_timestamp_rel
 int16 vehicle_air_data_timestamp_rel

msg/EstimatorStatusFlags.msg

@@ -47,6 +47,7 @@ bool cs_rng_terrain             # 39 - true if we are fusing range finder data f
 bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain
 bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
 bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
+bool cs_baro_fault	        # 43 - true when the current baro has been declared faulty and is no longer being used
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes

msg/PwmInput.msg

@@ -1,4 +1,4 @@
-uint64 timestamp	# Time since system start (microseconds)
+uint64 timestamp   # Time since system start (microseconds)
-uint64 error_count	# Timer overcapture error flag (AUX5 or MAIN5)
+uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
-uint32 pulse_width	# Pulse width, timer counts
+uint32 pulse_width # Pulse width, timer counts (microseconds)
-uint32 period		# Period, timer counts
+uint32 period      # Period, timer counts (microseconds)

msg/Rpm.msg

@@ -1,4 +1,5 @@
 uint64 timestamp # time since system start (microseconds)
 
+# rpm values of 0.0 mean within a timeout there is no movement measured
 float32 rpm_estimate # filtered revolutions per minute
 float32 rpm_raw

msg/VehicleImu.msg

@@ -9,8 +9,8 @@ uint32 gyro_device_id           # Gyroscope unique device ID for the sensor that
 float32[3] delta_angle          # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt)
 float32[3] delta_velocity       # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt)
 
-uint16 delta_angle_dt           # integration period in microseconds
+uint32 delta_angle_dt           # integration period in microseconds
-uint16 delta_velocity_dt        # integration period in microseconds
+uint32 delta_velocity_dt        # integration period in microseconds
 
 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2

msg/versioned/VehicleLocalPosition.msg

@@ -77,10 +77,12 @@ float32 evv				# Standard deviation of vertical velocity error, (metres/sec)
 bool dead_reckoning                     # True if this position is estimated through dead-reckoning
 
 # estimator specified vehicle limits
-float32 vxy_max				# maximum horizontal speed - set to 0 when limiting not required (meters/sec)
+# set to INFINITY when limiting not required
-float32 vz_max				# maximum vertical speed - set to 0 when limiting not required (meters/sec)
+float32 vxy_max				# maximum horizontal speed (meters/sec)
-float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
+float32 vz_max				# maximum vertical speed (meters/sec)
-float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
+float32 hagl_min			# minimum height above ground level (meters)
+float32 hagl_max_z			# maximum height above ground level for z-control (meters)
+float32 hagl_max_xy			# maximum height above ground level for xy-control (meters)
 
 # TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position
 # TOPICS estimator_local_position

platforms/nuttx/CMakeLists.txt

@@ -506,6 +506,11 @@ if(NOT NUTTX_DIR MATCHES "external")
 		message(STATUS "Found SVD: ${DEBUG_SVD_FILE_PATH}")
 	endif()
 
+	if(NOT CMAKE_GDB)
+		find_program(CMAKE_GDB gdb-multiarch)
+		message(STATUS "Found GDB: ${CMAKE_GDB}")
+	endif()
+
 	if(DEBUG_DEVICE)
 		configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Debug/launch.json.in ${PX4_SOURCE_DIR}/.vscode/launch.json @ONLY)
 	endif()

src/drivers/distance_sensor/lightware_sf45_serial/CMakeLists.txt

@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
+		CollisionPrevention
 	MODULE_CONFIG
 		module.yaml
 	)

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp

@@ -33,19 +33,18 @@
 
 #include "lightware_sf45_serial.hpp"
 
-#include <inttypes.h>
 #include <fcntl.h>
+#include <float.h>
+#include <inttypes.h>
 #include <termios.h>
+
 #include <lib/crc/crc.h>
 #include <lib/mathlib/mathlib.h>
-
+#include <matrix/matrix/math.hpp>
-#include <float.h>
+#include <ObstacleMath.hpp>
 
 using namespace time_literals;
 
-/* Configuration Constants */
-
-
 SF45LaserSerial::SF45LaserSerial(const char *port) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
@@ -93,6 +92,11 @@ int SF45LaserSerial::init()
 	param_get(param_find("SF45_ORIENT_CFG"), &_orient_cfg);
 	param_get(param_find("SF45_YAW_CFG"), &_yaw_cfg);
 
+	// set the sensor orientation
+	const float yaw_cfg_angle = ObstacleMath::sensor_orientation_to_yaw_offset(static_cast<ObstacleMath::SensorOrientation>
+				    (_yaw_cfg));
+	_obstacle_distance.angle_offset = math::degrees(matrix::wrap_2pi(yaw_cfg_angle));
+
 	start();
 	return PX4_OK;
 }
@@ -136,8 +140,6 @@ int SF45LaserSerial::measure()
 
 int SF45LaserSerial::collect()
 {
-	float distance_m = -1.0f;
-
 	if (_sensor_state == STATE_UNINIT) {
 
 		perf_begin(_sample_perf);
@@ -196,8 +198,7 @@ int SF45LaserSerial::collect()
 		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);
 
 		if (_crc_valid) {
-			sf45_process_replies(&distance_m);
+			sf45_process_replies();
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
 			perf_end(_sample_perf);
 			return PX4_OK;
 		}
@@ -592,13 +593,12 @@ void SF45LaserSerial::sf45_send(uint8_t msg_id, bool write, int32_t *data, uint8
 	}
 }
 
-void SF45LaserSerial::sf45_process_replies(float *distance_m)
+void SF45LaserSerial::sf45_process_replies()
 {
 	switch (rx_field.msg_id) {
 	case SF_DISTANCE_DATA_CM: {
 			const float raw_distance = (rx_field.data[0] << 0) | (rx_field.data[1] << 8);
 			int16_t raw_yaw = ((rx_field.data[2] << 0) | (rx_field.data[3] << 8));
-			int16_t scaled_yaw = 0;
 
 			// The sensor scans from 0 to -160, so extract negative angle from int16 and represent as if a float
 			if (raw_yaw > 32000) {
@@ -611,49 +611,41 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			}
 
 			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+			float scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
 
-			switch (_yaw_cfg) {
+			// Adjust for sensor orientation
-			case ROTATION_FORWARD_FACING:
+			scaled_yaw = sf45_wrap_360(scaled_yaw + _obstacle_distance.angle_offset);
-				break;
-
-			case ROTATION_BACKWARD_FACING:
-				if (scaled_yaw > 180) {
-					scaled_yaw = scaled_yaw - 180;
-
-				} else {
-					scaled_yaw = scaled_yaw + 180; // rotation facing aft
-				}
-
-				break;
-
-			case ROTATION_RIGHT_FACING:
-				scaled_yaw = scaled_yaw + 90; // rotation facing right
-				break;
-
-			case ROTATION_LEFT_FACING:
-				scaled_yaw = scaled_yaw - 90; // rotation facing left
-				break;
-
-			default:
-				break;
-			}
 
 			// Convert to meters for the debug message
-			*distance_m = raw_distance * SF45_SCALE_FACTOR;
+			float distance_m = raw_distance * SF45_SCALE_FACTOR;
 			_current_bin_dist = ((uint16_t)raw_distance < _current_bin_dist) ? (uint16_t)raw_distance : _current_bin_dist;
 
 			uint8_t current_bin = sf45_convert_angle(scaled_yaw);
 
 			if (current_bin != _previous_bin) {
-				PX4_DEBUG("scaled_yaw: \t %d, \t current_bin: \t %d, \t distance: \t %8.4f\n", scaled_yaw, current_bin,
+				PX4_DEBUG("scaled_yaw: \t %f, \t current_bin: \t %d, \t distance: \t %8.4f\n", (double)scaled_yaw, current_bin,
-					  (double)*distance_m);
+					  (double)distance_m);
+
+				if (_vehicle_attitude_sub.updated()) {
+					vehicle_attitude_s vehicle_attitude;
+
+					if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
+						_vehicle_attitude = matrix::Quatf(vehicle_attitude.q);
+					}
+				}
+
+				float current_bin_dist = static_cast<float>(_current_bin_dist);
+				float scaled_yaw_rad = math::radians(static_cast<float>(scaled_yaw));
+				ObstacleMath::project_distance_on_horizontal_plane(current_bin_dist, scaled_yaw_rad, _vehicle_attitude);
+				_current_bin_dist = static_cast<uint16_t>(current_bin_dist);
 
 				if (_current_bin_dist > _obstacle_distance.max_distance) {
 					_current_bin_dist = _obstacle_distance.max_distance + 1; // As per ObstacleDistance.msg definition
 				}
 
 				hrt_abstime now = hrt_absolute_time();
+
+				_obstacle_distance.distances[current_bin] = _current_bin_dist;
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);
 
 				_publish_obstacle_msg(now);
@@ -691,47 +683,31 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 {
 	// if the sensor has its cycle delay configured for a low value like 5, it can happen that not every bin gets a measurement.
 	// in this case we assume the measurement to be valid for all bins between the previous and the current bin.
-	uint8_t start;
-	uint8_t end;
 
-	if (abs(current_bin - previous_bin) > BIN_COUNT / 4) {
+	// Shift bin indices such that we can never have the wrap-around case.
-		// wrap-around case is assumed to have happend when the distance between the bins is larger than 1/4 of all Bins
+	const float    fov_offset_angle    = 360.0f - SF45_FIELDOF_VIEW / 2.f;
-		// THis is simplyfied as we are not considering the scaning direction
+	const uint16_t current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  _obstacle_distance.increment,
-		start = math::max(previous_bin, current_bin);
+					     fov_offset_angle);
-		end = math::min(previous_bin, current_bin);
+	const uint16_t previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, _obstacle_distance.increment,
+					     fov_offset_angle);
 
-	} else if (previous_bin < current_bin) {	// Scanning clockwise
+	const uint16_t start = math::min(current_bin_offset, previous_bin_offset) + 1;
-		start = previous_bin + 1;
+	const uint16_t end   = math::max(current_bin_offset, previous_bin_offset);
-		end = current_bin;
 
-	} else { 					// scanning counter-clockwise
+	// populate the missed bins with the measurement
-		start = current_bin;
+	for (uint16_t i = start; i < end; i++) {
-		end = previous_bin - 1;
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, _obstacle_distance.increment, -fov_offset_angle);
-	}
+		_obstacle_distance.distances[bin_index] = measurement;
-
+		_data_timestamps[bin_index] = now;
-	if (start <= end) {
-		for (uint8_t i = start; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-	} else { // wrap-around case
-		for (uint8_t i = start; i < BIN_COUNT; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
-
-		for (uint8_t i = 0; i <= end; i++) {
-			_obstacle_distance.distances[i] = measurement;
-			_data_timestamps[i] = now;
-		}
 	}
 }
+
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;
 	float adjusted_yaw = sf45_wrap_360(yaw - _obstacle_distance.angle_offset);
-	mapped_sector = round(adjusted_yaw / _obstacle_distance.increment);
+	mapped_sector = floor(adjusted_yaw / _obstacle_distance.increment);
 
 	return mapped_sector;
 }

src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.hpp

@@ -49,7 +49,9 @@
 #include <lib/perf/perf_counter.h>
 
 #include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/vehicle_attitude.h>
 
 #include "sf45_commands.h"
 
@@ -92,7 +94,7 @@ public:
 	void				sf45_get_and_handle_request(const int payload_length, const SF_SERIAL_CMD msg_id);
 	void				sf45_send(uint8_t msg_id, bool r_w, int32_t *data, uint8_t data_len);
 	uint16_t			sf45_format_crc(uint16_t crc, uint8_t data_value);
-	void				sf45_process_replies(float *data);
+	void				sf45_process_replies();
 	uint8_t				sf45_convert_angle(const int16_t yaw);
 	float				sf45_wrap_360(float f);
 
@@ -103,6 +105,7 @@ private:
 				obstacle_distance_s::distances[0]);
 	static constexpr uint64_t 	SF45_MEAS_TIMEOUT{100_ms};
 	static constexpr float 		SF45_SCALE_FACTOR = 0.01f;
+	static constexpr float		SF45_FIELDOF_VIEW = 320.f; // degrees
 
 	void				start();
 	void				stop();
@@ -113,6 +116,7 @@ private:
 
 	void 				_handle_missed_bins(uint8_t current_bin, uint8_t previous_bin, uint16_t measurement, hrt_abstime now);
 	void 				_publish_obstacle_msg(hrt_abstime now);
+	uORB::Subscription 		_vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uint64_t			_data_timestamps[BIN_COUNT];
 
 
@@ -141,6 +145,7 @@ private:
 	int32_t				_orient_cfg{0};
 	uint8_t				_previous_bin{0};
 	uint16_t			_current_bin_dist{UINT16_MAX};
+	matrix::Quatf			_vehicle_attitude{};
 
 	// end of SF45/B data members
 

src/drivers/distance_sensor/ulanding_radar/ulanding_radar_main.cpp

@@ -102,7 +102,7 @@ static int usage()
 
 Serial bus driver for the Aerotenna uLanding radar.
 
-Setup/usage information: https://docs.px4.io/v1.9.0/en/sensor/ulanding_radar.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/ulanding_radar.html
 
 ### Examples
 

src/drivers/drv_dshot.h

@@ -88,7 +88,7 @@ typedef enum {
  * @param channel_mask		Bitmask of channels (LSB = channel 0) to enable.
  *				This allows some of the channels to remain configured
  *				as GPIOs or as another function. Already used channels/timers will not be configured as DShot
- * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200
+ * @param dshot_pwm_freq	Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, or DSHOT600
  * @return <0 on error, the initialized channels mask.
  */
 __EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot);

src/drivers/dshot/DShot.cpp

@@ -125,9 +125,6 @@ void DShot::enable_dshot_outputs(const bool enabled)
 			} else if (tim_config == -3) {
 				dshot_frequency_request = DSHOT600;
 
-			} else if (tim_config == -2) {
-				dshot_frequency_request = DSHOT1200;
-
 			} else {
 				_output_mask &= ~channels; // don't use for dshot
 			}
@@ -824,7 +821,7 @@ On startup, the module tries to occupy all available pins for DShot output.
 It skips all pins already in use (e.g. by a camera trigger module).
 
 It supports:
-- DShot150, DShot300, DShot600, DShot1200
+- DShot150, DShot300, DShot600
 - telemetry via separate UART and publishing as esc_status message
 - sending DShot commands via CLI
 

src/drivers/dshot/DShot.h

@@ -52,7 +52,6 @@ using namespace time_literals;
 static constexpr unsigned int DSHOT150  =  150000u;
 static constexpr unsigned int DSHOT300  =  300000u;
 static constexpr unsigned int DSHOT600  =  600000u;
-static constexpr unsigned int DSHOT1200 = 1200000u;
 
 static constexpr int DSHOT_DISARM_VALUE = 0;
 static constexpr int DSHOT_MIN_THROTTLE = 1;
@@ -107,7 +106,6 @@ private:
 		DShot150  = 150,
 		DShot300  = 300,
 		DShot600  = 600,
-		DShot1200 = 1200,
 	};
 
 	struct Command {

src/drivers/ins/vectornav/VectorNav.cpp

@@ -328,7 +328,8 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			local_position.vxy_max = INFINITY;
 			local_position.vz_max = INFINITY;
 			local_position.hagl_min = INFINITY;
-			local_position.hagl_max = INFINITY;
+			local_position.hagl_max_z = INFINITY;
+			local_position.hagl_max_xy = INFINITY;
 
 			local_position.unaided_heading = NAN;
 			local_position.timestamp = hrt_absolute_time();

src/drivers/power_monitor/ina238/ina238.cpp

@@ -160,6 +160,8 @@ int INA238::Reset()
 
 	int ret = PX4_ERROR;
 
+	_retries = 3;
+
 	if (RegisterWrite(Register::CONFIG, (uint16_t)(ADC_RESET_BIT)) != PX4_OK) {
 		return ret;
 	}
@@ -231,6 +233,16 @@ int INA238::collect()
 	success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
 	success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
 
+	if (success) {
+		_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
+		_battery.updateCurrent(static_cast<float>(current * _current_lsb));
+		_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
+
+		_battery.setConnected(success);
+
+		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
+	}
+
 	if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 		// check configuration registers periodically or immediately following any failure
 		if (RegisterCheck(_register_cfg[_checked_register])) {
@@ -242,26 +254,21 @@ int INA238::collect()
 			PX4_DEBUG("register check failed");
 			perf_count(_bad_register_perf);
 			success = false;
+
+			_battery.setConnected(success);
+
+			_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 		}
 	}
 
-	if (!success) {
-		PX4_DEBUG("error reading from sensor");
-		bus_voltage = current = 0;
-	}
-
-	_battery.setConnected(success);
-	_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
-	_battery.updateCurrent(static_cast<float>(current * _current_lsb));
-	_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
-	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
-
 	perf_end(_sample_perf);
 
 	if (success) {
 		return PX4_OK;
 
 	} else {
+		PX4_DEBUG("error reading from sensor");
+
 		return PX4_ERROR;
 	}
 }

src/drivers/pwm_out/module.yaml

@@ -23,7 +23,6 @@ actuator_output:
             -5: DShot150
             -4: DShot300
             -3: DShot600
-            -2: DShot1200
             -1: OneShot
             50: PWM 50 Hz
             100: PWM 100 Hz

src/drivers/uavcan/uavcan_main.cpp

@@ -705,11 +705,12 @@ UavcanNode::Run()
 
 		if (can_init_res < 0) {
 			PX4_ERR("CAN driver init failed %i", can_init_res);
+
+		} else {
+			_instance->init(node_id, can->driver.updateEvent());
+
+			_node_init = true;
 		}
-
-		_instance->init(node_id, can->driver.updateEvent());
-
-		_node_init = true;
 	}
 
 	pthread_mutex_lock(&_node_mutex);
@@ -1072,8 +1073,6 @@ void UavcanMixingInterfaceServo::Run()
 void
 UavcanNode::print_info()
 {
-	(void)pthread_mutex_lock(&_node_mutex);
-
 	// Memory status
 	printf("Pool allocator status:\n");
 	printf("\tCapacity hard/soft: %" PRIu16 "/%" PRIu16 " blocks\n",
@@ -1111,15 +1110,29 @@ UavcanNode::print_info()
 	printf("\n");
 
 #if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER)
-	printf("ESC outputs:\n");
-	_mixing_interface_esc.mixingOutput().printStatus();
 
-	printf("Servo outputs:\n");
+	// Print esc status if at least one channel is enabled
-	_mixing_interface_servo.mixingOutput().printStatus();
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_esc.mixingOutput().isFunctionSet(i)) {
+			printf("ESC outputs:\n");
+			_mixing_interface_esc.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
+	// Print servo status if at least one channel is enabled
+	for (int i = 0; i < OutputModuleInterface::MAX_ACTUATORS; i++) {
+		if (_mixing_interface_servo.mixingOutput().isFunctionSet(i)) {
+			printf("Servo outputs:\n");
+			_mixing_interface_servo.mixingOutput().printStatus();
+			printf("\n");
+			break;
+		}
+	}
+
 #endif
 
-	printf("\n");
-
 	// Sensor bridges
 	for (const auto &br : _sensor_bridges) {
 		printf("Sensor '%s':\n", br->get_name());
@@ -1139,8 +1152,6 @@ UavcanNode::print_info()
 
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-
-	(void)pthread_mutex_unlock(&_node_mutex);
 }
 
 void

src/lib/CMakeLists.txt

@@ -43,6 +43,7 @@ add_subdirectory(cdrstream EXCLUDE_FROM_ALL)
 add_subdirectory(circuit_breaker EXCLUDE_FROM_ALL)
 add_subdirectory(collision_prevention EXCLUDE_FROM_ALL)
 add_subdirectory(component_information EXCLUDE_FROM_ALL)
+add_subdirectory(control_allocation EXCLUDE_FROM_ALL)
 add_subdirectory(controllib EXCLUDE_FROM_ALL)
 add_subdirectory(conversion EXCLUDE_FROM_ALL)
 add_subdirectory(crc EXCLUDE_FROM_ALL)

src/lib/collision_prevention/CMakeLists.txt

@@ -31,7 +31,13 @@
 #
 ############################################################################
 
-px4_add_library(CollisionPrevention CollisionPrevention.cpp)
+px4_add_library(CollisionPrevention
+	CollisionPrevention.cpp
+	ObstacleMath.cpp
+)
 target_compile_options(CollisionPrevention PRIVATE -Wno-cast-align) # TODO: fix and enable
+target_include_directories(CollisionPrevention PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(CollisionPrevention PRIVATE mathlib)
 
 px4_add_functional_gtest(SRC CollisionPreventionTest.cpp LINKLIBS CollisionPrevention)
+px4_add_unit_gtest(SRC ObstacleMathTest.cpp LINKLIBS CollisionPrevention)

src/lib/collision_prevention/CollisionPrevention.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 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
@@ -38,6 +38,7 @@
  */
 
 #include "CollisionPrevention.hpp"
+#include "ObstacleMath.hpp"
 #include <px4_platform_common/events.h>
 
 using namespace matrix;
@@ -400,18 +401,8 @@ CollisionPrevention::_addDistanceSensorData(distance_sensor_s &distance_sensor,
 		int lower_bound = (int)round((sensor_yaw_body_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 		int upper_bound = (int)round((sensor_yaw_body_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) / BIN_SIZE);
 
-		const Quatf q_sensor(Quatf(cosf(sensor_yaw_body_rad / 2.f), 0.f, 0.f, sinf(sensor_yaw_body_rad / 2.f)));
-
-		const Vector3f forward_vector(1.0f, 0.0f, 0.0f);
-
-		const Quatf q_sensor_rotation = vehicle_attitude * q_sensor;
-
-		const Vector3f rotated_sensor_vector = q_sensor_rotation.rotateVector(forward_vector);
-
-		const float sensor_dist_scale = rotated_sensor_vector.xy().norm();
-
 		if (distance_reading < distance_sensor.max_distance) {
-			distance_reading = distance_reading * sensor_dist_scale;
+			ObstacleMath::project_distance_on_horizontal_plane(distance_reading, sensor_yaw_body_rad, vehicle_attitude);
 		}
 
 		uint16_t sensor_range = static_cast<uint16_t>(100.0f * distance_sensor.max_distance + 0.5f); // convert to cm

src/lib/collision_prevention/CollisionPrevention.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2024 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

src/lib/collision_prevention/ObstacleMath.cpp

@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 "ObstacleMath.hpp"
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+
+namespace ObstacleMath
+{
+
+void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle)
+{
+	const Quatf q_vehicle_sensor(Quatf(cosf(yaw / 2.f), 0.f, 0.f, sinf(yaw / 2.f)));
+	const Quatf q_world_sensor = q_world_vehicle * q_vehicle_sensor;
+	const Vector3f forward(1.f, 0.f, 0.f);
+	const Vector3f sensor_direction_in_world = q_world_sensor.rotateVector(forward);
+
+	float horizontal_projection_scale = sensor_direction_in_world.xy().norm();
+	horizontal_projection_scale = math::constrain(horizontal_projection_scale, FLT_EPSILON, 1.0f);
+	distance *= horizontal_projection_scale;
+}
+
+int get_bin_at_angle(float bin_width, float angle)
+{
+	int bin_at_angle = (int)round(matrix::wrap(angle, 0.f, 360.f) / bin_width);
+	return wrap_bin(bin_at_angle, 360 / bin_width);
+}
+
+int get_offset_bin_index(int bin, float bin_width, float angle_offset)
+{
+	int offset = get_bin_at_angle(bin_width, angle_offset);
+	return wrap_bin(bin - offset, 360 / bin_width);
+}
+
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation)
+{
+	float offset = 0.0f;
+
+	switch (orientation) {
+	case SensorOrientation::ROTATION_YAW_0:
+		offset = 0.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_45:
+		offset = M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_90:
+		offset = M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_135:
+		offset = 3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_180:
+		offset = M_PI_F;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_225:
+		offset = -3.0f * M_PI_F / 4.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_270:
+		offset = -M_PI_F / 2.0f;
+		break;
+
+	case SensorOrientation::ROTATION_YAW_315:
+		offset = -M_PI_F / 4.0f;
+		break;
+	}
+
+	return offset;
+}
+
+int wrap_bin(int bin, int bin_count)
+{
+	return (bin + bin_count) % bin_count;
+}
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMath.hpp

@@ -0,0 +1,91 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2025 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 <matrix/math.hpp>
+
+namespace ObstacleMath
+{
+
+enum SensorOrientation {
+	ROTATION_YAW_0   = 0,     // MAV_SENSOR_ROTATION_NONE
+	ROTATION_YAW_45  = 1,	  // MAV_SENSOR_ROTATION_YAW_45
+	ROTATION_YAW_90  = 2,	  // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_YAW_135 = 3,	  // MAV_SENSOR_ROTATION_YAW_135
+	ROTATION_YAW_180 = 4,	  // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_YAW_225 = 5,	  // MAV_SENSOR_ROTATION_YAW_225
+	ROTATION_YAW_270 = 6,	  // MAV_SENSOR_ROTATION_YAW_270
+	ROTATION_YAW_315 = 7,	  // MAV_SENSOR_ROTATION_YAW_315
+
+	ROTATION_FORWARD_FACING  = 0, // MAV_SENSOR_ROTATION_NONE
+	ROTATION_RIGHT_FACING    = 2, // MAV_SENSOR_ROTATION_YAW_90
+	ROTATION_BACKWARD_FACING = 4, // MAV_SENSOR_ROTATION_YAW_180
+	ROTATION_LEFT_FACING     = 6  // MAV_SENSOR_ROTATION_YAW_270
+};
+
+/**
+ * Converts a sensor orientation to a yaw offset
+ * @param orientation sensor orientation
+ */
+float sensor_orientation_to_yaw_offset(const SensorOrientation orientation);
+
+/**
+ * Scales a distance measurement taken in the vehicle body horizontal plane onto the world horizontal plane
+ * @param distance measurement which is scaled down
+ * @param yaw orientation of the measurement on the body horizontal plane
+ * @param q_world_vehicle vehicle attitude quaternion
+ */
+void project_distance_on_horizontal_plane(float &distance, const float yaw, const matrix::Quatf &q_world_vehicle);
+
+/**
+ * Returns bin index at a given angle from the 0th bin
+ * @param bin_width width of a bin in degrees
+ * @param angle clockwise angle from start bin in degrees
+ */
+int get_bin_at_angle(float bin_width, float angle);
+
+/**
+ * Returns bin index for the current bin after an angle offset
+ * @param bin current bin index
+ * @param bin_width width of a bin in degrees
+ * @param angle_offset clockwise angle offset in degrees
+ */
+int get_offset_bin_index(int bin, float bin_width, float angle_offset);
+
+/**
+ * Wraps a bin index to the range [0, bin_count)
+ * @param bin bin index
+ * @param bin_count number of bins
+ */
+int wrap_bin(int bin, int bin_count);
+
+} // ObstacleMath

src/lib/collision_prevention/ObstacleMathTest.cpp

@@ -0,0 +1,241 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/math.hpp>
+#include <lib/mathlib/mathlib.h>
+#include "ObstacleMath.hpp"
+
+using namespace matrix;
+
+TEST(ObstacleMathTest, ProjectDistanceOnHorizontalPlane)
+{
+	// standard vehicle orientation inputs
+	Quatf vehicle_pitch_up_45(Eulerf(0.0f, M_PI_4_F, 0.0f));
+	Quatf vehicle_roll_right_45(Eulerf(M_PI_4_F, 0.0f, 0.0f));
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	float distance = 1.0f;
+	float sensor_orientation = 0; // radians (forward facing)
+
+	// WHEN: we project the distance onto the horizontal plane
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	float expected_scale    = sqrtf(2) / 2;
+	float expected_distance = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+	sensor_orientation = M_PI_2_F; // radians (right facing)
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_roll_right_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = sqrtf(2) / 2;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+
+	// GIVEN: a distance, sensor orientation, and quaternion representing the vehicle's orientation
+	distance = 1.0f;
+
+	ObstacleMath::project_distance_on_horizontal_plane(distance, sensor_orientation, vehicle_pitch_up_45);
+
+	// THEN: the distance should be scaled correctly
+	expected_scale     = 1.f;
+	expected_distance  = 1.0f * expected_scale;
+
+	EXPECT_NEAR(distance, expected_distance, 1e-5);
+}
+
+TEST(ObstacleMathTest, GetBinAtAngle)
+{
+	float bin_width = 5.0f;
+
+	// GIVEN: a start bin, bin width, and angle
+	float angle = 0.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	uint16_t bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 0);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = 90.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 18);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = -90.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 54);
+
+	// GIVEN: a start bin, bin width, and angle
+	angle = 450.0f;
+
+	// WHEN: we calculate the bin index at the angle
+	bin_index = ObstacleMath::get_bin_at_angle(bin_width, angle);
+
+	// THEN: the bin index should be correct
+	EXPECT_EQ(bin_index, 18);
+}
+
+
+TEST(ObstacleMathTest, OffsetBinIndex)
+{
+	// In this test, we want to offset the bin index by a negative and positive angle.
+	// We take the output of the first offset and offset it by the same angle in the
+	// opposite direction to return back to the original bin index.
+
+	// GIVEN: a bin index, bin width, and a negative angle offset
+	uint16_t bin = 0;
+	float bin_width = 5.0f;
+	float angle_offset = -120.0f;
+
+	// WHEN: we offset the bin index by the negative angle
+	uint16_t new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
+
+	// THEN: the new bin index should be correctly offset by the wrapped angle
+	EXPECT_EQ(new_bin_index, 24);
+
+	// GIVEN: the output bin index of the previous offset, bin width, and the same angle
+	// offset in positive direction
+	bin = 24;
+	bin_width = 5.0f;
+	angle_offset = 120.0f;
+
+	// WHEN: we offset the bin index by the positive angle
+	new_bin_index = ObstacleMath::get_offset_bin_index(bin, bin_width, angle_offset);
+
+	// THEN: the new bin index should return back to the original bin index
+	EXPECT_EQ(new_bin_index, 0);
+}
+
+
+TEST(ObstacleMathTest, WrapBin)
+{
+	// GIVEN: a bin index within bounds and the number of bins
+	int bin = 0;
+	int bin_count = 72;
+
+	// WHEN: we wrap a bin index within the bounds
+	int wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should stay 0
+	EXPECT_EQ(wrapped_bin, 0);
+
+	// GIVEN: a bin index that is out of bounds, and the number of bins
+	bin = 73;
+	bin_count = 72;
+
+	// WHEN: we wrap a bin index that is larger than the number of bins
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be wrapped back to the beginning
+	EXPECT_EQ(wrapped_bin, 1);
+
+	// GIVEN: a negative bin index and the number of bins
+	bin = -1;
+	bin_count = 72;
+
+	// WHEN: we wrap a bin index that is negative
+	wrapped_bin = ObstacleMath::wrap_bin(bin, bin_count);
+
+	// THEN: the wrapped bin index should be wrapped back to the end
+	EXPECT_EQ(wrapped_bin, 71);
+}
+
+TEST(ObstacleMathTest, HandleMissedBins)
+{
+	// In this test, the current and previous bin are adjacent to the bins that are outside
+	// the sensor field of view. The missed bins (0,1,6 & 7) should be populated, and no
+	// data should be filled in the bins outside the FOV.
+
+	// GIVEN: measurements, current bin, previous bin, bin width, and field of view offset
+	float measurements[8] = {0, 0, 1, 0, 0, 2, 0, 0};
+	int   current_bin     = 2;
+	int   previous_bin    = 5;
+	int   bin_width       = 45.0f;
+	float fov             = 270.0f;
+	float fov_offset      = 360.0f - fov / 2;
+
+	float measurement     = measurements[current_bin];
+
+	// WHEN: we handle missed bins
+	int current_bin_offset  = ObstacleMath::get_offset_bin_index(current_bin,  bin_width, fov_offset);
+	int previous_bin_offset = ObstacleMath::get_offset_bin_index(previous_bin, bin_width, fov_offset);
+
+	int start = math::min(current_bin_offset, previous_bin_offset) + 1;
+	int end   = math::max(current_bin_offset, previous_bin_offset);
+
+	EXPECT_EQ(start, 1);
+	EXPECT_EQ(end,   5);
+
+	for (uint16_t i = start; i < end; i++) {
+		uint16_t bin_index = ObstacleMath::get_offset_bin_index(i, bin_width, -fov_offset);
+		measurements[bin_index] = measurement;
+	}
+
+	// THEN: the correct missed bins should be populated with the measurement
+	EXPECT_EQ(measurements[0], 1);
+	EXPECT_EQ(measurements[1], 1);
+	EXPECT_EQ(measurements[2], 1);
+	EXPECT_EQ(measurements[3], 0);
+	EXPECT_EQ(measurements[4], 0);
+	EXPECT_EQ(measurements[5], 2);
+	EXPECT_EQ(measurements[6], 1);
+	EXPECT_EQ(measurements[7], 1);
+}

src/lib/control_allocation/CMakeLists.txt

@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 2025 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.
+#
+############################################################################
+
+add_subdirectory(control_allocation)
+add_subdirectory(actuator_effectiveness)

src/lib/control_allocation/actuator_effectiveness/ActuatorEffectiveness.cpp

@@ -32,7 +32,6 @@
  ****************************************************************************/
 
 #include "ActuatorEffectiveness.hpp"
-#include "../ControlAllocation/ControlAllocation.hpp"
 
 #include <px4_platform_common/log.h>
 
@@ -51,12 +50,12 @@ int ActuatorEffectiveness::Configuration::addActuator(ActuatorType type, const m
 		return -1;
 	}
 
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::ROLL, actuator_idx) = torque(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::ROLL, actuator_idx) = torque(0);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::PITCH, actuator_idx) = torque(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::PITCH, actuator_idx) = torque(1);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::YAW, actuator_idx) = torque(2);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::YAW, actuator_idx) = torque(2);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_X, actuator_idx) = thrust(0);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Y, actuator_idx) = thrust(1);
-	effectiveness_matrices[selected_matrix](ControlAllocation::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
+	effectiveness_matrices[selected_matrix](ActuatorEffectiveness::ControlAxis::THRUST_Z, actuator_idx) = thrust(2);
 	matrix_selection_indexes[totalNumActuators()] = selected_matrix;
 	++num_actuators[(int)type];
 	return num_actuators_matrix[selected_matrix]++;

src/lib/control_allocation/actuator_effectiveness/CMakeLists.txt

@@ -34,34 +34,6 @@
 px4_add_library(ActuatorEffectiveness
 	ActuatorEffectiveness.cpp
 	ActuatorEffectiveness.hpp
-	ActuatorEffectivenessUUV.cpp
-	ActuatorEffectivenessUUV.hpp
-	ActuatorEffectivenessControlSurfaces.cpp
-	ActuatorEffectivenessControlSurfaces.hpp
-	ActuatorEffectivenessCustom.cpp
-	ActuatorEffectivenessCustom.hpp
-	ActuatorEffectivenessFixedWing.cpp
-	ActuatorEffectivenessFixedWing.hpp
-	ActuatorEffectivenessHelicopter.cpp
-	ActuatorEffectivenessHelicopter.hpp
-	ActuatorEffectivenessHelicopterCoaxial.cpp
-	ActuatorEffectivenessHelicopterCoaxial.hpp
-	ActuatorEffectivenessMCTilt.cpp
-	ActuatorEffectivenessMCTilt.hpp
-	ActuatorEffectivenessMultirotor.cpp
-	ActuatorEffectivenessMultirotor.hpp
-	ActuatorEffectivenessTilts.cpp
-	ActuatorEffectivenessTilts.hpp
-	ActuatorEffectivenessRotors.cpp
-	ActuatorEffectivenessRotors.hpp
-	ActuatorEffectivenessStandardVTOL.cpp
-	ActuatorEffectivenessStandardVTOL.hpp
-	ActuatorEffectivenessTiltrotorVTOL.cpp
-	ActuatorEffectivenessTiltrotorVTOL.hpp
-	ActuatorEffectivenessTailsitterVTOL.cpp
-	ActuatorEffectivenessTailsitterVTOL.hpp
-	ActuatorEffectivenessRoverAckermann.hpp
-	ActuatorEffectivenessRoverAckermann.cpp
 )
 
 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -69,7 +41,5 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
+		PID
 )
-
-px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
-px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)

src/lib/control_allocation/control_allocation/ControlAllocation.hpp

@@ -71,7 +71,7 @@
 
 #include <matrix/matrix/math.hpp>
 
-#include "ActuatorEffectiveness/ActuatorEffectiveness.hpp"
+#include "control_allocation/actuator_effectiveness/ActuatorEffectiveness.hpp"
 
 class ControlAllocation
 {

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.cpp

@@ -51,6 +51,11 @@ ControlAllocationPseudoInverse::setEffectivenessMatrix(
 			update_normalization_scale);
 	_mix_update_needed = true;
 	_normalization_needs_update = update_normalization_scale;
+
+	if (_metric_allocation && update_normalization_scale) {
+		// adding #include <px4_platform_common/log.h> + PX4_WARN leads to failed linking on test
+		_normalization_needs_update = false;
+	}
 }
 
 void
@@ -59,12 +64,15 @@ ControlAllocationPseudoInverse::updatePseudoInverse()
 	if (_mix_update_needed) {
 		matrix::geninv(_effectiveness, _mix);
 
-		if (_normalization_needs_update && !_had_actuator_failure) {
+		if (!_metric_allocation) {
-			updateControlAllocationMatrixScale();
+			if (_normalization_needs_update && !_had_actuator_failure) {
-			_normalization_needs_update = false;
+				updateControlAllocationMatrixScale();
+				_normalization_needs_update = false;
+			}
+
+			normalizeControlAllocationMatrix();
 		}
 
-		normalizeControlAllocationMatrix();
 		_mix_update_needed = false;
 	}
 }

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverse.hpp

@@ -57,11 +57,13 @@ public:
 	void setEffectivenessMatrix(const matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &effectiveness,
 				    const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators,
 				    bool update_normalization_scale) override;
+	void setMetricAllocation(bool metric_allocation) { _metric_allocation = metric_allocation; }
 
 protected:
 	matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
 
 	bool _mix_update_needed{false};
+	bool _metric_allocation{false};
 
 	/**
 	 * Recalculate pseudo inverse if required.

src/lib/control_allocation/control_allocation/ControlAllocationPseudoInverseTest.cpp

@@ -67,3 +67,27 @@ TEST(ControlAllocationTest, AllZeroCase)
 	EXPECT_EQ(actuator_sp, actuator_sp_expected);
 	EXPECT_EQ(control_allocated, control_allocated_expected);
 }
+
+TEST(ControlAllocationMetricTest, AllZeroCase)
+{
+	ControlAllocationPseudoInverse method;
+
+	matrix::Vector<float, 6> control_sp;
+	matrix::Vector<float, 6> control_allocated;
+	matrix::Vector<float, 6> control_allocated_expected;
+	matrix::Matrix<float, 6, 16> effectiveness;
+	matrix::Vector<float, 16> actuator_sp;
+	matrix::Vector<float, 16> actuator_trim;
+	matrix::Vector<float, 16> linearization_point;
+	matrix::Vector<float, 16> actuator_sp_expected;
+
+	method.setMetricAllocation(true);
+	method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false);
+	method.setControlSetpoint(control_sp);
+	method.allocate();
+	actuator_sp = method.getActuatorSetpoint();
+	control_allocated_expected = method.getAllocatedControl();
+
+	EXPECT_EQ(actuator_sp, actuator_sp_expected);
+	EXPECT_EQ(control_allocated, control_allocated_expected);
+}