add standard vtol airframe

SIH: clean up control surface configuration (#24205 )
* fix sign error in appropriate place In PR https://github.com/PX4/PX4-Autopilot/pull/24175 I changed the control surface deflection signs in generate_fw_aerodynamics to make the 1103 airframe work correctly. However, this breaks the 1101 airframe, introducing sing errors there. So, here the change in generate_fw_aerodynamics is reverted to the state before PR #24175. Instead, the signs are set correctly by using the HIL_ACT_REV bitfield in the respective airframe config files. * match control surface parameters to SIH model
2026-07-01 03:00:35 +08:00 · 2025-01-27 17:39:00 +01:00 · 2025-01-27 16:52:29 +01:00 · 2025-01-27 08:43:11 +01:00 · 2025-01-27 08:43:11 +01:00 · 2025-01-26 23:05:39 -05:00
87 changed files with 889 additions and 304 deletions
@@ -33,18 +33,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_TYPE 1
+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
+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
@@ -0,0 +1,116 @@
+#!/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 Aileron
+# @output Servo2 Elevator
+# @output Servo3 Rudder
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+# SIH in SITL changes. could we source these from a common file?  {{{
+# inspired by: diff ROMFS/px4fmu_common/init.d/airframes/4001_quad_x ROMFS/px4fmu_common/init.d-posix/airframes/10040_sihsim_quadx
+
+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 EKF2_GPS_DELAY 0
+
+# battery check disabled below already
+# HIL_ACT_FUNC* set below -- do we need PWM_MAIN_FUNC* as well?
+# SIH_VEHICLE_TYPE set below too.
+
+# }}}
+
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_ELEV_MC_LOCK 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_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
+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
+
+# disable some checks to allow to fly:
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default CBRK_IO_SAFETY 22027
+
+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.145
+
+# sih as standard vtol
+param set SIH_VEHICLE_TYPE 3
+
+param set-default VT_ARSP_TRANS 6
@@ -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
@@ -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
@@ -41,20 +41,19 @@ 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 0.5
-param set-default CA_SV_CS0_TYPE 2
+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
+param set-default CA_SV_CS1_TYPE 3  # elevator
 param set-default CA_SV_CS2_TRQ_Y 1
-
-param set HIL_ACT_REV 32
+param set-default CA_SV_CS2_TYPE 4  # rudder

 param set-default FW_AIRSPD_MAX 12
 param set-default FW_AIRSPD_MIN 7
@@ -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
@@ -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
@@ -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 needs to be this early for in-air-restarts.
+	# Commander
 	#
 	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.
@@ -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
@@ -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
@@ -36,6 +36,7 @@ CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_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
@@ -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
@@ -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
@@ -352,6 +352,7 @@
 		GPIO_nSAFETY_SWITCH_LED_OUT,	        \
 		GPIO_SAFETY_SWITCH_IN,			\
 		GPIO_nARMED,				\
+		GPIO_PWM_LEVEL_CONTROL,			\
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -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
@@ -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
@@ -41,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
@@ -26,10 +26,10 @@ 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
@@ -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
@@ -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
@@ -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
@@ -104,4 +105,3 @@ CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
 CONFIG_EXAMPLES_FAKE_GPS=y
-# CONFIG_EKF2_AUX_GLOBAL_POSITION is not set
@@ -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
@@ -41,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
@@ -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
@@ -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
@@ -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
-uint16 delta_velocity_dt        # integration period in microseconds
+uint32 delta_angle_dt           # integration period in microseconds
+uint32 delta_velocity_dt        # integration period in microseconds

 uint8 CLIPPING_X = 1
 uint8 CLIPPING_Y = 2
@@ -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)
-float32 vz_max				# maximum vertical speed - set to 0 when limiting not required (meters/sec)
-float32 hagl_min			# minimum height above ground level - set to 0 when limiting not required (meters)
-float32 hagl_max			# maximum height above ground level - set to 0 when limiting not required (meters)
+# set to INFINITY when limiting not required
+float32 vxy_max				# maximum horizontal speed (meters/sec)
+float32 vz_max				# maximum vertical speed (meters/sec)
+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
@@ -41,6 +41,7 @@ px4_add_module(
 	DEPENDS
 		drivers_rangefinder
 		px4_work_queue
+		CollisionPrevention
 	MODULE_CONFIG
 		module.yaml
 	)
@@ -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 <float.h>
+#include <matrix/matrix/math.hpp>
+#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")),
@@ -136,8 +135,6 @@ int SF45LaserSerial::measure()

 int SF45LaserSerial::collect()
 {
-	float distance_m = -1.0f;
-
 	if (_sensor_state == STATE_UNINIT) {

 		perf_begin(_sample_perf);
@@ -196,8 +193,7 @@ int SF45LaserSerial::collect()
 		sf45_get_and_handle_request(payload_length, SF_DISTANCE_DATA_CM);

 		if (_crc_valid) {
-			sf45_process_replies(&distance_m);
-			PX4_DEBUG("val (float): %8.4f, valid: %s", (double)distance_m, ((_crc_valid) ? "OK" : "NO"));
+			sf45_process_replies();
 			perf_end(_sample_perf);
 			return PX4_OK;
 		}
@@ -592,7 +588,7 @@ 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: {
@@ -640,20 +636,34 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 			}

 			// 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,
-					  (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();
+
 				_handle_missed_bins(current_bin, _previous_bin, _current_bin_dist, now);

 				_publish_obstacle_msg(now);
@@ -727,6 +737,7 @@ void SF45LaserSerial::_handle_missed_bins(uint8_t current_bin, uint8_t previous_
 		}
 	}
 }
+
 uint8_t SF45LaserSerial::sf45_convert_angle(const int16_t yaw)
 {
 	uint8_t mapped_sector = 0;
@@ -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);

@@ -113,6 +115,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 +144,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

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

@@ -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 {
@@ -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();
@@ -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;
 	}
 }
@@ -23,7 +23,6 @@ actuator_output:
            -5: DShot150
            -4: DShot300
            -3: DShot600
-            -2: DShot1200
            -1: OneShot
            50: PWM 50 Hz
            100: PWM 100 Hz
@@ -1073,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",
@@ -1112,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");
-	_mixing_interface_servo.mixingOutput().printStatus();
+	// Print esc status if at least one channel is enabled
+	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());
@@ -1140,8 +1152,6 @@ UavcanNode::print_info()

 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);
-
-	(void)pthread_mutex_unlock(&_node_mutex);
 }

 void
@@ -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)
@@ -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
@@ -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
@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   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;
+}
+
+} // ObstacleMath
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   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
+{
+
+/**
+ * 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);
+
+} // ObstacleMath
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   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 "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);
+
+}
@@ -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) {
-			updateControlAllocationMatrixScale();
-			_normalization_needs_update = false;
+		if (!_metric_allocation) {
+			if (_normalization_needs_update && !_had_actuator_failure) {
+				updateControlAllocationMatrixScale();
+				_normalization_needs_update = false;
+			}
+
+			normalizeControlAllocationMatrix();
 		}

-		normalizeControlAllocationMatrix();
 		_mix_update_needed = false;
 	}
 }
@@ -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.
@@ -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);
+}
@@ -71,15 +71,14 @@ parameters:
            description:
                short: Motor ${i} slew rate limit
                long: |
-                  Minimum time allowed for the motor input signal to pass through
-                  the full output range. A value x means that the motor signal
-                  can only go from 0 to 1 in minimum x seconds (in case of
-                  reversible motors, the range is -1 to 1).
+                  Forces the motor output signal to take at least the configured time (in seconds)
+                  to traverse its full range (normally [0, 1], or if reversible [-1, 1]).

                  Zero means that slew rate limiting is disabled.
            type: float
            decimal: 2
            increment: 0.01
+            unit: s
            num_instances: *max_num_mc_motors
            min: 0
            max: 10
@@ -90,14 +89,14 @@ parameters:
            description:
                short: Servo ${i} slew rate limit
                long: |
-                  Minimum time allowed for the servo input signal to pass through
-                  the full output range. A value x means that the servo signal
-                  can only go from -1 to 1 in minimum x seconds.
+                  Forces the servo output signal to take at least the configured time (in seconds)
+                  to traverse its full range [-100%, 100%].

                  Zero means that slew rate limiting is disabled.
            type: float
            decimal: 2
            increment: 0.05
+            unit: s
            num_instances: *max_num_servos
            min: 0
            max: 10
@@ -393,7 +393,11 @@ bool Ekf::isYawEmergencyEstimateAvailable() const
 		return false;
 	}

-	return _yawEstimator.getYawVar() < sq(_params.EKFGSF_yaw_err_max);
+	const float yaw_var = _yawEstimator.getYawVar();
+
+	return (yaw_var > 0.f)
+	       && (yaw_var < sq(_params.EKFGSF_yaw_err_max))
+	       && PX4_ISFINITE(yaw_var);
 }

 bool Ekf::isYawFailure() const
@@ -50,7 +50,7 @@
 void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 {
 	_control_status.flags.fuse_beta = _params.beta_fusion_enabled
-					  && _control_status.flags.fixed_wing
+					  && (_control_status.flags.fixed_wing || _control_status.flags.fuse_aspd)
 					  && _control_status.flags.in_air
 					  && !_control_status.flags.fake_pos;

@@ -208,8 +208,9 @@ public:
 	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
 	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
 	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
-	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;
+	//  hagl_max_z : Maximum height above ground for vertical altitude control (meters). NaN when limiting is not needed.
+	//  hagl_max_xy : Maximum height above ground for horizontal position control (meters). NaN when limiting is not needed.
+	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z, float *hagl_max_xy) const;

 	void resetGyroBias();
 	void resetGyroBiasCov();
@@ -327,13 +327,15 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }

-void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
+void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max_z,
+			      float *hagl_max_xy) const
 {
 	// Do not require limiting by default
 	*vxy_max = NAN;
 	*vz_max = NAN;
 	*hagl_min = NAN;
-	*hagl_max = NAN;
+	*hagl_max_z = NAN;
+	*hagl_max_xy = NAN;

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
@@ -347,7 +349,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl

 	if (relying_on_rangefinder) {
 		*hagl_min = rangefinder_hagl_min;
-		*hagl_max = rangefinder_hagl_max;
+		*hagl_max_z = rangefinder_hagl_max;
 	}

 # if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -370,11 +372,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);

 		// Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion
-		const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height;
+		flow_hagl_max = math::max(flow_hagl_max * 0.9f, flow_hagl_max - 1.0f);

 		*vxy_max = flow_vxy_max;
 		*hagl_min = flow_hagl_min;
-		*hagl_max = flow_hagl_max;
+		*hagl_max_xy = flow_hagl_max;
 	}

 # endif // CONFIG_EKF2_OPTICAL_FLOW
@@ -167,6 +167,10 @@ void EKFGSF_yaw::fuseVelocity(const Vector2f &vel_NE, const float vel_accuracy,
 			const float yaw_delta = wrap_pi(_ekf_gsf[model_index].X(2) - _gsf_yaw);
 			_gsf_yaw_variance += _model_weights(model_index) * (_ekf_gsf[model_index].P(2, 2) + yaw_delta * yaw_delta);
 		}
+
+		if (_gsf_yaw_variance <= 0.f || !PX4_ISFINITE(_gsf_yaw_variance)) {
+			reset();
+		}
 	}
 }

@@ -745,6 +745,7 @@ void EKF2::Run()
 		ekf2_timestamps_s ekf2_timestamps {
 			.timestamp = now,
 			.airspeed_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
+			.airspeed_validated_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.distance_sensor_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.optical_flow_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
 			.vehicle_air_data_timestamp_rel = ekf2_timestamps_s::RELATIVE_TIMESTAMP_INVALID,
@@ -1628,7 +1629,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 			      || _ekf.control_status_flags().wind_dead_reckoning;

 	// get control limit information
-	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
+	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max_z, &lpos.hagl_max_xy);

 	// convert NaN to INFINITY
 	if (!PX4_ISFINITE(lpos.vxy_max)) {
@@ -1643,8 +1644,12 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 		lpos.hagl_min = INFINITY;
 	}

-	if (!PX4_ISFINITE(lpos.hagl_max)) {
-		lpos.hagl_max = INFINITY;
+	if (!PX4_ISFINITE(lpos.hagl_max_z)) {
+		lpos.hagl_max_z = INFINITY;
+	}
+
+	if (!PX4_ISFINITE(lpos.hagl_max_xy)) {
+		lpos.hagl_max_xy = INFINITY;
 	}

 	// publish vehicle local position data
@@ -2076,6 +2081,9 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 			}

 			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
+
+			ekf2_timestamps.airspeed_validated_timestamp_rel = (int16_t)((int64_t)airspeed_validated.timestamp / 100 -
+					(int64_t)ekf2_timestamps.timestamp / 100);
 		}

 	} else if (((ekf2_timestamps.timestamp - _airspeed_validated_timestamp_last) > 3_s) && _airspeed_sub.updated()) {
@@ -573,12 +573,9 @@ private:
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_SIDESLIP)
-		(ParamExtFloat<px4::params::EKF2_BETA_GATE>)
-		_param_ekf2_beta_gate, ///< synthetic sideslip innovation consistency gate size (STD)
-		(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise, ///< synthetic sideslip noise (rad)
-
-		(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
-		_param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables
+		(ParamExtFloat<px4::params::EKF2_BETA_GATE>) _param_ekf2_beta_gate,
+		(ParamExtFloat<px4::params::EKF2_BETA_NOISE>) _param_ekf2_beta_noise,
+		(ParamExtInt<px4::params::EKF2_FUSE_BETA>) _param_ekf2_fuse_beta,
 #endif // CONFIG_EKF2_SIDESLIP

 #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -158,7 +158,7 @@ parameters:
          If there's a jump from larger than RNG_FOG to smaller than EKF2_RNG_FOG, the
          measurement may gets rejected. 0 - disabled
      type: float
-      default: 1.0
+      default: 3.0
      min: 0.0
      max: 20.0
      unit: m
@@ -6,8 +6,8 @@ parameters:
      description:
        short: Enable synthetic sideslip fusion
        long: 'For reliable wind estimation both sideslip and airspeed fusion (see
-          EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or
-          VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported
+          EKF2_ARSP_THR) should be enabled. Only applies to vehicles in fixed-wing mode
+          or with airspeed fusion active. Note: side slip fusion is currently not supported
          for tailsitters.'
      type: boolean
      default: 0
@@ -282,12 +282,6 @@ void FlightTaskAuto::_prepareLandSetpoints()
 			sticks_xy.setZero();
 		}

-		// If ground distance estimate valid (distance sensor) during nudging then limit horizontal speed
-		if (PX4_ISFINITE(_dist_to_bottom)) {
-			// Below 50cm no horizontal speed, above allow per meter altitude 0.5m/s speed
-			max_speed = math::max(0.f, math::min(max_speed, (_dist_to_bottom - .5f) * .5f));
-		}
-
 		_stick_acceleration_xy.setVelocityConstraint(max_speed);
 		_stick_acceleration_xy.generateSetpoints(sticks_xy, _yaw, _land_heading, _position,
 				_velocity_setpoint_feedback.xy(), _deltatime);
@@ -59,8 +59,29 @@ bool FlightTaskManualAcceleration::activate(const trajectory_setpoint_s &last_se

 bool FlightTaskManualAcceleration::update()
 {
+	const vehicle_local_position_s vehicle_local_pos = _sub_vehicle_local_position.get();
+	setMaxDistanceToGround(vehicle_local_pos.hagl_max_xy);
 	bool ret = FlightTaskManualAltitudeSmoothVel::update();

+	float max_hagl_ratio = 0.0f;
+
+	if (PX4_ISFINITE(vehicle_local_pos.hagl_max_xy) && vehicle_local_pos.hagl_max_xy > FLT_EPSILON) {
+		max_hagl_ratio = (vehicle_local_pos.dist_bottom) / vehicle_local_pos.hagl_max_xy;
+	}
+
+	// limit horizontal velocity near max hagl to decrease chance of larger gound distance jumps
+	static constexpr float factor_threshold = 0.8f; // threshold ratio of max_hagl
+	static constexpr float min_vel = 2.f; // minimum max-velocity near max_hagl
+
+	if (max_hagl_ratio > factor_threshold) {
+		max_hagl_ratio = math::min(max_hagl_ratio, 1.f);
+		const float vxy_max = math::min(vehicle_local_pos.vxy_max, _param_mpc_vel_manual.get());
+		_stick_acceleration_xy.setVelocityConstraint(interpolate(vxy_max, factor_threshold, min_vel, vxy_max, min_vel));
+
+	} else {
+		_stick_acceleration_xy.setVelocityConstraint(math::min(_param_mpc_vel_manual.get(), vehicle_local_pos.vxy_max));
+	}
+
 	_stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position,
 			_velocity_setpoint_feedback.xy(), _deltatime);
 	_stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint);
@@ -52,4 +52,9 @@ protected:

 	StickAccelerationXY _stick_acceleration_xy{this};
 	WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTask,
+					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor
+				       )
 };
@@ -81,11 +81,8 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator()
 		_min_distance_to_ground = -INFINITY;
 	}

-	if (PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max)) {
-		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max;
-
-	} else {
-		_max_distance_to_ground = INFINITY;
+	if (!PX4_ISFINITE(_max_distance_to_ground) && PX4_ISFINITE(_sub_vehicle_local_position.get().hagl_max_z)) {
+		_max_distance_to_ground = _sub_vehicle_local_position.get().hagl_max_z;
 	}
 }

@@ -154,8 +151,6 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 	if ((_param_mpc_alt_mode.get() == 1 || _terrain_hold) && PX4_ISFINITE(_dist_to_bottom)) {
 		// terrain following
 		_terrainFollowing(apply_brake, stopped);
-		// respect maximum altitude
-		_respectMaxAltitude();

 	} else {
 		// normal mode where height is dependent on local frame
@@ -185,9 +180,10 @@ void FlightTaskManualAltitude::_updateAltitudeLock()
 			// user demands velocity change
 			_position_setpoint(2) = NAN;
 			// ensure that maximum altitude is respected
-			_respectMaxAltitude();
 		}
 	}
+
+	_respectMaxAltitude();
 }

 void FlightTaskManualAltitude::_respectMinAltitude()
@@ -229,29 +225,20 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 {
 	if (PX4_ISFINITE(_dist_to_bottom)) {

-		// if there is a valid maximum distance to ground, linearly increase speed limit with distance
-		// below the maximum, preserving control loop vertical position error gain.
-		// TODO: manipulate the velocity setpoint instead of tweaking the saturation of the controller
+		float vel_constrained = _param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom);
+
 		if (PX4_ISFINITE(_max_distance_to_ground)) {
-			_constraints.speed_up = math::constrain(_param_mpc_z_p.get() * (_max_distance_to_ground - _dist_to_bottom),
-								-_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());
+			_constraints.speed_up = math::constrain(vel_constrained, -_param_mpc_z_vel_max_dn.get(), _param_mpc_z_vel_max_up.get());

 		} else {
 			_constraints.speed_up = _param_mpc_z_vel_max_up.get();
 		}

-		// if distance to bottom exceeded maximum distance, slowly approach maximum distance
-		if (_dist_to_bottom > _max_distance_to_ground) {
-			// difference between current distance to ground and maximum distance to ground
-			const float delta_distance_to_max = _dist_to_bottom - _max_distance_to_ground;
-			// set position setpoint to maximum distance to ground
-			_position_setpoint(2) = _position(2) + delta_distance_to_max;
-			// limit speed downwards to 0.7m/s
-			_constraints.speed_down = math::min(_param_mpc_z_vel_max_dn.get(), 0.7f);
-
-		} else {
-			_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
+		if (_dist_to_bottom > _max_distance_to_ground && !(_sticks.getThrottleZeroCenteredExpo() < FLT_EPSILON)) {
+			_velocity_setpoint(2) = math::constrain(-vel_constrained, 0.f, _param_mpc_z_vel_max_dn.get());
 		}
+
+		_constraints.speed_down = _param_mpc_z_vel_max_dn.get();
 	}
 }

@@ -306,6 +293,7 @@ bool FlightTaskManualAltitude::update()
 	_scaleSticks();
 	_updateSetpoints();
 	_constraints.want_takeoff = _checkTakeoff();
+	_max_distance_to_ground = INFINITY;

 	return ret;
 }
@@ -53,6 +53,7 @@ public:
 	bool activate(const trajectory_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
+	void setMaxDistanceToGround(float max_distance) { _max_distance_to_ground = max_distance; }

 protected:
 	void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */
@@ -73,8 +73,20 @@ void StickAccelerationXY::resetAcceleration(const matrix::Vector2f &acceleration
 void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw, const float yaw_sp, const Vector3f &pos,
 		const matrix::Vector2f &vel_sp_feedback, const float dt)
 {
+	// gradually adjust velocity constraint because good tracking is required for the drag estimation
+	if (fabsf(_targeted_velocity_constraint - _current_velocity_constraint) > 0.1f) {
+		if (!PX4_ISFINITE(_current_velocity_constraint) || !PX4_ISFINITE(_targeted_velocity_constraint)) {
+			_current_velocity_constraint = _targeted_velocity_constraint;
+
+		} else {
+			_current_velocity_constraint = math::constrain(_targeted_velocity_constraint,
+						       _current_velocity_constraint - dt * _param_mpc_acc_hor.get(),
+						       _current_velocity_constraint + dt * _param_mpc_acc_hor.get());
+		}
+	}
+
 	// maximum commanded velocity can be constrained dynamically
-	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _velocity_constraint);
+	const float velocity_sc = fminf(_param_mpc_vel_manual.get(), _current_velocity_constraint);
 	Vector2f velocity_scale(velocity_sc, velocity_sc);
 	// maximum commanded acceleration is scaled down with velocity
 	const float acceleration_sc = _param_mpc_acc_hor.get() * (velocity_sc / _param_mpc_vel_manual.get());
@@ -63,7 +63,8 @@ public:
 	void getSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, matrix::Vector3f &acc_sp);
 	float getMaxAcceleration() { return _param_mpc_acc_hor.get(); };
 	float getMaxJerk() { return _param_mpc_jerk_max.get(); };
-	void setVelocityConstraint(float vel) { _velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	void setVelocityConstraint(float vel) { _targeted_velocity_constraint = fmaxf(vel, FLT_EPSILON); };
+	float getVelocityConstraint() { return _current_velocity_constraint; };

 private:
 	CollisionPrevention _collision_prevention{this};
@@ -85,7 +86,8 @@ private:
 	matrix::Vector2f _acceleration_setpoint;
 	matrix::Vector2f _acceleration_setpoint_prev;

-	float _velocity_constraint{INFINITY};
+	float _targeted_velocity_constraint{INFINITY};
+	float _current_velocity_constraint{INFINITY};

 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
@@ -643,7 +643,8 @@ void BlockLocalPositionEstimator::publishLocalPos()
 		_pub_lpos.get().vxy_max = INFINITY;
 		_pub_lpos.get().vz_max = INFINITY;
 		_pub_lpos.get().hagl_min = INFINITY;
-		_pub_lpos.get().hagl_max = INFINITY;
+		_pub_lpos.get().hagl_max_z = INFINITY;
+		_pub_lpos.get().hagl_max_xy = INFINITY;
 		_pub_lpos.get().timestamp = hrt_absolute_time();;
 		_pub_lpos.update();
 	}
@@ -294,6 +294,7 @@ void LoggedTopics::add_estimator_replay_topics()

 	// current EKF2 subscriptions
 	add_topic("airspeed");
+	add_topic("airspeed_validated");
 	add_topic("vehicle_optical_flow");
 	add_topic("sensor_combined");
 	add_topic("sensor_selection");
@@ -1744,6 +1744,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
 		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
 		configure_stream_local("ATTITUDE", 20.0f);
+		configure_stream_local("ATTITUDE_QUATERNION", 20.0f);
 		configure_stream_local("ALTITUDE", 10.0f);
 		configure_stream_local("DISTANCE_SENSOR", 10.0f);
 		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
@@ -2738,7 +2738,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.vxy_max = INFINITY;
 		hil_local_pos.vz_max = INFINITY;
 		hil_local_pos.hagl_min = INFINITY;
-		hil_local_pos.hagl_max = INFINITY;
+		hil_local_pos.hagl_max_z = INFINITY;
+		hil_local_pos.hagl_max_xy = INFINITY;
 		hil_local_pos.timestamp = hrt_absolute_time();
 		_local_pos_pub.publish(hil_local_pos);
 	}
@@ -46,12 +46,7 @@ FeasibilityChecker::FeasibilityChecker() :

 void FeasibilityChecker::reset()
 {
-	_mission_validity_failed = false;
-	_takeoff_failed = false;
-	_land_pattern_validity_failed = false;
-	_distance_between_waypoints_failed = false;
-	_fixed_wing_land_approach_failed = false;
-	_takeoff_land_available_failed = false;
+	_checks_failed.value = 0;

 	_found_item_with_position = false;
 	_has_vtol_takeoff = false;
@@ -155,11 +150,11 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
 		updateData();
 	}

-	if (!_mission_validity_failed) {
-		_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
+	if (!_checks_failed.flags.mission_validity_failed) {
+		_checks_failed.flags.mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
 	}

-	if (_mission_validity_failed) {
+	if (_checks_failed.flags.mission_validity_failed) {
 		// if a mission item is not valid then abort the other checks
 		return false;
 	}
@@ -177,7 +172,7 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
 	}

 	if (current_index == total_count - 1) {
-		_takeoff_land_available_failed = !checkTakeoffLandAvailable();
+		_checks_failed.flags.takeoff_land_available_failed = !checkTakeoffLandAvailable();
 	}

 	_mission_item_previous = mission_item;
@@ -188,39 +183,39 @@ bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int
 void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index)
 {

-	if (!_distance_between_waypoints_failed) {
-		_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
+	if (!_checks_failed.flags.distance_between_waypoints_failed) {
+		_checks_failed.flags.distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
 	}

 	if (!_first_waypoint_found) {
 		checkHorizontalDistanceToFirstWaypoint(mission_item);
 	}

-	if (!_takeoff_failed) {
-		_takeoff_failed = !checkTakeoff(mission_item);
+	if (!_checks_failed.flags.takeoff_failed) {
+		_checks_failed.flags.takeoff_failed = !checkTakeoff(mission_item);
 	}

-	if (!_items_fit_to_vehicle_type_failed) {
-		_items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
+	if (!_checks_failed.flags.items_fit_to_vehicle_type_failed) {
+		_checks_failed.flags.items_fit_to_vehicle_type_failed = !checkItemsFitToVehicleType(mission_item);
 	}
 }

 void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
 {
-	if (!_land_pattern_validity_failed) {
-		_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
+	if (!_checks_failed.flags.land_pattern_validity_failed) {
+		_checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
 	}

 }

 void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index)
 {
-	if (!_land_pattern_validity_failed) {
-		_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
+	if (!_checks_failed.flags.land_pattern_validity_failed) {
+		_checks_failed.flags.land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
 	}

-	if (!_fixed_wing_land_approach_failed) {
-		_fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
+	if (!_checks_failed.flags.fixed_wing_land_approach_failed) {
+		_checks_failed.flags.fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
 	}

 }
@@ -77,12 +77,7 @@ public:
 	*/
 	bool someCheckFailed()
 	{
-		return _takeoff_failed ||
-		       _distance_between_waypoints_failed ||
-		       _land_pattern_validity_failed ||
-		       _fixed_wing_land_approach_failed ||
-		       _mission_validity_failed ||
-		       _takeoff_land_available_failed;
+		return _checks_failed.value != 0;
 	}

 	/**
@@ -110,14 +105,18 @@ private:
 	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
 	VehicleType _vehicle_type{VehicleType::RotaryWing};

-	// internal flags to keep track of which checks failed
-	bool _mission_validity_failed{false};
-	bool _takeoff_failed{false};
-	bool _land_pattern_validity_failed{false};
-	bool _distance_between_waypoints_failed{false};
-	bool _fixed_wing_land_approach_failed{false};
-	bool _takeoff_land_available_failed{false};
-	bool _items_fit_to_vehicle_type_failed{false};
+	union checks_failed_u {
+		struct {
+			bool mission_validity_failed : 1;
+			bool takeoff_failed : 1;
+			bool land_pattern_validity_failed : 1;
+			bool distance_between_waypoints_failed : 1;
+			bool fixed_wing_land_approach_failed : 1;
+			bool takeoff_land_available_failed : 1;
+			bool items_fit_to_vehicle_type_failed : 1;
+		} flags;
+		uint16_t value {0};
+	} _checks_failed{};

 	// internal checkTakeoff related variables
 	bool _found_item_with_position{false};
@@ -1026,7 +1026,8 @@ void MissionBlock::updateMaxHaglFailsafe()
 	const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt;

 	if (_navigator->get_global_position()->terrain_alt_valid
-	    && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) {
+	    && ((target_alt - _navigator->get_global_position()->terrain_alt)
+		> math::min(_navigator->get_local_position()->hagl_max_z, _navigator->get_local_position()->hagl_max_xy))) {
 		// Handle case where the altitude setpoint is above the maximum HAGL (height above ground level)
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t");
 		events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL");
@@ -118,7 +118,7 @@ VtolTakeoff::on_active()
 				_mission_item.time_inside = 1.f;
 				_mission_item.loiter_radius = _navigator->get_loiter_radius();
 				_mission_item.acceptance_radius  = _navigator->get_acceptance_radius();
-				_mission_item.altitude = _navigator->get_home_position()->alt + _param_loiter_alt.get();
+				_mission_item.altitude = _takeoff_alt_msl + _param_loiter_alt.get();

 				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 				pos_sp_triplet->current.lat = _loiter_location(0);
@@ -170,6 +170,8 @@ VtolTakeoff::set_takeoff_position()
 	// set current mission item to takeoff
 	set_takeoff_item(&_mission_item, _transition_alt_amsl);

+	_takeoff_alt_msl = _navigator->get_global_position()->alt;
+
 	_mission_item.lat = _navigator->get_global_position()->lat;
 	_mission_item.lon = _navigator->get_global_position()->lon;

@@ -70,6 +70,7 @@ private:
 	} _takeoff_state;

 	float _transition_alt_amsl{0.f};	// absolute altitude at which vehicle will transition to forward flight
+	float _takeoff_alt_msl{0.f};
 	matrix::Vector2d _loiter_location;
 	float _loiter_height{0};

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -1592,8 +1592,6 @@ PARAM_DEFINE_INT32(RC_MAP_FLTM_BTN, 0);
 /**
 * AUX1 Passthrough RC channel
 *
- * Default function: Camera pitch
- *
 * @min 0
 * @max 18
 * @group Radio Calibration
@@ -1622,8 +1620,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);
 /**
 * AUX2 Passthrough RC channel
 *
- * Default function: Camera roll
- *
 * @min 0
 * @max 18
 * @group Radio Calibration
@@ -1652,8 +1648,6 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);
 /**
 * AUX3 Passthrough RC channel
 *
- * Default function: Camera azimuth / yaw
- *
 * @min 0
 * @max 18
 * @group Radio Calibration
@@ -39,6 +39,7 @@

 // for ekf2 replay
 #include <uORB/topics/airspeed.h>
+#include <uORB/topics/airspeed_validated.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/landing_target_pose.h>
 #include <uORB/topics/sensor_combined.h>
@@ -91,6 +92,9 @@ ReplayEkf2::onSubscriptionAdded(Subscription &sub, uint16_t msg_id)
 	} else if (sub.orb_meta == ORB_ID(airspeed)) {
 		_airspeed_msg_id = msg_id;

+	} else if (sub.orb_meta == ORB_ID(airspeed_validated)) {
+		_airspeed_validated_msg_id = msg_id;
+
 	} else if (sub.orb_meta == ORB_ID(distance_sensor)) {
 		_distance_sensor_msg_id = msg_id;

@@ -138,6 +142,7 @@ ReplayEkf2::publishEkf2Topics(const ekf2_timestamps_s &ekf2_timestamps, std::ifs
 	};

 	handle_sensor_publication(ekf2_timestamps.airspeed_timestamp_rel, _airspeed_msg_id);
+	handle_sensor_publication(ekf2_timestamps.airspeed_validated_timestamp_rel, _airspeed_validated_msg_id);
 	handle_sensor_publication(ekf2_timestamps.distance_sensor_timestamp_rel, _distance_sensor_msg_id);
 	handle_sensor_publication(ekf2_timestamps.optical_flow_timestamp_rel, _optical_flow_msg_id);
 	handle_sensor_publication(ekf2_timestamps.vehicle_air_data_timestamp_rel, _vehicle_air_data_msg_id);
@@ -225,6 +230,7 @@ ReplayEkf2::onExitMainLoop()
 	PX4_INFO("Topic, Num Published, Num Error (no timestamp match found):");

 	print_sensor_statistics(_airspeed_msg_id, "airspeed");
+	print_sensor_statistics(_airspeed_validated_msg_id, "airspeed_validated");
 	print_sensor_statistics(_distance_sensor_msg_id, "distance_sensor");
 	print_sensor_statistics(_optical_flow_msg_id, "vehicle_optical_flow");
 	print_sensor_statistics(_sensor_combined_msg_id, "sensor_combined");
@@ -82,6 +82,7 @@ private:
 	static constexpr uint16_t msg_id_invalid = 0xffff;

 	uint16_t _airspeed_msg_id = msg_id_invalid;
+	uint16_t _airspeed_validated_msg_id = msg_id_invalid;
 	uint16_t _distance_sensor_msg_id = msg_id_invalid;
 	uint16_t _optical_flow_msg_id = msg_id_invalid;
 	uint16_t _sensor_combined_msg_id = msg_id_invalid;
@@ -55,7 +55,7 @@ public:
 	~Integrator() = default;

 	static constexpr float DT_MIN{1e-6f}; // 1 microsecond
-	static constexpr float DT_MAX{static_cast<float>(UINT16_MAX) * 1e-6f};
+	static constexpr float DT_MAX{static_cast<float>(UINT32_MAX) * 1e-6f};

 	/**
 	 * Put an item into the integral.
@@ -111,7 +111,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 * @return		true if integral valid
 	 */
-	bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
+	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
 	{
 		if (integral_ready()) {
 			integral = _alpha;
@@ -200,7 +200,7 @@ public:
 	 * @param integral_dt	Get the dt in us of the current integration.
 	 * @return		true if integral valid
 	 */
-	bool reset(matrix::Vector3f &integral, uint16_t &integral_dt)
+	bool reset(matrix::Vector3f &integral, uint32_t &integral_dt)
 	{
 		if (Integrator::reset(integral, integral_dt)) {
 			// apply coning corrections
@@ -36,3 +36,7 @@ px4_add_library(vehicle_optical_flow
 	VehicleOpticalFlow.hpp
 )
 target_link_libraries(vehicle_optical_flow PRIVATE px4_work_queue)
+
+if(BUILD_TESTING)
+	add_subdirectory(test)
+endif()
@@ -161,7 +161,7 @@ void VehicleOpticalFlow::Run()
 			}

 			Vector3f delta_angle{NAN, NAN, NAN};
-			uint16_t delta_angle_dt;
+			uint32_t delta_angle_dt;

 			if (_gyro_integrator.reset(delta_angle, delta_angle_dt)) {
 				_delta_angle += delta_angle;
@@ -73,9 +73,12 @@ public:

 	void PrintStatus();

+protected:
+	void UpdateDistanceSensor();
+	int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
+
 private:
 	void ClearAccumulatedData();
-	void UpdateDistanceSensor();
 	void UpdateSensorGyro();

 	void Run() override;
@@ -117,7 +120,6 @@ private:
 	uint16_t _quality_sum{0};
 	uint8_t _accumulated_count{0};

-	int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations
 	hrt_abstime _last_range_sensor_update{0};

 	bool _delta_angle_available{false};
@@ -0,0 +1,35 @@
+############################################################################
+#
+#   Copyright (c) 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_functional_gtest(SRC VehicleOpticalFlowTest.cpp LINKLIBS vehicle_optical_flow uORB sensor_calibration)
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test for VehicleOpticalFlow
+ */
+
+#include <gtest/gtest.h>
+#include "../VehicleOpticalFlow.hpp"
+#include <uORB/uORBManager.hpp>
+#include <uORB/topics/distance_sensor.h>
+
+
+distance_sensor_s createDistanceSensorMessage(uint16_t orientation)
+{
+	distance_sensor_s message;
+	message.timestamp = hrt_absolute_time();
+	message.min_distance = 1.f;
+	message.max_distance = 100.f;
+	message.current_distance = 1.1f;
+
+	message.variance = 0.1f;
+	message.signal_quality = 100;
+	message.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	message.orientation = orientation;
+	message.h_fov = math::radians(50.f);
+	message.v_fov = math::radians(30.f);
+	return message;
+
+}
+
+class VehicleOpticalFlowTest : public ::testing::Test
+{
+public:
+
+	class VehicleOpticalFlowTestable  : public  sensors::VehicleOpticalFlow
+	{
+	public:
+		void UpdateDistanceSensorPublic()
+		{
+			VehicleOpticalFlow::UpdateDistanceSensor();
+		}
+		bool IsDistanceSensorSelected()
+		{
+			return _distance_sensor_selected >= 0;
+
+		}
+	};
+
+	void SetUp() override
+	{
+		uORB::Manager::initialize();
+
+	}
+	void TearDown() override
+	{
+		uORB::Manager::terminate();
+	}
+};
+
+
+TEST_F(VehicleOpticalFlowTest, CameraFacingDown)
+{
+	// GIVEN: message with sensor camera facing down
+	distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_DOWNWARD_FACING);
+	orb_advertise(ORB_ID(distance_sensor), &message);
+
+	// WHEN: update distance sensor
+	VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable;
+	testable.UpdateDistanceSensorPublic();
+
+	// THEN: sensor selected
+	EXPECT_TRUE(testable.IsDistanceSensorSelected());
+}
+
+TEST_F(VehicleOpticalFlowTest, CameraFacingForward)
+{
+	// GIVEN: message with sensor camera facing forward
+	distance_sensor_s message = createDistanceSensorMessage(distance_sensor_s::ROTATION_FORWARD_FACING);
+	orb_advertise(ORB_ID(distance_sensor), &message);
+
+	// WHEN: update distance sensor
+	VehicleOpticalFlowTest::VehicleOpticalFlowTestable testable;
+	testable.UpdateDistanceSensorPublic();
+
+	// THEN: sensor is not selected
+	EXPECT_FALSE(testable.IsDistanceSensorSelected());
+}
@@ -95,12 +95,6 @@ int GZBridge::init()
 				model_pose_v.push_back(0.0);
 			}

-			// If model position z is less equal than 0, move above floor to prevent floor glitching
-			if (model_pose_v[2] <= 0.0) {
-				PX4_INFO("Model position z is less or equal 0.0, moving upwards");
-				model_pose_v[2] = 0.5;
-			}
-
 			gz::msgs::Pose *p = req.mutable_pose();
 			gz::msgs::Vector3d *position = p->mutable_position();
 			position->set_x(model_pose_v[0]);
@@ -141,7 +135,7 @@ int GZBridge::init()

 				// If Gazebo has not been called, wait 2 seconds and try again.
 				else {
-					PX4_WARN("Service call timed out as Gazebo has not been detected.");
+					PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying...");
 					system_usleep(2000000);
 				}
 			}
@@ -159,7 +153,7 @@ int GZBridge::init()

 			while (scene_created == false) {
 				if (!callSceneInfoMsgService(scene_info_service)) {
-					PX4_WARN("Service call timed out as Gazebo has not been detected.");
+					PX4_WARN("Service call timed out as Gazebo has not been detected. Retrying...");
 					system_usleep(2000000);

 				} else {
@@ -919,7 +913,7 @@ bool GZBridge::callEntityFactoryService(const std::string &service, const gz::ms
 		}

 	} else {
-		PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
+		PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
 		return false;
 	}

@@ -932,7 +926,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service)
 	gz::msgs::Empty req;
 	gz::msgs::Scene rep;

-	if (_node.Request(service, req, 1000, rep, result)) {
+	if (_node.Request(service, req, 3000, rep, result)) {
 		if (!result) {
 			PX4_ERR("Scene Info service call failed.");
 			return false;
@@ -942,7 +936,7 @@ bool GZBridge::callSceneInfoMsgService(const std::string &service)
 		}

 	} else {
-		PX4_ERR("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
+		PX4_WARN("Service call timed out. Check GZ_SIM_RESOURCE_PATH is set correctly.");
 		return false;
 	}

@@ -955,7 +949,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::

 	gz::msgs::Boolean rep;

-	if (_node.Request(service, req, 1000, rep, result)) {
+	if (_node.Request(service, req, 3000, rep, result)) {
 		if (!rep.data() || !result) {
 			PX4_ERR("String service call failed");
 			return false;
@@ -964,7 +958,7 @@ bool GZBridge::callStringMsgService(const std::string &service, const gz::msgs::
 	}

 	else {
-		PX4_ERR("Service call timed out: %s", service.c_str());
+		PX4_WARN("Service call timed out: %s", service.c_str());
 		return false;
 	}

@@ -977,7 +971,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V

 	gz::msgs::Boolean rep;

-	if (_node.Request(service, req, 1000, rep, result)) {
+	if (_node.Request(service, req, 3000, rep, result)) {
 		if (!rep.data() || !result) {
 			PX4_ERR("String service call failed");
 			return false;
@@ -986,7 +980,7 @@ bool GZBridge::callVector3dService(const std::string &service, const gz::msgs::V
 	}

 	else {
-		PX4_ERR("Service call timed out: %s", service.c_str());
+		PX4_WARN("Service call timed out: %s", service.c_str());
 		return false;
 	}

@@ -607,7 +607,8 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 		hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
 		hil_lpos.vz_max = std::numeric_limits<float>::infinity();
 		hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
-		hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_max_z = std::numeric_limits<float>::infinity();
+		hil_lpos.hagl_max_xy = std::numeric_limits<float>::infinity();

 		// always publish ground truth attitude message
 		_lpos_ground_truth_pub.publish(hil_lpos);
@@ -53,6 +53,7 @@ if(PX4_PLATFORM MATCHES "posix")
 		airplane
 		quadx
 		xvert
+		standard_vtol
 	)

 	# find corresponding airframes
@@ -361,7 +361,7 @@ void Sih::generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd,
 	_wing_l.update_aero(v_B, _w_B, alt, roll_cmd * FLAP_MAX);
 	_wing_r.update_aero(v_B, _w_B, alt, -roll_cmd * FLAP_MAX);

-	_tailplane.update_aero(v_B, _w_B, alt, pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd);
+	_tailplane.update_aero(v_B, _w_B, alt, -pitch_cmd * FLAP_MAX, _T_MAX * throttle_cmd);
 	_fin.update_aero(v_B, _w_B, alt, yaw_cmd * FLAP_MAX, _T_MAX * throttle_cmd);
 	_fuselage.update_aero(v_B, _w_B, alt);

@@ -220,7 +220,12 @@ private:

 	float       _u[NUM_ACTUATORS_MAX] {};         // thruster signals

+	// MC = Multicopter
+	// FW = Fixed Wing
+	// TS = Tailsitter VTOL
+	// SVTOL = Standard VTOL
 	enum class VehicleType {MC, FW, TS, SVTOL};
+
 	VehicleType _vehicle = VehicleType::MC;

 	// aerodynamic segments for the fixedwing
@@ -354,7 +354,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f);
 * Minimum pitch angle during hover.
 *
 * Any pitch setpoint below this value is translated to a forward force by the fixed-wing forward actuation if
- * VT_FW_TRHUST_EN is set to 1.
+ * VT_FWD_TRHUST_EN is set.
 *
 * @unit deg
 * @min -10.0
Author	SHA1	Message	Date
Balduin	b669de4d9a	add standard vtol airframe	2025-01-27 17:39:00 +01:00
Balduin	5bca71791a	SIH: clean up control surface configuration (#24205 ) * fix sign error in appropriate place In PR https://github.com/PX4/PX4-Autopilot/pull/24175 I changed the control surface deflection signs in generate_fw_aerodynamics to make the 1103 airframe work correctly. However, this breaks the 1101 airframe, introducing sing errors there. So, here the change in generate_fw_aerodynamics is reverted to the state before PR #24175. Instead, the signs are set correctly by using the HIL_ACT_REV bitfield in the respective airframe config files. * match control surface parameters to SIH model	2025-01-27 16:52:29 +01:00
Matthias Grob	165f644580	control_allocator: fix typo and use [0,1] instead of [0%, 100%] in slew rate description Co-authored-by: Hamish Willee <hamishwillee@gmail.com>	2025-01-27 08:43:11 +01:00
Matthias Grob	c1cab2d4e0	control_allocator: add unit for slew rate and reword description	2025-01-27 08:43:11 +01:00
bresch	c76e74338b	ekf-replay: fix airspeed replay If available, the EKF uses airspeed_validated, not airspeed	2025-01-26 23:05:39 -05:00
alexklimaj	c3ba39f931	dshot: remove dshot 1200	2025-01-23 23:50:50 -05:00
cuav-liu1	1aab194f9e	boards: fix 7-nano pwm voltage control pin not initialized	2025-01-23 15:58:49 -05:00
Silvan Fuhrer	d0042aa275	FW defaults: remove EKF2_GPS_CHECK custom FW setting We want to align the default over all vehicle types for this param. There are still some thresholds that are increased for FW. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2025-01-23 15:56:52 -05:00
Alex Klimaj	07e7c64e60	drivers/power_monitor/ina238: retry if read fails * ina238: retry if read fails * ina238: increase retries and only publish not connected if register check fails * ina238: use I2C resets	2025-01-22 15:32:26 -05:00
Daniel Agar	6322ebc3db	rcS: shift sensors + EKF startup earlier - we want the drivers, sensors hub, and estimator running as soon as possible to initialize and avoid commander false positives complaining about missing data	2025-01-22 15:27:56 -05:00
PX4 BuildBot	752b25235c	boards: update all NuttX defconfigs	2025-01-22 14:46:36 -05:00
PX4 BuildBot	1aa83d954b	update all px4board kconfig	2025-01-22 14:46:24 -05:00
Julian Oes	e3b98e6ed2	Tools: remove unused/wrong define and fix comment	2025-01-22 11:26:10 -05:00
Roman Bapst	045c8d9831	Mission feasibility checks: make adding new check less error prone (#24241 ) * make adding new feasibility checks less prone to errors Signed-off-by: RomanBapst <bapstroman@gmail.com> * Update src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp Co-authored-by: Silvan Fuhrer <silvan@auterion.com> --------- Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2025-01-22 16:56:31 +01:00
RomanBapst	57fdda597b	vtol_takeoff: store altitude on takeoff and don't use home position altitude as vehicle does not need to be close to home position Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-22 14:03:18 +03:00
PavloZMN	ee150a15b4	Optical Flow: add unit testes for only using downward distance sensor (#23266 ) * Test for Optical Flow checks correct camera position * Formatting fixed * Update src/modules/sensors/vehicle_optical_flow/test/CMakeLists.txt Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com> * Update src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com> * Update src/modules/sensors/vehicle_optical_flow/test/VehicleOpticalFlowTest.cpp Co-authored-by: Silvan Fuhrer <silvan@auterion.com> * For test GIVEN/WHEN/THEN added --------- Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2025-01-22 11:58:07 +01:00
bresch	caaae6ed51	ekf2: allow sideslip fusion to always start with airspeed fusion This allow sideslip fusion to start during VTOL front transition or even on a multirotor with a vertical stabilizer and an airspeed sensor for example.	2025-01-22 09:33:16 +01:00
Jacob Dahl	1900d2c98f	uavcan: fix hw_errors from mutex lock, hide ESC/Servo status if no function set (#23888 )	2025-01-22 12:48:05 +13:00
Jacob Dahl	a3215419d7	gz: remove model spawn offset (#24165 )	2025-01-22 12:47:25 +13:00
Jacob Dahl	918eca8de4	gz: increase timeout for service request (#24164 ) * gz: increase timeout for service request * change error messages to warnings, specify retrying * fix typo	2025-01-21 22:14:38 +01:00
Alex Klimaj	8d1bfb77c6	boards: ark fpv add gimbal module (#24229 )	2025-01-21 08:59:37 -09:00
RomanBapst	f36b45b2ff	VehicleOpticalFlow: use 32bit integer for dt Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-21 16:48:33 +03:00
RomanBapst	c7e494b8d9	VehicleImu: use 32 bit integer for dt Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-21 16:48:33 +03:00
RomanBapst	25e76883b7	Integrator: use 32bit integer to store dt to avoid overflow Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-01-21 16:48:33 +03:00
Matthias Grob	af062c85eb	Revert "FLightTaskAuto: limit nudging speed based on distance sensor" This reverts commit `97cb933cff`.	2025-01-21 09:59:47 +01:00
Marco Hauswirth	7a9608e54b	increase EKF2_RNG_FOG for FW and VTOL	2025-01-20 14:43:30 -05:00
Marco Hauswirth	4a5aa1e947	Fix max-hagl restriction to position/altitude control (#23667 ) * fix max-hagl restriction to position/altitude control * max hagl vel restriction in ManAcc position mode * use interpolate func, change naming * simplyfied vertical vel limitation * move velocity-constraint adjustment to StickAccelXY	2025-01-20 15:50:21 +01:00
Mathieu Bresciani	0d22905558	VTOL params: fix param name	2025-01-20 15:50:06 +01:00
Pedro Roque	b09340cc98	Control Allocator: Add option for metric allocation (skip normalization) (#24199 ) * add: metric allocation * add: actual files * rft: moved metric allocation to pseudo-inverse via flag with public method * del: removed metric allocation test and added test in pseudo-inverse testing * rft: deleted extra newline at the end of pseudo inverse test file * feat: removed unnecessary log include	2025-01-20 13:30:51 +01:00
mahimayoga	65a8cc0e0a	sf45: scale measured distance with pitch and roll. Calls function from ObstacleMath library that accounts for the vehicle's attitude w.r.t the obstacle. Obstacles are assumed to be flat, vertical walls.	2025-01-20 11:11:29 +01:00
mahimayoga	ab46502cbd	obstacle-math: add unit tests for project_distance_on_horizontal_plane function.	2025-01-20 11:11:29 +01:00
mahimayoga	29f981f14c	collision-prevention: extract rotation scaling function into new ObstacleMath library. New library created for static and/or repeated code across collision prevention and driver files.	2025-01-20 11:11:29 +01:00
Daniel Agar	b1ca0495e2	ekf2: yaw estimator additional validity checks	2025-01-20 09:45:41 +01:00
Balduin	1eb9434b8c	stream ATTITUDE_QUATERNION in low bandwidth mode	2025-01-17 11:20:12 +01:00
Matthias Grob	f693fab7c8	rc_update params: remove comment about default static mixing Was added back in 2014: `7441efde47`	2025-01-17 08:18:42 +01:00
alexklimaj	33841cf438	Update GPS submodule	2025-01-15 11:54:04 -05:00