uORB: Add subscription with single field selection

Fix SITL Test failure: Place PX4 instance runner after Gazebo server runner (#21230 )
* MAVSDK_Test_Runner: Place PX4 instance runner after Gazebo server runner - This was a nasty bug where starting PX4 instance first, then starting Gazebo server was causing PX4 instance' EKF to freak out, probably because it doesn't like getting data a while after it is started - Detailed breakdown is given here: https://github.com/PX4/PX4-Autopilot/issues/21229#issuecomment-1450761542 - This now guarantees that such edge case won't occur and MAVSDK test will run as it should * MAVSDK Test Runner: Retain comment within 79 character limit - To pass flake8 python style check * Update test/mavsdk_tests/mavsdk_test_runner.py Co-authored-by: Julian Oes <julian@oes.ch> --------- Co-authored-by: Julian Oes <julian@oes.ch>
2026-06-05 03:10:04 +08:00 · 2023-03-27 09:44:20 +02:00 · 2023-03-26 11:27:01 +09:00 · 2023-03-24 21:43:59 -04:00 · 2023-03-24 10:03:14 -04:00 · 2023-03-24 10:01:58 -04:00
163 changed files with 7549 additions and 6153 deletions
@@ -105,3 +105,6 @@ src/modules/simulator/simulator_config.h
 src/systemcmds/topic_listener/listener_generated.cpp

 !src/drivers/distance_sensor/broadcom/afbrs50/Lib/*
+
+# colcon
+log/
@@ -256,7 +256,7 @@ endif()

 set(package-contact "px4users@googlegroups.com")

-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)
@@ -104,8 +104,9 @@ These boards fully comply with Pixhawk Standard, and are maintained by the PX4-A
 These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.

 * [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/arkv6x.html)
-* [Hex Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
-* [Hex Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html)
+* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
 * [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
 * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
 * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
@@ -11,6 +11,8 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1
@@ -9,6 +9,8 @@

 param set-default MAV_TYPE 20

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default CA_AIRFRAME 4

 param set-default CA_ROTOR_COUNT 4
@@ -17,6 +17,8 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set UAVCAN_ENABLE 0
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
@@ -12,6 +12,8 @@

 . ${R}etc/init.d/rc.vtol_defaults

+param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters
+
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
@@ -7,7 +7,7 @@ import os
 class JsonOutput():
    def __init__(self, groups):
        all_json = {}
-        all_json['version'] = 1
+        all_json['version'] = 2
        component = {}
        all_json['components'] = {1: component} #1: autopilot component

@@ -35,7 +35,20 @@ function spawn_model() {
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
 	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
-	python3 ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+
+	set --
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/scripts/jinja_gen.py
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/${MODEL}/${MODEL}.sdf.jinja
+	set -- ${@} ${src_path}/Tools/simulation/gazebo-classic/sitl_gazebo-classic
+	set -- ${@} --mavlink_tcp_port $((4560+${N}))
+	set -- ${@} --mavlink_udp_port $((14560+${N}))
+	set -- ${@} --mavlink_id $((1+${N}))
+	set -- ${@} --gst_udp_port $((5600+${N}))
+	set -- ${@} --video_uri $((5600+${N}))
+	set -- ${@} --mavlink_cam_udp_port $((14530+${N}))
+	set -- ${@} --output-file /tmp/${MODEL}_${N}.sdf
+
+	python3 ${@}

 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"

@@ -106,7 +119,7 @@ if [ -z ${SCRIPT} ]; then
 	fi

 	while [ $n -lt $num_vehicles ]; do
-		spawn_model ${vehicle_model} $n
+		spawn_model ${vehicle_model} $(($n + 1))
 		n=$(($n + 1))
 	done
 else
@@ -127,7 +140,7 @@ else
 		m=0
 		while [ $m -lt ${target_number} ]; do
 			export PX4_SIM_MODEL=gazebo-classic_${target_vehicle}
-			spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y
+			spawn_model ${target_vehicle}${LABEL} $(($n + 1)) $target_x $target_y
 			m=$(($m + 1))
 			n=$(($n + 1))
 		done
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -39,7 +39,6 @@ CONFIG_UAVCANNODE_STATIC_PRESSURE=y
 CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_NSH_BUILTIN_APPS=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -26,7 +26,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_UAVCAN=y
@@ -15,6 +15,8 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -14,6 +14,8 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -209,7 +209,6 @@ CONFIG_STM32H7_SPI2=y
 CONFIG_STM32H7_SPI4=y
 CONFIG_STM32H7_SPI4_DMA=y
 CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
-CONFIG_STM32H7_SPI_DMA=y
 CONFIG_STM32H7_TIM1=y
 CONFIG_STM32H7_TIM3=y
 CONFIG_STM32H7_TIM4=y
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y
 CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
 CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 CONFIG_DRIVERS_QSHELL_QURT=y
+CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -12,7 +13,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_MUORB_SLPI=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_COMMANDER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_ORB_COMMUNICATOR=y
@@ -1,6 +1,7 @@
 CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_QSHELL_POSIX=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_DATAMAN=y
@@ -17,4 +18,3 @@ CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_ORB_COMMUNICATOR=y
-CONFIG_BOARD_ROOTFSDIR="/data/px4"
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -24,7 +24,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -26,7 +26,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -27,7 +27,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -23,13 +23,18 @@ CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
+# CONFIG_EKF2_AUXVEL is not set
+# CONFIG_EKF2_BARO_COMPENSATION is not set
+# CONFIG_EKF2_DRAG_FUSION is not set
+# CONFIG_EKF2_EXTERNAL_VISION is not set
+# CONFIG_EKF2_GNSS_YAW is not set
+# CONFIG_EKF2_SIDESLIP is not set
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_SYSTEMCMDS_BL_UPDATE=n
@@ -1,6 +1,5 @@
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
 CONFIG_MODULES_MC_POS_CONTROL=n
 CONFIG_MODULES_MC_RATE_CONTROL=n
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
@@ -31,7 +31,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -30,7 +30,6 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -29,7 +29,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -2,6 +2,6 @@
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
-CONFIG_CYPHAL_ESC_CONTROLLER=y
@@ -14,7 +14,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -35,7 +35,6 @@ CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_COMMON_RC=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -9,7 +9,6 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_AIRSPEED_SELECTOR=n
@@ -17,7 +17,6 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_EXAMPLES_FAKE_GPS=n
@@ -11,7 +11,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_PWM_INPUT=n
-CONFIG_DRIVERS_ROBOCLAW=n
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
@@ -141,12 +141,13 @@
 #define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
 #define HW_INFO_INIT_PREFIX    "V6C"

-#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0, 10 and Mini Sensor sets
 //                 Base/FMUM
 #define V6C00   HW_VER_REV(0x0,0x0) // FMUV6C,                 Rev 0  I2C4 External but with Internal devices
 #define V6C01   HW_VER_REV(0x0,0x1) // FMUV6C,                 Rev 1  I2C4 Internal I2C2 External
 #define V6C10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0  I2C4 External but with Internal devices
 #define V6C11   HW_VER_REV(0x1,0x1) // NO PX4IO,               Rev 1  I2C4 Internal I2C2 External
+#define V6C21   HW_VER_REV(0x2,0x1) // FMUV6CMini,             Rev 1  I2C4 Internal I2C2 External


 /* HEATER
@@ -79,6 +79,11 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 		.mandatory   = 1,
 		.connection  = px4_hw_con_onboard,
 	},
+	{
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
 };

 static const px4_hw_mft_item_t hw_mft_list_v0610[] = {
@@ -101,6 +106,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6C01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1
 	{V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 0 No PX4IO
 	{V6C11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // Rev 1 No PX4IO
+	{V6C21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // Rev 1 MINI
 };

 /************************************************************************************
@@ -46,7 +46,17 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION
 			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
 		}),
 	}),
-	initSPIHWVersion(V6C10, {
+	initSPIHWVersion(V6C01, {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}),
+		}, {GPIO::PortB, GPIO::Pin2}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4})
+		}),
+	}),
+	initSPIHWVersion(V6C21, {
 		initSPIBus(SPI::Bus::SPI1, {
 			initSPIDevice(DRV_GYR_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}),
 			initSPIDevice(DRV_ACC_DEVTYPE_BMI055,  SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}),
@@ -28,7 +28,6 @@ CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -25,7 +25,6 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
-CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_SMART_BATTERY_BATMON=y
 CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
@@ -4,7 +4,7 @@ project(googletest-download NONE)

 include(ExternalProject)
 ExternalProject_Add(googletest
-	URL https://github.com/google/googletest/archive/58d77fa8070e8cec2dc1ed015d66b454c8d78850.zip # 1.12.1
+	URL https://github.com/google/googletest/archive/b796f7d44681514f58a683a3a71ff17c94edb0c1.zip # 1.13
 	SOURCE_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
 	BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
 	CONFIGURE_COMMAND ""
@@ -46,4 +46,7 @@ add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src ${CMAKE_CURRENT_BINA
 get_target_property(GTEST_COMPILE_FLAGS gtest COMPILE_OPTIONS)
 list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-include")
 list(REMOVE_ITEM GTEST_COMPILE_FLAGS "visibility.h")
+# Remove float warnings added by PX4 which trigger in gtest-printers.h
+list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-Wdouble-promotion")
+list(REMOVE_ITEM GTEST_COMPILE_FLAGS "-Wfloat-equal")
 set_target_properties(gtest PROPERTIES COMPILE_OPTIONS "${GTEST_COMPILE_FLAGS}")
@@ -5,5 +5,3 @@ uint8 INDEX_YAW = 2

 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 float32[3] control
-
-float32 retract_gimbal
@@ -93,6 +93,13 @@ bool keystore_put_key(keystore_session_handle_t handle, uint8_t idx, uint8_t *ke

 void crypto_init(void);

+/*
+ * De-initialize hw level crypto
+ * This may be called to shut down hw level crypto
+ */
+
+void crypto_deinit(void);
+
 /*
 * Open a session for performing crypto functions
 * algorithm: The crypto algorithm to be used in this session
@@ -245,4 +245,71 @@ private:
 	T _data{};
 };

+template<auto member>
+class SubscriptionSelection;
+
+template <class T, class R, R T::*member>
+class SubscriptionSelection<member>: public Subscription
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param id The uORB metadata ORB_ID enum for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(ORB_ID id, uint8_t instance = 0) :
+		Subscription(id, instance)
+	{
+		_copySelection();
+	}
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(const orb_metadata *meta, uint8_t instance = 0) :
+		Subscription(meta, instance)
+	{
+		_copySelection();
+	}
+
+	~SubscriptionSelection() = default;
+
+	// no copy, assignment, move, move assignment
+	SubscriptionSelection(const SubscriptionSelection &) = delete;
+	SubscriptionSelection &operator=(const SubscriptionSelection &) = delete;
+	SubscriptionSelection(SubscriptionSelection &&) = delete;
+	SubscriptionSelection &operator=(SubscriptionSelection &&) = delete;
+
+	// update the embedded struct.
+	bool update()
+	{
+		bool updated = Subscription::updated();
+
+		if (updated) {
+			_copySelection();
+		}
+
+		return updated;
+	}
+
+	const R &get() const { return _data; }
+
+private:
+	void _copySelection()
+	{
+		T full_data;
+
+		if (copy(&full_data)) {
+			_data = full_data.*member;
+		}
+	}
+
+	R _data{};
+};
+
+
 } // namespace uORB
@@ -392,6 +392,10 @@ jump_to_app()

 #endif

+#ifdef BOOTLOADER_USE_SECURITY
+	crypto_deinit();
+#endif
+
 	/* just for paranoia's sake */
 	arch_flash_lock();

@@ -236,7 +236,7 @@ LeddarOne::open_serial_port(const speed_t speed)
 	uart_config.c_cflag |= (CS8 | CREAD | CLOCAL);

 	// Clear: echo, echo new line, canonical input and extended input.
-	uart_config.c_lflag &= (ECHO | ECHONL | ICANON | IEXTEN);
+	uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN);

 	// Clear ONLCR flag (which appends a CR for every LF).
 	uart_config.c_oflag &= ~ONLCR;
@@ -37,7 +37,7 @@
 #include <inttypes.h>
 #include <fcntl.h>
 #include <termios.h>
-#include <lib/systemlib/crc.h>
+#include <lib/crc/crc.h>

 #include <float.h>
 #include <mathlib/mathlib.h>
@@ -1208,7 +1208,7 @@ GPS::publish()

 		if (_report_gps_pos.jamming_state != _jamming_state) {

-			if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_OK) {
+			if (_report_gps_pos.jamming_state > sensor_gps_s::JAMMING_STATE_WARNING) {
 				PX4_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _report_gps_pos.jamming_state,
 					 (uint8_t)_report_gps_pos.jamming_indicator);
 			}
@@ -325,13 +325,7 @@ void RCInput::Run()
 			updateParams();
 		}

-		if (_vehicle_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
-
-			if (_vehicle_status_sub.copy(&vehicle_status)) {
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			}
-		}
+		_vehicle_status_arming_state_sub.update();

 		const hrt_abstime cycle_timestamp = hrt_absolute_time();

@@ -346,7 +340,7 @@ void RCInput::Run()
 				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)

-				if (!_rc_scan_locked && !_armed) {
+				if (!_rc_scan_locked && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 					if ((int)vcmd.param1 == 0) {
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;
@@ -758,7 +752,8 @@ void RCInput::Run()

 			_to_input_rc.publish(_rc_in);

-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)
+			   && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}

@@ -767,7 +762,7 @@ void RCInput::Run()
 		}

 		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && rc_updated && _rc_scan_locked
 		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
 		    && (_param_rc_input_proto.get() < 0)
 		   ) {
@@ -138,16 +138,13 @@ private:

 	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state>	_vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};

 	input_rc_s	_rc_in{};

 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};

-	bool _armed{false};
-
-
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};

 	int		_rcs_fd{-1};
@@ -59,6 +59,10 @@ extern void libtomcrypt_init(void);
 #define SECMEM_FREE XFREE
 #endif

+#define SHA256_HASHLEN 32
+#define OAEP_MAX_RSA_MODLEN 256  /* RSA2048 */
+#define OAEP_MAX_MSGLEN (OAEP_MAX_RSA_MODLEN - 2 * SHA256_HASHLEN - 2)
+
 /*
 * For now, this is just a dummy up/down counter for tracking open/close calls
 */
@@ -151,6 +155,10 @@ void crypto_init()
 	clear_key_cache();
 }

+void crypto_deinit()
+{
+}
+
 crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm)
 {
 	crypto_session_handle_t ret;
@@ -378,12 +386,22 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle,
 					  max_len);

 	} else {
-		// The key size, encrypted, is a multiple of minimum block size for the algorithm+key
-		size_t min_block = crypto_get_min_blocksize(handle, encryption_key_idx);
-		*max_len = key_sz / min_block * min_block;
+		switch (handle.algorithm) {

-		if (key_sz % min_block) {
-			*max_len += min_block;
+		case CRYPTO_RSA_OAEP:
+			/* The length is the RSA key modulus length, and the maximum plaintext
+			 * length is calculated from that. This is now just fixed for RSA2048,
+			 * but one could also parse the RSA key
+			 * (encryption_key_idx) here and calculate the lengths.
+			 */
+
+			*max_len = key_sz <= OAEP_MAX_MSGLEN ?  OAEP_MAX_RSA_MODLEN : 0;
+			ret = true;
+			break;
+
+		default:
+			*max_len = 0;
+			break;
 		}
 	}

@@ -423,24 +441,6 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx)
 		ret = 64;
 		break;

-	case CRYPTO_RSA_OAEP: {
-			rsa_key enc_key;
-			size_t pub_key_sz;
-			uint8_t *pub_key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &pub_key_sz);
-
-			initialize_tomcrypt();
-
-			if (pub_key &&
-			    rsa_import(pub_key, pub_key_sz, &enc_key) == CRYPT_OK) {
-				ret = ltc_mp.unsigned_size(enc_key.N);
-				rsa_free(&enc_key);
-
-			} else {
-				ret = 0;
-			}
-		}
-		break;
-
 	default:
 		ret = 1;
 	}
@@ -33,8 +33,8 @@ if DRIVERS_UAVCANNODE
        bool "Include GNSS fix"
        default n

-    config UAVCANNODE_HYDROMETER_MEASUREMENT
-        bool "Include hydrometer measurement"
+    config UAVCANNODE_HYGROMETER_MEASUREMENT
+        bool "Include hygrometer measurement"
        default n

    config UAVCANNODE_LIGHTS_COMMAND
@@ -52,9 +52,9 @@
 #include "Publishers/FlowMeasurement.hpp"
 #endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT

-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
+#if defined(UAVCANNODE_HYGROMETER_MEASUREMENT)
 #include "Publishers/HygrometerMeasurement.hpp"
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
+#endif // UAVCANNODE_HYGROMETER_MEASUREMENT

 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 #include "Publishers/GnssFix2.hpp"
@@ -360,9 +360,9 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events
 	_publisher_list.add(new FlowMeasurement(this, _node));
 #endif // CONFIG_UAVCANNODE_FLOW_MEASUREMENT

-#if defined(CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT)
+#if defined(UAVCANNODE_HYGROMETER_MEASUREMENT)
 	_publisher_list.add(new HygrometerMeasurement(this, _node));
-#endif // CONFIG_UAVCANNODE_HYDROMETER_MEASUREMENT
+#endif // UAVCANNODE_HYGROMETER_MEASUREMENT

 #if defined(CONFIG_UAVCANNODE_GNSS_FIX)
 	_publisher_list.add(new GnssFix2(this, _node));
@@ -53,7 +53,7 @@ else()
 	set(comp_metadata_param_uri_fallback ${comp_metadata_param_uri_board})
 endif()
 # arguments: type, metadata file, uri, fallback uri, optional translation uri
-list(APPEND comp_metadata_types "--type" "1,${PX4_BINARY_DIR}/parameters.json.xz,${comp_metadata_param_uri},${comp_metadata_param_uri_fallback},")
+list(APPEND comp_metadata_types "--type" "1,${PX4_BINARY_DIR}/parameters.json.xz,${comp_metadata_param_uri},${comp_metadata_param_uri_fallback},https://raw.githubusercontent.com/PX4/PX4-Metadata-Translations/main/translated/parameters_summary.json")


 set(comp_metadata_events_uri_board "${s3_url}/Firmware/{version}/${comp_metadata_board}/all_events.json.xz")
@@ -66,7 +66,7 @@ else()
 	set(comp_metadata_events_uri "mftp://etc/extras/all_events.json.xz")
 	set(comp_metadata_events_uri_fallback ${comp_metadata_events_uri_board})
 endif()
-list(APPEND comp_metadata_types "--type" "4,${PX4_BINARY_DIR}/events/all_events.json.xz,${comp_metadata_events_uri},${comp_metadata_events_uri_fallback},")
+list(APPEND comp_metadata_types "--type" "4,${PX4_BINARY_DIR}/events/all_events.json.xz,${comp_metadata_events_uri},${comp_metadata_events_uri_fallback},https://raw.githubusercontent.com/PX4/PX4-Metadata-Translations/main/translated/events_summary.json")

 set(comp_metadata_actuators_uri_board "${s3_url}/Firmware/{version}/${comp_metadata_board}/actuators.json.xz")
 list(FIND config_romfs_extra_dependencies "actuators_json" index)
@@ -1,5 +1,47 @@
 {
-    "version": 1,
+    "version": 2,
+    "translation": {
+        "items": {
+            "components": {
+                "items": {
+                    "*": {
+                        "items": {
+                            "enums": {
+                                "items": {
+                                    "*": {
+                                        "items": {
+                                            "entries": {
+                                                "items": {
+                                                    "*": {
+                                                        "translate": [ "description" ]
+                                                    }
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            },
+                            "event_groups": {
+                                "items": {
+                                    "*": {
+                                        "items": {
+                                            "events": {
+                                                "items": {
+                                                    "*": {
+                                                        "translate": [ "message", "description" ]
+                                                    }
+                                                }
+                                            }
+                                        }
+                                    }
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    },
    "components": {
        "1": {
            "namespace": "px4",
@@ -48,3 +48,4 @@ px4_add_unit_gtest(SRC math/FunctionsTest.cpp)
 px4_add_unit_gtest(SRC math/test/UtilitiesTest.cpp)
 px4_add_unit_gtest(SRC math/WelfordMeanTest.cpp)
 px4_add_unit_gtest(SRC math/WelfordMeanVectorTest.cpp)
+px4_add_unit_gtest(SRC math/MaxDistanceToCircleTest.cpp)
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/matrix/math.hpp>
+#include "Functions.hpp"
+
+#include "TrajMath.hpp"
+
+using namespace math;
+using matrix::Vector2f;
+
+TEST(MaxDistanceToCircle, noDirection)
+{
+	Vector2f pos;
+	Vector2f circle_center;
+	float radius = 3.f;
+	Vector2f direction;
+	float distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_TRUE(isnan(distance));
+}
+
+TEST(MaxDistanceToCircle, insideCircle)
+{
+	// North position, South direction
+	Vector2f pos(1.f, 0.f);
+	Vector2f circle_center;
+	float radius = 3.f;
+	Vector2f direction(-1.f, 0.f);
+	float distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, 4.f);
+
+	// North position, West direction (direction doesn't need to be unit length)
+	direction = Vector2f(0.f, -3.42f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, sqrtf(radius * radius - 1.f));
+
+	// SE position, directed towards the center
+	pos = Vector2f(-2.f, 1.f);
+	direction = Vector2f(2.f, -1.f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, pos.norm() + radius);
+}
+
+TEST(MaxDistanceToCircle, outsideCircle)
+{
+	// North position, South direction
+	Vector2f pos(4.f, 0.f);
+	Vector2f circle_center;
+	float radius = 3.f;
+	Vector2f direction(-1.f, 0.f);
+	float distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, pos(0) + radius);
+
+	// looking away from the circle
+	direction = Vector2f(0.f, -3.42f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_TRUE(isnan(distance));
+
+	// SE position, directed towards the center
+	pos = Vector2f(-4.f, 2.f);
+	direction = Vector2f(2.f, -1.f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, pos.norm() + radius);
+}
+
+TEST(MaxDistanceToCircle, onCircle)
+{
+	// South, looking North
+	Vector2f pos(-4.f, 1.f);
+	Vector2f circle_center(-1.f, 1.f);
+	float radius = 3.f;
+	Vector2f direction(1.f, 0.f);
+	float distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, 2.f * radius);
+
+	// looking tangent to the circle
+	direction = Vector2f(0.f, -3.42f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, 0.f);
+
+	// looking away from the circle
+	direction = Vector2f(-10.f, -3.42f);
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, 0.f);
+
+	// SE position, directed towards the center
+	pos = Vector2f(-sqrtf(2.f) / 2.f, sqrtf(2.f) / 2.f) * radius + circle_center;
+	direction = -pos;
+	distance = trajectory::getMaxDistanceToCircle(pos, circle_center, radius, direction);
+	EXPECT_FLOAT_EQ(distance, 2.f * radius);
+}
@@ -67,7 +67,7 @@ inline float computeMaxSpeedFromDistance(const float jerk, const float accel, co
 	float max_speed = 0.5f * (-b + sqrtf(sqr(b) - 4.0f * c));

 	// don't slow down more than the end speed, even if the conservative accel ramp time requests it
-	return max(max_speed, final_speed);
+	return fmaxf(max_speed, final_speed);
 }

 /* Compute the maximum tangential speed in a circle defined by two line segments of length "d"
@@ -110,5 +110,44 @@ inline float computeBrakingDistanceFromVelocity(const float velocity, const floa
 	return velocity * (velocity / (2.0f * accel) + accel_delay_max / jerk);
 }

+/* Compute the maximum distance between a point and a circle given a direction vector pointing from the point
+ * towards the circle. The point can be inside or outside the circle.
+ *                  _
+ *               ,=' '=,               __
+ *    P-->------/-------A   Distance = PA
+ *       Dir   |    x    |
+ *              \       /
+ *               "=,_,="
+ * Equation to solve: ||(point - circle_pos) + direction_unit * distance_to_circle|| = radius
+ *
+ * @param pos position of the point
+ * @param circle_pos position of the center of the circle
+ * @param radius radius of the circle
+ * @param direction vector pointing from the point towards the circle
+ *
+ * @return longest distance between the point to the circle in the direction indicated by the vector or NAN if the
+ * vector does not point towards the circle
+ */
+inline float getMaxDistanceToCircle(const matrix::Vector2f &pos, const matrix::Vector2f &circle_pos, float radius,
+				    const matrix::Vector2f &direction)
+{
+	matrix::Vector2f center_to_pos = pos - circle_pos;
+	const float b = 2.f * center_to_pos.dot(direction.unit_or_zero());
+	const float c = center_to_pos.norm_squared() - radius * radius;
+	const float delta = b * b - 4.f * c;
+
+	float distance_to_circle;
+
+	if (delta >= 0.f && direction.longerThan(0.f)) {
+		distance_to_circle = fmaxf((-b + sqrtf(delta)) / 2.f, 0.f);
+
+	} else {
+		// Never intersecting the circle
+		distance_to_circle = NAN;
+	}
+
+	return distance_to_circle;
+}
+
 } /* namespace traj */
 } /* namespace math */
@@ -92,6 +92,16 @@ struct Dual {
 		return (*this = *this / a);
 	}

+	bool operator==(const Dual<Scalar, N> &other) const
+	{
+		return isEqualF(value, other.value) && (derivative == other.derivative);
+	}
+
+	bool operator!=(const Dual<Scalar, N> &other) const
+	{
+		const Dual<Scalar, N> &self = *this;
+		return !(self == other);
+	}
 };

 // operators
@@ -359,23 +369,12 @@ Matrix<Scalar, M, N> collectReals(const Matrix<Dual<Scalar, D>, M, N> &input)
 	return r;
 }

-#if defined(SUPPORT_STDIOSTREAM)
-template<typename Type, size_t N>
-std::ostream &operator<<(std::ostream &os,
-			 const matrix::Dual<Type, N> &dual)
+template<typename OStream, typename Type, size_t N>
+OStream &operator<<(OStream &os, const matrix::Dual<Type, N> &dual)
 {
-	os << "[";
-	os << std::setw(10) << dual.value << ";";
-
-	for (size_t j = 0; j < N; ++j) {
-		os << "\t";
-		os << std::setw(10) << static_cast<double>(dual.derivative(j));
-	}
-
-	os << "]";
+	os << "\nValue: " << dual.value << "\nDerivative:" << dual.derivative;
 	return os;
 }
-#endif // defined(SUPPORT_STDIOSTREAM)

 }

@@ -12,11 +12,6 @@
 #include <cstdio>
 #include <cstring>

-#if defined(SUPPORT_STDIOSTREAM)
-#include <iostream>
-#include <iomanip>
-#endif // defined(SUPPORT_STDIOSTREAM)
-
 #include "math.hpp"

 namespace matrix
@@ -372,10 +367,9 @@ public:
 	{
 		// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
 		static const size_t n = 15 * N * M + M + 1;
-		char *buf = new char[n];
-		write_string(buf, n);
-		printf("%s\n", buf);
-		delete[] buf;
+		char string[n];
+		write_string(string, n);
+		printf("%s\n", string);
 	}

 	Matrix<Type, N, M> transpose() const
@@ -816,24 +810,16 @@ Matrix<Type, M, N> constrain(const Matrix<Type, M, N> &x,
 	return m;
 }

-#if defined(SUPPORT_STDIOSTREAM)
-template<typename Type, size_t  M, size_t N>
-std::ostream &operator<<(std::ostream &os,
-			 const matrix::Matrix<Type, M, N> &matrix)
+template<typename OStream, typename Type, size_t M, size_t N>
+OStream &operator<<(OStream &os, const matrix::Matrix<Type, M, N> &matrix)
 {
-	for (size_t i = 0; i < M; ++i) {
-		os << "[";
-
-		for (size_t j = 0; j < N; ++j) {
-			os << std::setw(10) << matrix(i, j);
-			os << "\t";
-		}
-
-		os << "]" << std::endl;
-	}
-
+	os << "\n";
+	// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
+	static const size_t n = 15 * N * M + M + 1;
+	char string[n];
+	matrix.write_string(string, n);
+	os << string;
 	return os;
 }
-#endif // defined(SUPPORT_STDIOSTREAM)

 } // namespace matrix
@@ -50,7 +50,7 @@ class AxisAngle;
 * described by this class.
 */
 template<typename Type>
-class Quaternion : public Vector<Type, 4>
+class Quaternion : public Vector4<Type>
 {
 public:
 	using Matrix41 = Matrix<Type, 4, 1>;
@@ -62,7 +62,7 @@ public:
 	 * @param data_ array
 	 */
 	explicit Quaternion(const Type data_[4]) :
-		Vector<Type, 4>(data_)
+		Vector4<Type>(data_)
 	{
 	}

@@ -84,7 +84,7 @@ public:
 	 * @param other Matrix41 to copy
 	 */
 	Quaternion(const Matrix41 &other) :
-		Vector<Type, 4>(other)
+		Vector4<Type>(other)
 	{
 	}

@@ -148,6 +148,23 @@ public:

 		return r;
 	}
+
+	void print() const
+	{
+		(*this).transpose().print();
+	}
 };

+template<typename OStream, typename Type, size_t M>
+OStream &operator<<(OStream &os, const matrix::Vector<Type, M> &vector)
+{
+	os << "\n";
+	// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
+	static const size_t n = 15 * M * 1 + 1 + 1;
+	char string[n];
+	vector.transpose().write_string(string, n);
+	os << string;
+	return os;
+}
+
 } // namespace matrix
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector4.hpp
+ *
+ * 4D vector class.
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include "math.hpp"
+
+namespace matrix
+{
+
+template <typename Type, size_t M, size_t N>
+class Matrix;
+
+template <typename Type, size_t M>
+class Vector;
+
+template<typename Type>
+class Vector4 : public Vector<Type, 4>
+{
+public:
+	using Matrix41 = Matrix<Type, 4, 1>;
+
+	Vector4() = default;
+
+	Vector4(const Matrix41 &other) :
+		Vector<Type, 4>(other)
+	{
+	}
+
+	explicit Vector4(const Type data_[3]) :
+		Vector<Type, 4>(data_)
+	{
+	}
+
+	Vector4(Type x1, Type x2, Type x3, Type x4)
+	{
+		Vector4 &v(*this);
+		v(0) = x1;
+		v(1) = x2;
+		v(2) = x3;
+		v(3) = x4;
+	}
+
+	template<size_t P, size_t Q>
+	Vector4(const Slice<Type, 4, 1, P, Q> &slice_in) : Vector<Type, 4>(slice_in)
+	{
+	}
+
+	template<size_t P, size_t Q>
+	Vector4(const Slice<Type, 1, 4, P, Q> &slice_in) : Vector<Type, 4>(slice_in)
+	{
+	}
+};
+
+using Vector4f = Vector4<float>;
+
+} // namespace matrix
@@ -11,6 +11,7 @@
 #include "Vector.hpp"
 #include "Vector2.hpp"
 #include "Vector3.hpp"
+#include "Vector4.hpp"
 #include "Euler.hpp"
 #include "Dcm.hpp"
 #include "Scalar.hpp"
@@ -260,7 +260,7 @@ TEST(MatrixAttitudeTest, Attitude)

 	// quaterion copy ctors
 	float data_v4[] = {1, 2, 3, 4};
-	Vector<float, 4> v4(data_v4);
+	Vector4f v4(data_v4);
 	Quatf q_from_v(v4);
 	EXPECT_EQ(q_from_v, v4);

@@ -270,15 +270,13 @@ TEST(MatrixAttitudeTest, Attitude)

 	// quaternion derivative in frame 1
 	Quatf q1(0, 1, 0, 0);
-	Vector<float, 4> q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
-	float data_q_dot1_check[] = { -0.5f, 0.0f, -1.5f, 1.0f};
-	Vector<float, 4> q1_dot1_check(data_q_dot1_check);
+	Vector4f q1_dot1 = q1.derivative1(Vector3f(1, 2, 3));
+	Vector4f q1_dot1_check(-0.5f, 0.0f, -1.5f, 1.0f);
 	EXPECT_EQ(q1_dot1, q1_dot1_check);

 	// quaternion derivative in frame 2
-	Vector<float, 4> q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
-	float data_q_dot2_check[] = { -0.5f, 0.0f, 1.5f, -1.0f};
-	Vector<float, 4> q1_dot2_check(data_q_dot2_check);
+	Vector4f q1_dot2 = q1.derivative2(Vector3f(1, 2, 3));
+	Vector4f q1_dot2_check(-0.5f, 0.0f, 1.5f, -1.0f);
 	EXPECT_EQ(q1_dot2, q1_dot2_check);

 	// quaternion product
@@ -39,12 +39,6 @@

 using namespace matrix;

-template <typename Scalar, size_t N>
-bool isEqualAll(Dual<Scalar, N> a, Dual<Scalar, N> b)
-{
-	return isEqualF(a.value, b.value) && a.derivative == b.derivative;
-}
-
 template <typename T>
 T testFunction(const Vector<T, 3> &point)
 {
@@ -83,9 +77,9 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_FLOAT_EQ(c.derivative(0), 2.f);

 		Dual<float, 1> d = +a;
-		EXPECT_TRUE(isEqualAll(d, a));
+		EXPECT_EQ(d, a);
 		d += b;
-		EXPECT_TRUE(isEqualAll(d, c));
+		EXPECT_EQ(d, c);

 		Dual<float, 1> e = a;
 		e += b.value;
@@ -93,7 +87,7 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_EQ(e.derivative, a.derivative);

 		Dual<float, 1> f = b.value + a;
-		EXPECT_TRUE(isEqualAll(f, e));
+		EXPECT_EQ(f, e);
 	}

 	{
@@ -103,9 +97,9 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_FLOAT_EQ(c.derivative(0), 0.f);

 		Dual<float, 1> d = b;
-		EXPECT_TRUE(isEqualAll(d, b));
+		EXPECT_EQ(d, b);
 		d -= a;
-		EXPECT_TRUE(isEqualAll(d, c));
+		EXPECT_EQ(d, c);

 		Dual<float, 1> e = b;
 		e -= a.value;
@@ -113,7 +107,7 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_EQ(e.derivative, b.derivative);

 		Dual<float, 1> f = a.value - b;
-		EXPECT_TRUE(isEqualAll(f, -e));
+		EXPECT_EQ(f, -e);
 	}

 	{
@@ -123,9 +117,9 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_FLOAT_EQ(c.derivative(0), 9.f);

 		Dual<float, 1> d = a;
-		EXPECT_TRUE(isEqualAll(d, a));
+		EXPECT_EQ(d, a);
 		d *= b;
-		EXPECT_TRUE(isEqualAll(d, c));
+		EXPECT_EQ(d, c);

 		Dual<float, 1> e = a;
 		e *= b.value;
@@ -133,7 +127,7 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_EQ(e.derivative, a.derivative * b.value);

 		Dual<float, 1> f = b.value * a;
-		EXPECT_TRUE(isEqualAll(f, e));
+		EXPECT_EQ(f, e);
 	}

 	{
@@ -143,9 +137,9 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_FLOAT_EQ(c.derivative(0), -1.f / 3.f);

 		Dual<float, 1> d = b;
-		EXPECT_TRUE(isEqualAll(d, b));
+		EXPECT_EQ(d, b);
 		d /= a;
-		EXPECT_TRUE(isEqualAll(d, c));
+		EXPECT_EQ(d, c);

 		Dual<float, 1> e = b;
 		e /= a.value;
@@ -153,7 +147,7 @@ TEST(MatrixDualTest, Dual)
 		EXPECT_EQ(e.derivative, b.derivative / a.value);

 		Dual<float, 1> f = a.value / b;
-		EXPECT_TRUE(isEqualAll(f, 1.f / e));
+		EXPECT_EQ(f, 1.f / e);
 	}

 	{
@@ -170,9 +164,9 @@ TEST(MatrixDualTest, Dual)

 	{
 		// abs
-		EXPECT_TRUE(isEqualAll(a, abs(-a)));
-		EXPECT_FALSE(isEqualAll(-a, abs(a)));
-		EXPECT_TRUE(isEqualAll(-a, -abs(a)));
+		EXPECT_EQ(a, abs(-a));
+		EXPECT_NE(-a, abs(a));
+		EXPECT_EQ(-a, -abs(a));
 	}

 	{
@@ -197,8 +191,8 @@ TEST(MatrixDualTest, Dual)

 	{
 		// max/min
-		EXPECT_TRUE(isEqualAll(b, max(a, b)));
-		EXPECT_TRUE(isEqualAll(a, min(a, b)));
+		EXPECT_EQ(b, max(a, b));
+		EXPECT_EQ(a, min(a, b));
 	}

 	{
@@ -248,7 +242,7 @@ TEST(MatrixDualTest, Dual)
 	{
 		// atan2
 		EXPECT_FLOAT_EQ(atan2(a, b).value, atan2(a.value, b.value));
-		EXPECT_TRUE(isEqualAll(atan2(a, Dual<float, 1>(b.value)), atan(a / b.value))); // atan2'(y, x) = atan'(y/x)
+		EXPECT_EQ(atan2(a, Dual<float, 1>(b.value)), atan(a / b.value)); // atan2'(y, x) = atan'(y/x)
 	}

 	{
@@ -50,19 +50,12 @@ TEST(MatrixLeastSquaresTest, 4x3)
 			  -1.f,  -1.1f,  -1.2f
 			 };
 	Matrix<float, 4, 3> A(data);
-
-	float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
-	Vector<float, 4> b(b_data);
-
-	float x_check_data[3] = {-0.69168233f,
-				 -0.26227593f,
-				 -1.03767522f
-				};
-	Vector<float, 3> x_check(x_check_data);
+	Vector4f b(2.0f, 3.0f, 4.0f, 5.0f);
+	Vector3f x_check(-0.69168233f, -0.26227593f, -1.03767522f);

 	LeastSquaresSolver<float, 4, 3> qrd = LeastSquaresSolver<float, 4, 3>(A);

-	Vector<float, 3> x = qrd.solve(b);
+	Vector3f x = qrd.solve(b);
 	EXPECT_EQ(x, x_check);
 }

@@ -75,20 +68,12 @@ TEST(MatrixLeastSquaresTest, 4x4)
 				 -1.f,  -1.1f,  -1.2f,  -1.3f
 			       };
 	Matrix<float, 4, 4> A(data);
-
-	float b_data[4] = {2.0f, 3.0f, 4.0f, 5.0f};
-	Vector<float, 4> b(b_data);
-
-	float x_check_data[4] = { 0.97893433f,
-				  -2.80798701f,
-				  -0.03175765f,
-				  -2.19387649f
-				};
-	Vector<float, 4> x_check(x_check_data);
+	Vector4f b(2.0f, 3.0f, 4.0f, 5.0f);
+	Vector4f x_check(0.97893433f, -2.80798701f, -0.03175765f, -2.19387649f);

 	LeastSquaresSolver<float, 4, 4> qrd = LeastSquaresSolver<float, 4, 4>(A);

-	Vector<float, 4> x = qrd.solve(b);
+	Vector4f x = qrd.solve(b);
 	EXPECT_EQ(x, x_check);
 }

@@ -92,11 +92,11 @@ TEST(MatrixSparseVectorTest, setZero)

 TEST(MatrixSparseVectorTest, additionWithDenseVector)
 {
-	Vector<float, 4> dense_vec;
+	Vector4f dense_vec;
 	dense_vec.setAll(1.f);
 	const float data[3] = {1.f, 2.f, 3.f};
 	const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
-	const Vector<float, 4> res = sparse_vec + dense_vec;
+	const Vector4f res = sparse_vec + dense_vec;
 	EXPECT_FLOAT_EQ(res(0), 1.f);
 	EXPECT_FLOAT_EQ(res(1), 2.f);
 	EXPECT_FLOAT_EQ(res(2), 3.f);
@@ -115,7 +115,7 @@ TEST(MatrixSparseVectorTest, addScalar)

 TEST(MatrixSparseVectorTest, dotProductWithDenseVector)
 {
-	Vector<float, 4> dense_vec;
+	Vector4f dense_vec;
 	dense_vec.setAll(3.f);
 	const float data[3] = {1.f, 2.f, 3.f};
 	const SparseVectorf<4, 1, 2, 3> sparse_vec(data);
@@ -71,13 +71,7 @@ TEST(MatrixVector3Test, Vector3)

 	Vector3f h;
 	EXPECT_EQ(h, Vector3f(0, 0, 0));
-
-	Vector<float, 4> j;
-	j(0) = 1;
-	j(1) = 2;
-	j(2) = 3;
-	j(3) = 4;
-
+	Vector4f j(1.f, 2.f, 3.f, 4.f);
 	Vector3f k = j.slice<3, 1>(0, 0);
 	Vector3f k_test(1, 2, 3);
 	EXPECT_EQ(k, k_test);
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <matrix/math.hpp>
+
+using namespace matrix;
+
+TEST(MatrixVector4Test, Vector4)
+{
+	Vector4f a(1.f, 2.f, 3.f, 4.f);
+	EXPECT_EQ(a(0), 1.f);
+	EXPECT_EQ(a(1), 2.f);
+	EXPECT_EQ(a(2), 3.f);
+	EXPECT_EQ(a(3), 4.f);
+}
@@ -11,6 +11,32 @@ class JsonOutput():
        all_params=[]
        all_json['parameters']=all_params

+        all_json["translation"] = {
+            "items": {
+                "parameters": {
+                    "list": {
+                        "key": "name",
+                        "translate": [ "shortDesc", "longDesc" ],
+                        "translate-global": ["category", "group"],
+                        "items": {
+                            "bitmask": {
+                                "list": {
+                                    "key": "index",
+                                    "translate": [ "description" ]
+                                }
+                            },
+                            "values": {
+                                "list": {
+                                    "key": "value",
+                                    "translate": [ "description" ]
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
        schema_map = {
                        "short_desc": "shortDesc",
 			"long_desc": "longDesc",
@@ -61,9 +61,8 @@ TEST_F(ArxRlsTest, test211)
 	_rls.update(1, 2);
 	_rls.update(3, 4);
 	_rls.update(5, 6);
-	const Vector<float, 4> coefficients = _rls.getCoefficients();
-	float data_check[] = {-1.79f, 0.97f, 0.42f, -0.48f}; // generated from Python script
-	const Vector<float, 4> coefficients_check(data_check);
+	const Vector4f coefficients = _rls.getCoefficients();
+	const Vector4f coefficients_check(-1.79f, 0.97f, 0.42f, -0.48f); // generated from Python script
 	float eps = 1e-2;
 	EXPECT_TRUE((coefficients - coefficients_check).abs().max() < eps);
 }
@@ -47,80 +47,80 @@ static constexpr int LON_DIM = 37;
 // Magnetic declination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.074,
+// Date: 2023.2246,
 static constexpr const int16_t declination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ {  25974, 24228, 22483, 20738, 18992, 17247, 15502, 13756, 12011, 10266,  8520,  6775,  5030,  3285,  1539,  -206, -1951, -3697, -5442, -7187, -8932,-10678,-12423,-14168,-15914,-17659,-19404,-21150,-22895,-24641,-26386,-28131,-29877, 31210, 29464, 27719, 25974, },
-	/* LAT: -80 */ {  22539, 20409, 18470, 16697, 15057, 13519, 12055, 10644,  9269,  7919,  6588,  5269,  3960,  2655,  1348,    28, -1314, -2686, -4097, -5550, -7045, -8583,-10161,-11781,-13446,-15165,-16951,-18824,-20807,-22927,-25201,-27636,-30204, 29989, 27372, 24868, 22539, },
-	/* LAT: -70 */ {  14980, 13581, 12454, 11491, 10621,  9788,  8946,  8058,  7105,  6086,  5016,  3923,  2838,  1784,   762,  -247, -1285, -2392, -3595, -4897, -6279, -7711, -9160,-10603,-12030,-13447,-14876,-16363,-17989,-19907,-22434,-26242, 30671, 24126, 19621, 16851, 14980, },
-	/* LAT: -60 */ {   8431,  8183,  7900,  7624,  7369,  7114,  6804,  6370,  5754,  4934,  3932,  2821,  1702,   676,  -203,  -958, -1687, -2511, -3518, -4720, -6054, -7427, -8749, -9960,-11024,-11923,-12637,-13132,-13304,-12855,-10750, -3465,  4953,  7682,  8446,  8568,  8431, },
-	/* LAT: -50 */ {   5493,  5528,  5471,  5380,  5305,  5267,  5231,  5103,  4758,  4093,  3081,  1808,   468,  -708, -1573, -2132, -2525, -2967, -3658, -4680, -5931, -7213, -8355, -9255, -9851,-10086, -9887, -9126, -7614, -5249, -2342,   406,  2516,  3941,  4815,  5289,  5493, },
-	/* LAT: -40 */ {   3960,  4053,  4059,  4013,  3952,  3917,  3921,  3909,  3735,  3198,  2172,   728,  -835, -2144, -3000, -3444, -3621, -3678, -3852, -4435, -5425, -6507, -7392, -7926, -8027, -7642, -6746, -5359, -3650, -1951,  -494,   727,  1771,  2639,  3297,  3728,  3960, },
-	/* LAT: -30 */ {   2988,  3074,  3103,  3087,  3027,  2948,  2887,  2852,  2725,  2245,  1205,  -318, -1925, -3172, -3904, -4243, -4315, -4097, -3662, -3454, -3836, -4592, -5288, -5631, -5498, -4904, -3940, -2735, -1532,  -593,    81,   681,  1306,  1912,  2424,  2788,  2988, },
-	/* LAT: -20 */ {   2346,  2391,  2407,  2407,  2363,  2267,  2158,  2082,  1936,  1438,   377, -1116, -2593, -3648, -4172, -4280, -4070, -3504, -2640, -1851, -1591, -1957, -2607, -3071, -3101, -2736, -2098, -1284,  -514,   -30,   229,   529,   972,  1454,  1880,  2191,  2346, },
-	/* LAT: -10 */ {   1952,  1946,  1923,  1917,  1888,  1801,  1689,  1597,  1412,   860,  -206, -1586, -2856, -3678, -3935, -3699, -3119, -2335, -1489,  -739,  -282,  -324,  -798, -1303, -1511, -1418, -1103,  -605,  -113,   123,   168,   326,   699,  1142,  1540,  1831,  1952, },
-	/* LAT:   0 */ {   1739,  1705,  1647,  1636,  1624,  1553,  1445,  1330,  1075,   454,  -589, -1816, -2866, -3445, -3435, -2928, -2158, -1377,  -727,  -193,   217,   323,    39,  -378,  -630,  -684,  -586,  -326,   -39,    46,   -19,    67,   411,   859,  1283,  1607,  1739, },
-	/* LAT:  10 */ {   1601,  1609,  1565,  1580,  1604,  1553,  1429,  1240,   864,   146,  -875, -1949, -2771, -3104, -2893, -2282, -1505,  -796,  -283,    96,   421,   566,   401,    78,  -155,  -262,  -288,  -209,  -108,  -153,  -296,  -271,    33,   493,   978,  1389,  1601, },
-	/* LAT:  20 */ {   1414,  1563,  1623,  1714,  1799,  1777,  1620,  1316,   772,   -84, -1125, -2073, -2665, -2769, -2437, -1833, -1122,  -474,   -16,   292,   550,   694,   601,   357,   159,    40,   -55,  -123,  -206,  -398,  -638,  -697,  -461,   -10,   535,  1056,  1414, },
-	/* LAT:  30 */ {   1108,  1475,  1735,  1959,  2118,  2128,  1939,  1516,   789,  -238, -1352, -2225, -2635, -2570, -2169, -1589,  -933,  -316,   144,   449,   679,   824,   802,   652,   504,   378,   217,     1,  -285,  -662, -1032, -1191, -1030,  -601,   -23,   587,  1108, },
-	/* LAT:  40 */ {    746,  1331,  1826,  2219,  2469,  2513,  2298,  1765,   855,  -368, -1602, -2463, -2780, -2626, -2180, -1587,  -934,  -305,   204,   568,   837,  1034,  1125,  1114,  1038,   888,   615,   198,  -346,  -954, -1475, -1718, -1599, -1181,  -583,    88,   746, },
-	/* LAT:  50 */ {    449,  1195,  1877,  2436,  2805,  2913,  2686,  2034,   897,  -593, -2008, -2918, -3211, -3019, -2529, -1883, -1173,  -476,   139,   644,  1062,  1415,  1693,  1862,  1880,  1689,  1238,   523,  -377, -1280, -1957, -2240, -2113, -1673, -1040,  -312,   449, },
-	/* LAT:  60 */ {    244,  1095,  1901,  2599,  3107,  3324,  3116,  2312,   809, -1137, -2844, -3822, -4075, -3817, -3246, -2502, -1676,  -837,   -30,   721,  1412,  2042,  2583,  2976,  3133,  2940,  2293,  1167,  -254, -1573, -2437, -2742, -2576, -2084, -1396,  -601,   244, },
-	/* LAT:  70 */ {     -8,   925,  1822,  2622,  3241,  3541,  3301,  2190,    -8, -2686, -4631, -5465, -5483, -5007, -4243, -3312, -2290, -1226,  -152,   907,  1931,  2896,  3763,  4468,  4912,  4938,  4317,  2828,   621, -1489, -2783, -3223, -3065, -2537, -1794,  -930,    -8, },
-	/* LAT:  80 */ {   -771,   149,  1000,  1691,  2080,  1915,   776, -1705, -4800, -6913, -7700, -7604, -7000, -6100, -5021, -3830, -2570, -1269,    54,  1379,  2692,  3974,  5202,  6341,  7339,  8100,  8444,  7991,  6025,  2161, -1488, -3200, -3553, -3219, -2541, -1692,  -771, },
-	/* LAT:  90 */ { -29638,-27893,-26147,-24402,-22657,-20911,-19166,-17421,-15675,-13930,-12185,-10440, -8695, -6949, -5204, -3459, -1714,    31,  1776,  3521,  5267,  7012,  8757, 10503, 12248, 13993, 15739, 17484, 19229, 20975, 22720, 24466, 26211, 27957, 29702,-31384,-29638, },
+	/* LAT: -90 */ {  25970, 24224, 22479, 20734, 18988, 17243, 15498, 13752, 12007, 10262,  8517,  6771,  5026,  3281,  1535,  -210, -1955, -3700, -5446, -7191, -8936,-10682,-12427,-14172,-15918,-17663,-19408,-21154,-22899,-24644,-26390,-28135,-29880, 31206, 29461, 27715, 25970, },
+	/* LAT: -80 */ {  22534, 20405, 18467, 16694, 15054, 13516, 12053, 10642,  9267,  7917,  6585,  5267,  3958,  2653,  1346,    26, -1316, -2689, -4100, -5553, -7049, -8586,-10165,-11785,-13450,-15170,-16956,-18829,-20813,-22932,-25208,-27642,-30211, 29983, 27366, 24863, 22534, },
+	/* LAT: -70 */ {  14981, 13582, 12454, 11491, 10621,  9788,  8945,  8057,  7103,  6084,  5014,  3921,  2837,  1783,   762,  -248, -1286, -2393, -3597, -4899, -6283, -7715, -9164,-10608,-12035,-13452,-14881,-16369,-17996,-19915,-22444,-26255, 30657, 24120, 19621, 16852, 14981, },
+	/* LAT: -60 */ {   8435,  8187,  7903,  7626,  7370,  7114,  6804,  6370,  5753,  4932,  3930,  2819,  1700,   675,  -202,  -957, -1686, -2510, -3519, -4722, -6058, -7431, -8754, -9965,-11029,-11927,-12642,-13136,-13308,-12859,-10752, -3452,  4968,  7692,  8453,  8574,  8435, },
+	/* LAT: -50 */ {   5496,  5532,  5474,  5382,  5306,  5267,  5231,  5103,  4757,  4092,  3079,  1805,   466,  -709, -1572, -2129, -2522, -2964, -3657, -4682, -5935, -7218, -8360, -9260, -9854,-10088, -9888, -9125, -7612, -5246, -2339,   410,  2520,  3945,  4819,  5293,  5496, },
+	/* LAT: -40 */ {   3963,  4055,  4061,  4015,  3952,  3917,  3921,  3909,  3734,  3196,  2169,   724,  -838, -2146, -3000, -3442, -3617, -3673, -3849, -4435, -5429, -6513, -7397, -7930, -8029, -7642, -6744, -5356, -3647, -1949,  -493,   728,  1772,  2641,  3299,  3731,  3963, },
+	/* LAT: -30 */ {   2990,  3076,  3105,  3088,  3027,  2947,  2886,  2851,  2724,  2242,  1201,  -323, -1930, -3175, -3905, -4241, -4312, -4092, -3656, -3452, -3839, -4597, -5292, -5633, -5498, -4902, -3937, -2732, -1530,  -592,    81,   680,  1306,  1912,  2425,  2790,  2990, },
+	/* LAT: -20 */ {   2348,  2393,  2409,  2407,  2363,  2266,  2157,  2080,  1934,  1436,   372, -1122, -2599, -3651, -4172, -4278, -4066, -3498, -2633, -1847, -1591, -1960, -2610, -3072, -3100, -2734, -2096, -1282,  -513,   -30,   228,   528,   971,  1455,  1881,  2192,  2348, },
+	/* LAT: -10 */ {   1954,  1948,  1924,  1917,  1887,  1800,  1687,  1595,  1410,   857,  -211, -1592, -2861, -3680, -3934, -3696, -3114, -2329, -1484,  -735,  -280,  -324,  -800, -1304, -1511, -1416, -1101,  -604,  -112,   123,   166,   324,   698,  1142,  1540,  1832,  1954, },
+	/* LAT:   0 */ {   1741,  1707,  1648,  1637,  1623,  1551,  1443,  1327,  1072,   450,  -593, -1821, -2870, -3446, -3432, -2923, -2153, -1372,  -723,  -190,   220,   324,    39,  -378,  -629,  -683,  -585,  -325,   -39,    45,   -22,    64,   409,   858,  1284,  1609,  1741, },
+	/* LAT:  10 */ {   1603,  1611,  1566,  1580,  1604,  1552,  1426,  1237,   861,   143,  -878, -1953, -2773, -3103, -2890, -2278, -1500,  -791,  -279,    99,   423,   568,   401,    79,  -154,  -262,  -287,  -210,  -109,  -154,  -299,  -274,    31,   492,   978,  1390,  1603, },
+	/* LAT:  20 */ {   1415,  1564,  1623,  1713,  1798,  1776,  1617,  1313,   768,   -87, -1128, -2075, -2665, -2767, -2433, -1828, -1117,  -470,   -13,   295,   553,   696,   602,   358,   160,    41,   -54,  -124,  -207,  -400,  -641,  -700,  -463,   -11,   535,  1056,  1415, },
+	/* LAT:  30 */ {   1108,  1474,  1734,  1958,  2117,  2126,  1937,  1513,   786,  -241, -1354, -2225, -2634, -2567, -2165, -1584,  -929,  -312,   148,   452,   682,   826,   803,   654,   505,   379,   217,     0,  -287,  -664, -1035, -1194, -1031,  -602,   -24,   587,  1108, },
+	/* LAT:  40 */ {    744,  1329,  1824,  2217,  2467,  2511,  2296,  1762,   853,  -370, -1602, -2461, -2777, -2622, -2175, -1582,  -929,  -300,   208,   571,   840,  1036,  1127,  1115,  1039,   889,   615,   196,  -348,  -957, -1478, -1720, -1601, -1183,  -584,    86,   744, },
+	/* LAT:  50 */ {    445,  1191,  1873,  2433,  2802,  2911,  2683,  2032,   895,  -593, -2006, -2914, -3205, -3013, -2523, -1877, -1167,  -470,   144,   649,  1066,  1418,  1696,  1864,  1881,  1690,  1238,   520,  -380, -1284, -1960, -2242, -2115, -1674, -1042,  -315,   445, },
+	/* LAT:  60 */ {    238,  1088,  1894,  2593,  3102,  3320,  3112,  2310,   809, -1133, -2837, -3814, -4067, -3809, -3238, -2494, -1669,  -830,   -23,   727,  1418,  2047,  2587,  2980,  3135,  2941,  2292,  1163,  -259, -1578, -2440, -2744, -2578, -2087, -1400,  -606,   238, },
+	/* LAT:  70 */ {    -19,   913,  1810,  2611,  3230,  3532,  3294,  2188,    -1, -2670, -4613, -5448, -5469, -4994, -4231, -3301, -2280, -1217,  -144,   914,  1938,  2903,  3769,  4473,  4916,  4940,  4317,  2823,   612, -1498, -2790, -3229, -3071, -2544, -1802,  -940,   -19, },
+	/* LAT:  80 */ {   -798,   122,   973,  1664,  2054,  1893,   767, -1687, -4757, -6867, -7661, -7573, -6974, -6078, -5002, -3813, -2554, -1254,    67,  1392,  2705,  3986,  5214,  6353,  7351,  8112,  8455,  8000,  6023,  2136, -1525, -3233, -3583, -3246, -2567, -1718,  -798, },
+	/* LAT:  90 */ { -29581,-27835,-26090,-24344,-22599,-20854,-19108,-17363,-15618,-13872,-12127,-10382, -8637, -6892, -5147, -3401, -1656,    89,  1834,  3579,  5324,  7070,  8815, 10560, 12306, 14051, 15796, 17542, 19287, 21033, 22778, 24524, 26269, 28015, 29760,-31326,-29581, },
 };

 // Magnetic inclination data in radians * 10^-4
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.074,
+// Date: 2023.2246,
 static constexpr const int16_t inclination_table[19][37] {
 	//    LONGITUDE:   -180,  -170,  -160,  -150,  -140,  -130,  -120,  -110,  -100,   -90,   -80,   -70,   -60,   -50,   -40,   -30,   -20,   -10,     0,    10,    20,    30,    40,    50,    60,    70,    80,    90,   100,   110,   120,   130,   140,   150,   160,   170,   180,
-	/* LAT: -90 */ { -12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568,-12568, },
-	/* LAT: -80 */ { -13652,-13518,-13358,-13177,-12983,-12782,-12578,-12378,-12187,-12011,-11855,-11721,-11610,-11524,-11461,-11419,-11399,-11402,-11429,-11483,-11567,-11681,-11826,-12000,-12198,-12415,-12645,-12878,-13106,-13318,-13504,-13654,-13758,-13809,-13805,-13751,-13652, },
-	/* LAT: -70 */ { -14100,-13781,-13462,-13139,-12807,-12464,-12109,-11752,-11409,-11101,-10849,-10666,-10554,-10502,-10488,-10491,-10497,-10505,-10528,-10583,-10692,-10867,-11114,-11429,-11803,-12221,-12669,-13133,-13599,-14052,-14469,-14813,-15000,-14945,-14714,-14416,-14100, },
-	/* LAT: -60 */ { -13515,-13161,-12823,-12490,-12147,-11774,-11360,-10905,-10438,-10009, -9680, -9506, -9504, -9641, -9845,-10036,-10159,-10199,-10185,-10169,-10216,-10376,-10668,-11078,-11578,-12135,-12722,-13318,-13906,-14468,-14965,-15254,-15075,-14689,-14282,-13889,-13515, },
-	/* LAT: -50 */ { -12494,-12152,-11820,-11498,-11175,-10828,-10429, -9958, -9429, -8909, -8521, -8399, -8603, -9065, -9628,-10137,-10492,-10650,-10616,-10461,-10319,-10327,-10551,-10971,-11515,-12108,-12695,-13233,-13678,-13975,-14083,-14010,-13806,-13522,-13193,-12845,-12494, },
-	/* LAT: -40 */ { -11239,-10890,-10542,-10196, -9857, -9519, -9159, -8734, -8214, -7650, -7227, -7191, -7656, -8482, -9403,-10229,-10884,-11313,-11446,-11272,-10927,-10657,-10655,-10942,-11406,-11912,-12356,-12676,-12833,-12838,-12748,-12612,-12433,-12202,-11915,-11587,-11239, },
-	/* LAT: -30 */ {  -9602, -9222, -8841, -8450, -8058, -7683, -7328, -6938, -6426, -5815, -5368, -5459, -6238, -7445, -8695, -9786,-10696,-11400,-11791,-11772,-11394,-10886,-10557,-10563,-10818,-11139,-11394,-11504,-11440,-11264,-11091,-10959,-10815,-10609,-10328, -9981, -9602, },
-	/* LAT: -20 */ {  -7372, -6929, -6509, -6080, -5636, -5206, -4817, -4407, -3842, -3158, -2716, -2989, -4113, -5732, -7358, -8721, -9779,-10547,-10977,-11002,-10632,-10014, -9458, -9219, -9271, -9434, -9573, -9588, -9414, -9143, -8952, -8869, -8769, -8563, -8247, -7833, -7372, },
-	/* LAT: -10 */ {  -4417, -3876, -3419, -2981, -2523, -2073, -1661, -1211,  -588,   113,   467,    25, -1319, -3245, -5217, -6815, -7899, -8523, -8780, -8710, -8293, -7603, -6940, -6599, -6563, -6656, -6771, -6793, -6604, -6309, -6158, -6175, -6141, -5928, -5544, -5014, -4417, },
-	/* LAT:   0 */ {   -909,  -280,   189,   593,  1012,  1428,  1816,  2255,  2834,  3407,  3602,  3092,  1782,  -137, -2178, -3815, -4810, -5227, -5279, -5102, -4655, -3933, -3228, -2861, -2801, -2870, -2993, -3065, -2935, -2701, -2652, -2804, -2876, -2693, -2269, -1636,  -909, },
-	/* LAT:  10 */ {   2559,  3190,  3628,  3970,  4326,  4692,  5042,  5424,  5871,  6236,  6266,  5767,  4674,  3101,  1413,    48,  -744,  -981,  -883,  -643,  -226,   418,  1051,  1385,  1448,  1406,  1307,  1216,  1258,  1360,  1279,  1003,   803,   871,  1224,  1829,  2559, },
-	/* LAT:  20 */ {   5415,  5946,  6328,  6624,  6938,  7281,  7622,  7966,  8295,  8491,  8399,  7930,  7088,  5988,  4865,  3963,  3443,  3332,  3487,  3734,  4068,  4536,  4997,  5251,  5309,  5294,  5247,  5189,  5176,  5156,  4978,  4639,  4335,  4243,  4415,  4840,  5415, },
-	/* LAT:  30 */ {   7568,  7943,  8261,  8544,  8853,  9200,  9556,  9895, 10169, 10279, 10131,  9706,  9074,  8362,  7704,  7196,  6910,  6873,  7019,  7233,  7482,  7783,  8072,  8245,  8303,  8318,  8322,  8312,  8288,  8204,  7979,  7623,  7264,  7043,  7027,  7222,  7568, },
-	/* LAT:  40 */ {   9266,  9487,  9743, 10028, 10355, 10715, 11082, 11419, 11669, 11749, 11597, 11236, 10762, 10290,  9896,  9615,  9469,  9468,  9581,  9744,  9921, 10106, 10278, 10403, 10482, 10544, 10600, 10633, 10615, 10503, 10259,  9907,  9538,  9253,  9110,  9123,  9266, },
-	/* LAT:  50 */ {  10802, 10923, 11124, 11393, 11716, 12069, 12422, 12737, 12958, 13016, 12876, 12578, 12214, 11870, 11596, 11411, 11320, 11318, 11386, 11491, 11606, 11722, 11839, 11955, 12074, 12197, 12311, 12384, 12373, 12247, 11999, 11670, 11330, 11045, 10856, 10776, 10802, },
-	/* LAT:  60 */ {  12319, 12391, 12541, 12758, 13027, 13327, 13628, 13894, 14069, 14096, 13961, 13713, 13425, 13155, 12935, 12779, 12690, 12660, 12676, 12725, 12794, 12879, 12986, 13119, 13278, 13455, 13619, 13728, 13732, 13611, 13385, 13106, 12828, 12591, 12421, 12329, 12319, },
-	/* LAT:  70 */ {  13758, 13799, 13893, 14034, 14212, 14415, 14622, 14803, 14908, 14890, 14755, 14554, 14338, 14134, 13961, 13827, 13733, 13679, 13660, 13674, 13716, 13789, 13893, 14030, 14197, 14384, 14568, 14708, 14753, 14680, 14518, 14320, 14125, 13961, 13840, 13771, 13758, },
-	/* LAT:  80 */ {  14995, 15007, 15043, 15102, 15177, 15261, 15339, 15385, 15369, 15293, 15183, 15060, 14938, 14824, 14725, 14643, 14581, 14541, 14522, 14526, 14553, 14603, 14674, 14767, 14879, 15006, 15142, 15274, 15381, 15425, 15385, 15297, 15201, 15116, 15050, 15010, 14995, },
-	/* LAT:  90 */ {  15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, },
+	/* LAT: -90 */ { -12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567, },
+	/* LAT: -80 */ { -13651,-13517,-13356,-13176,-12982,-12781,-12577,-12377,-12186,-12010,-11854,-11720,-11610,-11523,-11460,-11418,-11398,-11401,-11428,-11482,-11566,-11680,-11825,-11999,-12198,-12415,-12644,-12877,-13105,-13317,-13504,-13653,-13757,-13808,-13804,-13749,-13651, },
+	/* LAT: -70 */ { -14099,-13780,-13461,-13138,-12806,-12462,-12108,-11751,-11408,-11101,-10849,-10666,-10554,-10502,-10488,-10490,-10496,-10504,-10527,-10582,-10691,-10866,-11113,-11429,-11802,-12221,-12669,-13134,-13600,-14052,-14469,-14813,-14999,-14944,-14713,-14414,-14099, },
+	/* LAT: -60 */ { -13514,-13160,-12822,-12489,-12146,-11773,-11359,-10904,-10437,-10008, -9680, -9506, -9505, -9643, -9846,-10037,-10159,-10199,-10183,-10167,-10215,-10375,-10667,-11078,-11578,-12136,-12722,-13319,-13907,-14469,-14966,-15255,-15075,-14689,-14282,-13888,-13514, },
+	/* LAT: -50 */ { -12494,-12151,-11820,-11497,-11174,-10827,-10429, -9958, -9429, -8909, -8522, -8400, -8605, -9068, -9631,-10139,-10493,-10650,-10614,-10458,-10316,-10325,-10551,-10971,-11516,-12110,-12696,-13234,-13679,-13976,-14083,-14010,-13806,-13522,-13193,-12845,-12494, },
+	/* LAT: -40 */ { -11239,-10890,-10541,-10196, -9856, -9518, -9158, -8734, -8214, -7650, -7228, -7194, -7661, -8488, -9408,-10234,-10887,-11315,-11445,-11269,-10923,-10655,-10655,-10942,-11408,-11913,-12357,-12676,-12833,-12838,-12748,-12612,-12434,-12202,-11915,-11587,-11239, },
+	/* LAT: -30 */ {  -9602, -9221, -8840, -8449, -8057, -7682, -7327, -6938, -6426, -5816, -5369, -5463, -6245, -7453, -8702, -9793,-10701,-11404,-11792,-11771,-11391,-10883,-10556,-10563,-10819,-11140,-11394,-11503,-11439,-11263,-11091,-10959,-10815,-10610,-10328, -9982, -9602, },
+	/* LAT: -20 */ {  -7372, -6928, -6507, -6078, -5634, -5205, -4816, -4406, -3842, -3158, -2718, -2995, -4122, -5743, -7368, -8729, -9786,-10552,-10979,-11002,-10629,-10011, -9456, -9218, -9271, -9434, -9573, -9587, -9413, -9141, -8951, -8869, -8770, -8565, -8248, -7834, -7372, },
+	/* LAT: -10 */ {  -4417, -3875, -3417, -2978, -2521, -2071, -1660, -1210,  -588,   112,   464,    18, -1330, -3258, -5229, -6824, -7906, -8527, -8782, -8710, -8291, -7600, -6937, -6597, -6561, -6654, -6770, -6791, -6601, -6306, -6157, -6175, -6143, -5930, -5547, -5016, -4417, },
+	/* LAT:   0 */ {   -910,  -279,   192,   596,  1015,  1431,  1819,  2256,  2834,  3406,  3598,  3086,  1772,  -150, -2190, -3825, -4815, -5230, -5280, -5102, -4653, -3929, -3224, -2858, -2799, -2867, -2990, -3062, -2932, -2698, -2650, -2804, -2878, -2696, -2272, -1638,  -910, },
+	/* LAT:  10 */ {   2558,  3191,  3630,  3973,  4328,  4695,  5044,  5425,  5870,  6235,  6262,  5761,  4666,  3091,  1402,    40,  -749,  -983,  -883,  -642,  -224,   421,  1054,  1388,  1451,  1410,  1311,  1220,  1262,  1363,  1281,  1003,   801,   868,  1221,  1827,  2558, },
+	/* LAT:  20 */ {   5414,  5947,  6329,  6626,  6940,  7283,  7623,  7966,  8294,  8489,  8396,  7925,  7081,  5981,  4858,  3957,  3439,  3331,  3487,  3734,  4069,  4539,  4999,  5253,  5311,  5297,  5251,  5193,  5179,  5158,  4979,  4639,  4334,  4241,  4413,  4839,  5414, },
+	/* LAT:  30 */ {   7568,  7943,  8262,  8545,  8854,  9201,  9557,  9895, 10168, 10277, 10128,  9702,  9070,  8358,  7699,  7193,  6908,  6872,  7019,  7233,  7483,  7784,  8073,  8247,  8305,  8321,  8325,  8315,  8291,  8206,  7980,  7623,  7263,  7042,  7026,  7222,  7568, },
+	/* LAT:  40 */ {   9266,  9487,  9743, 10029, 10355, 10715, 11081, 11418, 11668, 11747, 11594, 11233, 10759, 10287,  9893,  9613,  9467,  9467,  9581,  9744,  9921, 10107, 10280, 10405, 10483, 10546, 10602, 10635, 10617, 10504, 10260,  9907,  9538,  9253,  9110,  9122,  9266, },
+	/* LAT:  50 */ {  10802, 10923, 11124, 11393, 11715, 12068, 12421, 12736, 12956, 13014, 12873, 12576, 12212, 11868, 11594, 11410, 11319, 11318, 11387, 11491, 11606, 11723, 11840, 11956, 12075, 12199, 12313, 12385, 12375, 12248, 12000, 11671, 11330, 11045, 10856, 10776, 10802, },
+	/* LAT:  60 */ {  12319, 12391, 12540, 12757, 13026, 13326, 13627, 13892, 14067, 14095, 13960, 13711, 13424, 13153, 12934, 12779, 12689, 12660, 12677, 12725, 12794, 12880, 12987, 13120, 13280, 13456, 13621, 13729, 13733, 13611, 13385, 13106, 12828, 12591, 12421, 12329, 12319, },
+	/* LAT:  70 */ {  13758, 13799, 13892, 14033, 14211, 14413, 14621, 14801, 14906, 14888, 14753, 14554, 14337, 14134, 13961, 13827, 13733, 13679, 13661, 13674, 13717, 13790, 13894, 14031, 14198, 14386, 14570, 14709, 14754, 14680, 14518, 14320, 14126, 13961, 13840, 13771, 13758, },
+	/* LAT:  80 */ {  14994, 15006, 15042, 15100, 15176, 15259, 15337, 15383, 15368, 15292, 15182, 15060, 14938, 14824, 14725, 14643, 14582, 14541, 14523, 14527, 14554, 14603, 14675, 14768, 14880, 15007, 15143, 15275, 15383, 15426, 15385, 15297, 15201, 15116, 15050, 15009, 14994, },
+	/* LAT:  90 */ {  15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15397, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, },
 };

 // Magnetic strength data in milli-Gauss * 10
 // Model: WMM-2020,
 // Version: 0.5.1.11,
-// Date: 2023.074,
+// Date: 2023.2246,
 static constexpr const int16_t strength_table[19][37] {
 	//    LONGITUDE:  -180, -170, -160, -150, -140, -130, -120, -110, -100,  -90,  -80,  -70,  -60,  -50,  -40,  -30,  -20,  -10,    0,   10,   20,   30,   40,   50,   60,   70,   80,   90,  100,  110,  120,  130,  140,  150,  160,  170,  180,
-	/* LAT: -90 */ {  5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, 5448, },
-	/* LAT: -80 */ {  6055, 5991, 5912, 5820, 5716, 5605, 5486, 5365, 5242, 5122, 5008, 4903, 4809, 4730, 4667, 4622, 4598, 4596, 4618, 4664, 4735, 4829, 4944, 5076, 5220, 5370, 5519, 5663, 5794, 5907, 6000, 6069, 6113, 6132, 6127, 6101, 6055, },
-	/* LAT: -70 */ {  6299, 6166, 6015, 5850, 5671, 5478, 5273, 5058, 4839, 4624, 4422, 4240, 4082, 3951, 3847, 3771, 3724, 3712, 3741, 3817, 3946, 4126, 4355, 4624, 4919, 5226, 5530, 5814, 6063, 6267, 6418, 6513, 6554, 6547, 6496, 6412, 6299, },
-	/* LAT: -60 */ {  6185, 5992, 5790, 5581, 5361, 5127, 4870, 4592, 4299, 4010, 3743, 3516, 3337, 3203, 3104, 3031, 2980, 2960, 2987, 3078, 3247, 3499, 3827, 4214, 4636, 5071, 5492, 5876, 6200, 6449, 6612, 6690, 6691, 6627, 6513, 6362, 6185, },
-	/* LAT: -50 */ {  5842, 5612, 5379, 5148, 4915, 4670, 4399, 4096, 3767, 3436, 3134, 2895, 2734, 2643, 2595, 2560, 2526, 2499, 2505, 2578, 2752, 3044, 3441, 3914, 4421, 4927, 5402, 5820, 6159, 6400, 6537, 6578, 6535, 6424, 6261, 6063, 5842, },
-	/* LAT: -40 */ {  5392, 5146, 4900, 4660, 4426, 4188, 3933, 3649, 3335, 3009, 2711, 2489, 2373, 2347, 2366, 2387, 2391, 2378, 2365, 2394, 2526, 2804, 3224, 3739, 4286, 4810, 5276, 5664, 5955, 6143, 6230, 6233, 6163, 6031, 5850, 5631, 5392, },
-	/* LAT: -30 */ {  4878, 4636, 4397, 4162, 3936, 3717, 3498, 3266, 3010, 2735, 2477, 2296, 2227, 2252, 2319, 2390, 2454, 2504, 2524, 2536, 2605, 2807, 3171, 3661, 4193, 4690, 5107, 5422, 5625, 5724, 5750, 5723, 5644, 5511, 5331, 5114, 4878, },
-	/* LAT: -20 */ {  4321, 4107, 3898, 3693, 3497, 3314, 3146, 2982, 2804, 2606, 2415, 2282, 2242, 2286, 2376, 2486, 2614, 2741, 2828, 2863, 2889, 2987, 3229, 3615, 4066, 4493, 4840, 5073, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4321, },
-	/* LAT: -10 */ {  3790, 3629, 3475, 3329, 3193, 3073, 2969, 2877, 2779, 2664, 2542, 2443, 2399, 2424, 2512, 2641, 2796, 2954, 3077, 3138, 3152, 3180, 3305, 3559, 3886, 4208, 4472, 4633, 4668, 4615, 4547, 4484, 4396, 4271, 4122, 3958, 3790, },
-	/* LAT:   0 */ {  3412, 3319, 3234, 3161, 3106, 3068, 3041, 3022, 2997, 2948, 2870, 2775, 2696, 2666, 2709, 2812, 2945, 3080, 3194, 3269, 3299, 3322, 3398, 3556, 3766, 3980, 4159, 4264, 4270, 4202, 4114, 4021, 3910, 3779, 3644, 3520, 3412, },
-	/* LAT:  10 */ {  3283, 3251, 3230, 3227, 3251, 3298, 3353, 3406, 3440, 3430, 3361, 3247, 3120, 3027, 3002, 3044, 3125, 3223, 3323, 3408, 3472, 3534, 3623, 3742, 3878, 4017, 4136, 4206, 4208, 4146, 4036, 3892, 3731, 3571, 3436, 3339, 3283, },
-	/* LAT:  20 */ {  3399, 3402, 3427, 3481, 3572, 3693, 3821, 3938, 4018, 4030, 3957, 3815, 3649, 3510, 3435, 3424, 3460, 3533, 3630, 3727, 3818, 3917, 4028, 4140, 4249, 4361, 4464, 4529, 4539, 4480, 4343, 4140, 3913, 3703, 3540, 3439, 3399, },
-	/* LAT:  30 */ {  3722, 3728, 3782, 3881, 4023, 4194, 4369, 4525, 4633, 4659, 4586, 4431, 4243, 4080, 3975, 3930, 3934, 3986, 4073, 4172, 4270, 4376, 4492, 4610, 4730, 4857, 4978, 5065, 5090, 5032, 4875, 4635, 4361, 4106, 3907, 3778, 3722, },
-	/* LAT:  40 */ {  4222, 4219, 4283, 4406, 4572, 4759, 4942, 5098, 5203, 5229, 5161, 5013, 4827, 4654, 4526, 4452, 4426, 4449, 4510, 4590, 4677, 4774, 4889, 5023, 5174, 5336, 5488, 5598, 5636, 5581, 5425, 5187, 4913, 4653, 4443, 4297, 4222, },
-	/* LAT:  50 */ {  4832, 4823, 4878, 4987, 5132, 5291, 5441, 5562, 5636, 5646, 5583, 5456, 5294, 5131, 4995, 4898, 4844, 4833, 4858, 4908, 4978, 5069, 5188, 5339, 5514, 5699, 5865, 5982, 6025, 5980, 5849, 5653, 5429, 5214, 5034, 4904, 4832, },
-	/* LAT:  60 */ {  5392, 5378, 5405, 5467, 5552, 5646, 5733, 5800, 5834, 5826, 5772, 5679, 5559, 5431, 5312, 5216, 5150, 5117, 5115, 5143, 5200, 5287, 5405, 5552, 5719, 5887, 6034, 6137, 6180, 6158, 6075, 5949, 5804, 5661, 5539, 5447, 5392, },
-	/* LAT:  70 */ {  5726, 5705, 5701, 5712, 5734, 5760, 5784, 5799, 5799, 5781, 5742, 5685, 5616, 5540, 5466, 5403, 5355, 5327, 5323, 5343, 5388, 5458, 5549, 5658, 5775, 5889, 5988, 6060, 6099, 6103, 6074, 6021, 5954, 5883, 5818, 5764, 5726, },
-	/* LAT:  80 */ {  5790, 5772, 5757, 5745, 5734, 5725, 5715, 5703, 5689, 5671, 5649, 5624, 5597, 5571, 5546, 5526, 5513, 5508, 5513, 5528, 5554, 5589, 5632, 5680, 5729, 5777, 5820, 5854, 5878, 5891, 5893, 5886, 5872, 5853, 5831, 5810, 5790, },
+	/* LAT: -90 */ {  5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, },
+	/* LAT: -80 */ {  6054, 5990, 5911, 5818, 5715, 5603, 5485, 5363, 5241, 5121, 5007, 4902, 4808, 4729, 4666, 4621, 4597, 4595, 4617, 4663, 4734, 4828, 4943, 5075, 5219, 5369, 5519, 5662, 5793, 5907, 5999, 6068, 6112, 6131, 6127, 6100, 6054, },
+	/* LAT: -70 */ {  6298, 6165, 6014, 5848, 5669, 5477, 5272, 5057, 4838, 4623, 4420, 4238, 4081, 3950, 3846, 3770, 3723, 3711, 3740, 3817, 3945, 4126, 4355, 4623, 4919, 5226, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6546, 6496, 6411, 6298, },
+	/* LAT: -60 */ {  6184, 5991, 5789, 5579, 5360, 5125, 4868, 4590, 4298, 4008, 3742, 3515, 3336, 3202, 3103, 3030, 2979, 2959, 2986, 3077, 3247, 3499, 3827, 4214, 4637, 5071, 5493, 5876, 6201, 6449, 6612, 6690, 6691, 6627, 6513, 6361, 6184, },
+	/* LAT: -50 */ {  5842, 5611, 5378, 5147, 4914, 4669, 4398, 4095, 3766, 3434, 3133, 2894, 2733, 2643, 2594, 2559, 2525, 2498, 2504, 2577, 2752, 3044, 3442, 3915, 4422, 4928, 5403, 5821, 6159, 6400, 6538, 6578, 6535, 6424, 6261, 6062, 5842, },
+	/* LAT: -40 */ {  5392, 5145, 4900, 4659, 4424, 4187, 3932, 3648, 3334, 3008, 2710, 2488, 2372, 2347, 2366, 2386, 2390, 2377, 2364, 2393, 2526, 2804, 3225, 3741, 4288, 4811, 5277, 5664, 5956, 6143, 6231, 6233, 6163, 6031, 5850, 5631, 5392, },
+	/* LAT: -30 */ {  4877, 4636, 4396, 4161, 3935, 3716, 3497, 3265, 3009, 2734, 2476, 2295, 2226, 2252, 2318, 2389, 2454, 2503, 2523, 2535, 2604, 2807, 3172, 3663, 4195, 4691, 5108, 5423, 5625, 5724, 5751, 5723, 5644, 5511, 5331, 5114, 4877, },
+	/* LAT: -20 */ {  4320, 4107, 3898, 3693, 3496, 3314, 3145, 2981, 2803, 2605, 2414, 2281, 2241, 2286, 2376, 2486, 2614, 2741, 2827, 2862, 2888, 2987, 3230, 3617, 4068, 4494, 4841, 5074, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4320, },
+	/* LAT: -10 */ {  3790, 3628, 3475, 3328, 3192, 3072, 2969, 2876, 2778, 2663, 2541, 2442, 2398, 2424, 2512, 2641, 2797, 2954, 3076, 3137, 3151, 3180, 3306, 3560, 3887, 4209, 4473, 4634, 4668, 4615, 4548, 4484, 4396, 4271, 4122, 3958, 3790, },
+	/* LAT:   0 */ {  3412, 3318, 3234, 3161, 3105, 3067, 3041, 3021, 2996, 2947, 2868, 2774, 2695, 2666, 2709, 2813, 2945, 3080, 3194, 3268, 3299, 3321, 3398, 3557, 3767, 3981, 4160, 4265, 4270, 4202, 4114, 4022, 3910, 3779, 3645, 3520, 3412, },
+	/* LAT:  10 */ {  3282, 3251, 3230, 3226, 3250, 3297, 3352, 3405, 3439, 3429, 3359, 3245, 3119, 3026, 3001, 3044, 3125, 3224, 3324, 3408, 3472, 3535, 3624, 3743, 3879, 4018, 4137, 4207, 4209, 4146, 4036, 3893, 3731, 3572, 3436, 3339, 3282, },
+	/* LAT:  20 */ {  3399, 3401, 3427, 3480, 3572, 3692, 3820, 3937, 4017, 4029, 3955, 3814, 3648, 3509, 3435, 3424, 3460, 3533, 3630, 3728, 3818, 3917, 4029, 4141, 4250, 4362, 4465, 4531, 4540, 4481, 4343, 4141, 3913, 3703, 3540, 3439, 3399, },
+	/* LAT:  30 */ {  3722, 3728, 3782, 3880, 4022, 4193, 4368, 4524, 4631, 4658, 4585, 4430, 4242, 4079, 3975, 3930, 3934, 3986, 4074, 4172, 4270, 4377, 4493, 4611, 4731, 4859, 4980, 5066, 5091, 5033, 4876, 4636, 4361, 4107, 3907, 3778, 3722, },
+	/* LAT:  40 */ {  4222, 4219, 4282, 4405, 4571, 4758, 4941, 5097, 5201, 5227, 5159, 5012, 4826, 4653, 4526, 4452, 4427, 4450, 4511, 4591, 4678, 4775, 4891, 5024, 5175, 5338, 5489, 5599, 5637, 5582, 5425, 5187, 4913, 4654, 4443, 4297, 4222, },
+	/* LAT:  50 */ {  4832, 4823, 4877, 4985, 5131, 5290, 5439, 5561, 5635, 5644, 5582, 5456, 5294, 5131, 4995, 4898, 4844, 4833, 4858, 4909, 4979, 5070, 5189, 5340, 5515, 5700, 5866, 5983, 6026, 5981, 5850, 5654, 5430, 5214, 5035, 4904, 4832, },
+	/* LAT:  60 */ {  5392, 5378, 5404, 5466, 5551, 5645, 5732, 5799, 5833, 5825, 5771, 5678, 5558, 5430, 5312, 5217, 5151, 5117, 5116, 5144, 5201, 5287, 5406, 5553, 5720, 5888, 6035, 6138, 6181, 6158, 6075, 5949, 5804, 5662, 5539, 5447, 5392, },
+	/* LAT:  70 */ {  5726, 5705, 5701, 5712, 5733, 5759, 5783, 5798, 5798, 5780, 5742, 5685, 5616, 5540, 5467, 5403, 5355, 5328, 5324, 5344, 5389, 5458, 5550, 5659, 5776, 5890, 5989, 6061, 6100, 6103, 6074, 6021, 5954, 5884, 5818, 5764, 5726, },
+	/* LAT:  80 */ {  5790, 5772, 5757, 5744, 5734, 5725, 5715, 5703, 5689, 5671, 5649, 5624, 5598, 5571, 5546, 5526, 5513, 5508, 5513, 5529, 5554, 5590, 5632, 5680, 5730, 5778, 5820, 5855, 5879, 5892, 5894, 5887, 5872, 5853, 5832, 5810, 5790, },
 	/* LAT:  90 */ {  5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, 5683, },
 };
@@ -54,7 +54,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	update_CAS_scale_applied();
 	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
 	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
-			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
+			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
 	check_airspeed_data_stuck(input_data.timestamp);
 	check_load_factor(input_data.accel_z);
@@ -72,20 +72,18 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)

 void
 AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
-		const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const float att_q[4])
+		const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att)
 {
 	_wind_estimator.update(time_now_usec);

 	if (lpos_valid && _in_fixed_wing_flight) {

-		Quatf q(att_q);
-
 		// airspeed fusion (with raw TAS)
 		const float hor_vel_variance =  lpos_evh * lpos_evh;
-		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
+		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q_att);

 		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
+		_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q_att);
 	}
 }

@@ -59,7 +59,7 @@ struct airspeed_validator_update_data {
 	bool lpos_valid;
 	float lpos_evh;
 	float lpos_evv;
-	float att_q[4];
+	matrix::Quatf q_att;
 	float air_pressure_pa;
 	float air_temperature_celsius;
 	float accel_z;
@@ -179,7 +179,7 @@ private:

 	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid,
 				   const matrix::Vector3f &vI,
-				   float lpos_evh, float lpos_evv, const float att_q[4]);
+				   float lpos_evh, float lpos_evv, const Quatf &q_att);
 	void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw);
 	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
@@ -128,7 +128,6 @@ private:
 	estimator_status_s _estimator_status {};
 	vehicle_acceleration_s _accel {};
 	vehicle_air_data_s _vehicle_air_data {};
-	vehicle_attitude_s _vehicle_attitude {};
 	vehicle_land_detected_s _vehicle_land_detected {};
 	vehicle_local_position_s _vehicle_local_position {};
 	vehicle_status_s _vehicle_status {};
@@ -142,6 +141,7 @@ private:
 	int32_t _prev_number_of_airspeed_sensors{0}; /**<  number of airspeed sensors in previous loop (to detect a new added sensor)*/
 	AirspeedValidator _airspeed_validator[MAX_NUM_AIRSPEED_SENSORS] {}; /**< airspeedValidator instances (one for each sensor) */

+	matrix::Quatf _q_att;
 	hrt_abstime _time_now_usec{0};
 	int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */
 	int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */
@@ -350,10 +350,7 @@ AirspeedModule::Run()
 		input_data.lpos_valid = _vehicle_local_position_valid;
 		input_data.lpos_evh = _vehicle_local_position.evh;
 		input_data.lpos_evv = _vehicle_local_position.evv;
-		input_data.att_q[0] = _vehicle_attitude.q[0];
-		input_data.att_q[1] = _vehicle_attitude.q[1];
-		input_data.att_q[2] = _vehicle_attitude.q[2];
-		input_data.att_q[3] = _vehicle_attitude.q[3];
+		input_data.q_att = _q_att;
 		input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa;
 		input_data.accel_z = _accel.xyz[2];
 		input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
@@ -498,13 +495,25 @@ void AirspeedModule::poll_topics()
 	_estimator_status_sub.update(&_estimator_status);
 	_vehicle_acceleration_sub.update(&_accel);
 	_vehicle_air_data_sub.update(&_vehicle_air_data);
-	_vehicle_attitude_sub.update(&_vehicle_attitude);
 	_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 	_vehicle_status_sub.update(&_vehicle_status);
 	_vtol_vehicle_status_sub.update(&_vtol_vehicle_status);
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 	_position_setpoint_sub.update(&_position_setpoint);

+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude;
+		_vehicle_attitude_sub.update(&vehicle_attitude);
+
+		if (_vehicle_status.is_vtol_tailsitter && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+
+			// if the vehicle is a tailsitter we have to rotate the attitude by 90° to get to the airspeed frame
+			_q_att = Quatf(vehicle_attitude.q) * Quatf(matrix::Eulerf(0.f, M_PI_2_F, 0.f));
+
+		} else {
+			_q_att = Quatf(vehicle_attitude.q);
+		}
+	}

 	_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
 					&& (_vehicle_local_position.timestamp > 0)
@@ -521,11 +530,9 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	    && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
 	    && !_vehicle_land_detected.landed) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
-		Quatf q(_vehicle_attitude.q);

 		const float hor_vel_variance =  _vehicle_local_position.evh * _vehicle_local_position.evh;
-
-		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q);
+		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, _q_att);
 	}

 	_wind_estimate_sideslip.timestamp = _time_now_usec;
@@ -171,7 +171,7 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 	failure_detector_status_u status_prev = _status;

 	if (vehicle_control_mode.flag_control_attitude_enabled) {
-		updateAttitudeStatus();
+		updateAttitudeStatus(vehicle_status);

 		if (_param_fd_ext_ats_en.get()) {
 			updateExternalAtsStatus();
@@ -206,15 +206,31 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 	return _status.value != status_prev.value;
 }

-void FailureDetector::updateAttitudeStatus()
+void FailureDetector::updateAttitudeStatus(const vehicle_status_s &vehicle_status)
 {
 	vehicle_attitude_s attitude;

 	if (_vehicle_attitude_sub.update(&attitude)) {

 		const matrix::Eulerf euler(matrix::Quatf(attitude.q));
-		const float roll(euler.phi());
-		const float pitch(euler.theta());
+		float roll(euler.phi());
+		float pitch(euler.theta());
+
+		// special handling for tailsitter
+		if (vehicle_status.is_vtol_tailsitter) {
+			if (vehicle_status.in_transition_mode) {
+				// disable attitude check during tailsitter transition
+				roll = 0.f;
+				pitch = 0.f;
+
+			} else if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				// in FW flight rotate the attitude by 90° around pitch (level FW flight = 0° pitch)
+				const matrix::Eulerf euler_rotated = matrix::Eulerf(matrix::Quatf(attitude.q) * matrix::Quatf(matrix::Eulerf(0.f,
+								     M_PI_2_F, 0.f)));
+				roll = euler_rotated.phi();
+				pitch = euler_rotated.theta();
+			}
+		}

 		const float max_roll_deg = _param_fd_fail_r.get();
 		const float max_pitch_deg = _param_fd_fail_p.get();
@@ -107,7 +107,7 @@ public:
 	uint16_t getMotorFailures() const { return _motor_failure_esc_timed_out_mask | _motor_failure_esc_under_current_mask; }

 private:
-	void updateAttitudeStatus();
+	void updateAttitudeStatus(const vehicle_status_s &vehicle_status);
 	void updateExternalAtsStatus();
 	void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
 	void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
@@ -61,6 +61,7 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 	}

 	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
+	_param_handles.yaw_collective_pitch_offset = param_find("CA_HELI_YAW_CP_O");
 	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
 	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
 	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
@@ -95,6 +96,7 @@ void ActuatorEffectivenessHelicopter::updateParams()
 	}

 	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
+	param_get(_param_handles.yaw_collective_pitch_offset, &_geometry.yaw_collective_pitch_offset);
 	param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
 	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
 	int32_t yaw_ccw = 0;
@@ -142,7 +144,7 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 	actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;

 	actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
-			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+			 + fabsf(collective_pitch - _geometry.yaw_collective_pitch_offset) * _geometry.yaw_collective_pitch_scale
 			 + throttle * _geometry.yaw_throttle_scale;

 	// Saturation check for yaw
@@ -60,6 +60,7 @@ public:
 		float throttle_curve[NUM_CURVE_POINTS];
 		float pitch_curve[NUM_CURVE_POINTS];
 		float yaw_collective_pitch_scale;
+		float yaw_collective_pitch_offset;
 		float yaw_throttle_scale;
 		float yaw_sign;
 		float spoolup_time;
@@ -109,6 +110,7 @@ private:
 		param_t throttle_curve[NUM_CURVE_POINTS];
 		param_t pitch_curve[NUM_CURVE_POINTS];
 		param_t yaw_collective_pitch_scale;
+		param_t yaw_collective_pitch_offset;
 		param_t yaw_throttle_scale;
 		param_t yaw_ccw;
 		param_t spoolup_time;
@@ -476,7 +476,24 @@ parameters:
              This allows to add a proportional factor of the collective pitch command to the yaw command.
              A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.

-              tail_output += CA_HELI_YAW_CP_S * collective_pitch
+              tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
+        CA_HELI_YAW_CP_O:
+          description:
+            short: Offset for yaw compensation based on collective pitch
+            long: |
+              This allows to specify which collective pitch command results in the least amount of rotor drag.
+              This is used to increase the accuracy of the yaw drag torque compensation based on collective pitch
+              by aligning the lowest rotor drag with zero compensation.
+              For symmetric profile blades this is the command that results in exactly 0° collective blade angle.
+              For lift profile blades this is typically a command resulting in slightly negative collective blade angle.
+
+              tail_output += CA_HELI_YAW_CP_S * abs(collective_pitch - CA_HELI_YAW_CP_O)
          type: float
          decimal: 3
          increment: 0.1
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -66,6 +66,78 @@ add_subdirectory(Utility)
 # 	add_custom_target(ekf2_symforce_generate DEPENDS ${EKF_GENERATED_SRC_FILES})
 # endif()

+set(EKF_SRCS)
+list(APPEND EKF_SRCS
+	EKF/baro_height_control.cpp
+	EKF/bias_estimator.cpp
+	EKF/control.cpp
+	EKF/covariance.cpp
+	EKF/ekf.cpp
+	EKF/ekf_helper.cpp
+	EKF/EKFGSF_yaw.cpp
+	EKF/estimator_interface.cpp
+	EKF/fake_height_control.cpp
+	EKF/fake_pos_control.cpp
+	EKF/gnss_height_control.cpp
+	EKF/gps_checks.cpp
+	EKF/gps_control.cpp
+	EKF/gravity_fusion.cpp
+	EKF/height_control.cpp
+	EKF/imu_down_sampler.cpp
+	EKF/mag_control.cpp
+	EKF/mag_fusion.cpp
+	EKF/output_predictor.cpp
+	EKF/vel_pos_fusion.cpp
+	EKF/zero_innovation_heading_update.cpp
+	EKF/zero_velocity_update.cpp
+)
+
+if(CONFIG_EKF2_AIRSPEED)
+	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_AUXVEL)
+	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_DRAG_FUSION)
+	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_EXTERNAL_VISION)
+	list(APPEND EKF_SRCS
+		EKF/ev_control.cpp
+		EKF/ev_height_control.cpp
+		EKF/ev_pos_control.cpp
+		EKF/ev_vel_control.cpp
+		EKF/ev_yaw_control.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_OPTICAL_FLOW)
+	list(APPEND EKF_SRCS
+		EKF/optical_flow_control.cpp
+		EKF/optflow_fusion.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_RANGE_FINDER)
+	list(APPEND EKF_SRCS
+		EKF/range_finder_consistency_check.cpp
+		EKF/range_height_control.cpp
+		EKF/sensor_range_finder.cpp
+		EKF/terrain_estimator.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_SIDESLIP)
+	list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
+endif()
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -81,44 +153,7 @@ px4_add_module(
 	STACK_MAX
 		3600
 	SRCS
-		EKF/airspeed_fusion.cpp
-		EKF/auxvel_fusion.cpp
-		EKF/baro_height_control.cpp
-		EKF/bias_estimator.cpp
-		EKF/control.cpp
-		EKF/covariance.cpp
-		EKF/drag_fusion.cpp
-		EKF/ekf.cpp
-		EKF/ekf_helper.cpp
-		EKF/EKFGSF_yaw.cpp
-		EKF/estimator_interface.cpp
-		EKF/ev_control.cpp
-		EKF/ev_height_control.cpp
-		EKF/ev_pos_control.cpp
-		EKF/ev_vel_control.cpp
-		EKF/ev_yaw_control.cpp
-		EKF/fake_height_control.cpp
-		EKF/fake_pos_control.cpp
-		EKF/gnss_height_control.cpp
-		EKF/gps_checks.cpp
-		EKF/gps_control.cpp
-		EKF/gps_yaw_fusion.cpp
-		EKF/gravity_fusion.cpp
-		EKF/height_control.cpp
-		EKF/imu_down_sampler.cpp
-		EKF/mag_control.cpp
-		EKF/mag_fusion.cpp
-		EKF/optical_flow_control.cpp
-		EKF/optflow_fusion.cpp
-		EKF/output_predictor.cpp
-		EKF/range_finder_consistency_check.cpp
-		EKF/range_height_control.cpp
-		EKF/sensor_range_finder.cpp
-		EKF/sideslip_fusion.cpp
-		EKF/terrain_estimator.cpp
-		EKF/vel_pos_fusion.cpp
-		EKF/zero_innovation_heading_update.cpp
-		EKF/zero_velocity_update.cpp
+		${EKF_SRCS}

 		EKF2.cpp
 		EKF2.hpp
@@ -31,47 +31,82 @@
 #
 ############################################################################

-add_library(ecl_EKF
-	airspeed_fusion.cpp
-	auxvel_fusion.cpp
+set(EKF_SRCS)
+list(APPEND EKF_SRCS
 	baro_height_control.cpp
 	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
-	drag_fusion.cpp
 	ekf.cpp
 	ekf_helper.cpp
 	EKFGSF_yaw.cpp
 	estimator_interface.cpp
-	ev_control.cpp
-	ev_height_control.cpp
-	ev_pos_control.cpp
-	ev_vel_control.cpp
-	ev_yaw_control.cpp
 	fake_height_control.cpp
 	fake_pos_control.cpp
 	gnss_height_control.cpp
 	gps_checks.cpp
 	gps_control.cpp
-	gps_yaw_fusion.cpp
 	gravity_fusion.cpp
 	height_control.cpp
 	imu_down_sampler.cpp
 	mag_control.cpp
 	mag_fusion.cpp
-	optical_flow_control.cpp
-	optflow_fusion.cpp
 	output_predictor.cpp
-	range_finder_consistency_check.cpp
-	range_height_control.cpp
-	sensor_range_finder.cpp
-	sideslip_fusion.cpp
-	terrain_estimator.cpp
 	vel_pos_fusion.cpp
 	zero_innovation_heading_update.cpp
 	zero_velocity_update.cpp
 )

+if(CONFIG_EKF2_AIRSPEED)
+	list(APPEND EKF_SRCS airspeed_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_AUXVEL)
+	list(APPEND EKF_SRCS auxvel_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_DRAG_FUSION)
+	list(APPEND EKF_SRCS drag_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_EXTERNAL_VISION)
+	list(APPEND EKF_SRCS
+		ev_control.cpp
+		ev_height_control.cpp
+		ev_pos_control.cpp
+		ev_vel_control.cpp
+		ev_yaw_control.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
+endif()
+
+if(CONFIG_EKF2_OPTICAL_FLOW)
+	list(APPEND EKF_SRCS
+		optical_flow_control.cpp
+		optflow_fusion.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_RANGE_FINDER)
+	list(APPEND EKF_SRCS
+		range_finder_consistency_check.cpp
+		range_height_control.cpp
+		sensor_range_finder.cpp
+		terrain_estimator.cpp
+	)
+endif()
+
+if(CONFIG_EKF2_SIDESLIP)
+	list(APPEND EKF_SRCS sideslip_fusion.cpp)
+endif()
+
+add_library(ecl_EKF
+	${EKF_SRCS}
+)
+
 add_dependencies(ecl_EKF prebuild_targets)
 target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model)
 target_compile_options(ecl_EKF PRIVATE -fno-associative-math)
@@ -222,21 +222,6 @@ void Ekf::stopAirspeedFusion()
 	}
 }

-float Ekf::getTrueAirspeed() const
-{
-	return (_state.vel - Vector3f(_state.wind_vel(0), _state.wind_vel(1), 0.f)).norm();
-}
-
-void Ekf::resetWind()
-{
-	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
-		resetWindUsingAirspeed(_airspeed_sample_delayed);
-
-	} else {
-		resetWindToZero();
-	}
-}
-
 void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 {
 	const float euler_yaw = getEulerYaw(_R_to_earth);
@@ -282,14 +267,3 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample
 	P(22, 22) += P(4, 4);
 	P(23, 23) += P(5, 5);
 }
-
-void Ekf::resetWindToZero()
-{
-	ECL_INFO("reset wind to zero");
-
-	// If we don't have an airspeed measurement, then assume the wind is zero
-	_state.wind_vel.setZero();
-
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
-}
@@ -108,10 +108,12 @@ enum MagFuseType : uint8_t {
 	NONE    = 5    	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 };

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 enum TerrainFusionMask : uint8_t {
 	TerrainFuseRangeFinder = (1 << 0),
 	TerrainFuseOpticalFlow = (1 << 1)
 };
+#endif // CONFIG_EKF2_RANGE_FINDER

 enum HeightSensor : uint8_t {
 	BARO  = 0,
@@ -238,6 +240,7 @@ struct flowSample {
 	uint8_t     quality{};     ///< quality indicator between 0 and 255
 };

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 struct extVisionSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
 	Vector3f    pos{};         ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
@@ -251,17 +254,22 @@ struct extVisionSample {
 	uint8_t     reset_counter{};
 	int8_t     quality{};     ///< quality indicator between 0 and 100
 };
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 struct dragSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
 	Vector2f    accelXY{};     ///< measured specific force along the X and Y body axes (m/sec**2)
 };
+#endif // CONFIG_EKF2_DRAG_FUSION

+#if defined(CONFIG_EKF2_AUXVEL)
 struct auxVelSample {
 	uint64_t    time_us{};     ///< timestamp of the measurement (uSec)
 	Vector2f    vel{};         ///< measured NE velocity relative to the local origin (m/sec)
 	Vector2f    velVar{};      ///< estimated error variance of the NE velocity (m/sec)**2
 };
+#endif // CONFIG_EKF2_AUXVEL

 struct systemFlagUpdate {
 	uint64_t time_us{};
@@ -294,10 +302,6 @@ struct parameters {
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 	int32_t baro_ctrl{1};
 	int32_t gnss_ctrl{GnssCtrl::HPOS | GnssCtrl::VEL};
-	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
-	int32_t ev_ctrl{0};
-	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
-				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator

 	int32_t sensor_interval_max_ms{10};     ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)

@@ -305,11 +309,6 @@ struct parameters {
 	float mag_delay_ms{0.0f};               ///< magnetometer measurement delay relative to the IMU (mSec)
 	float baro_delay_ms{0.0f};              ///< barometer height measurement delay relative to the IMU (mSec)
 	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
-	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
-	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
-	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
-	float ev_delay_ms{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
-	float auxvel_delay_ms{5.0f};            ///< auxiliary velocity measurement delay relative to the IMU (mSec)

 	// input noise
 	float gyro_noise{1.5e-2f};              ///< IMU angular rate noise used for covariance prediction (rad/sec)
@@ -323,10 +322,6 @@ struct parameters {
 	float wind_vel_nsd{1.0e-2f};        ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
 	const float wind_vel_nsd_scaler{0.5f};      ///< scaling of wind process noise with vertical velocity

-	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
-	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
-	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
-
 	// initialization errors
 	float switch_on_gyro_bias{0.1f};        ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
 	float switch_on_accel_bias{0.2f};       ///< 1-sigma accelerometer bias uncertainty at switch on (m/sec**2)
@@ -357,21 +352,35 @@ struct parameters {
 	float mag_acc_gate{0.5f};               ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2)
 	float mag_yaw_rate_gate{0.20f};         ///< yaw rate threshold used by mode select logic (rad/sec)

+#if defined(CONFIG_EKF2_GNSS_YAW)
 	// GNSS heading fusion
 	float gps_heading_noise{0.1f};          ///< measurement noise standard deviation used for GNSS heading fusion (rad)
+#endif // CONFIG_EKF2_GNSS_YAW

+#if defined(CONFIG_EKF2_AIRSPEED)
 	// airspeed fusion
+	float airspeed_delay_ms{100.0f};        ///< airspeed measurement delay relative to the IMU (mSec)
 	float tas_innov_gate{5.0f};             ///< True Airspeed innovation consistency gate size (STD)
 	float eas_noise{1.4f};                  ///< EAS measurement noise standard deviation used for airspeed fusion (m/s)
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_SIDESLIP)
 	// synthetic sideslip fusion
 	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
+#endif // CONFIG_EKF2_SIDESLIP

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range finder fusion
+	int32_t rng_ctrl{RngCtrl::CONDITIONAL};
+
+	int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder |
+				    TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator
+
+	float range_delay_ms{5.0f};             ///< range finder measurement delay relative to the IMU (mSec)
 	float range_noise{0.1f};                ///< observation noise for range finder measurements (m)
 	float range_innov_gate{5.0f};           ///< range finder fusion innovation consistency gate size (STD)
 	float rng_hgt_bias_nsd{0.13f};          ///< process noise for range height bias estimation (m/s/sqrt(Hz))
@@ -386,7 +395,19 @@ struct parameters {
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check

+	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
+
+	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
+	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// vision position fusion
+	int32_t ev_ctrl{0};
+	float ev_delay_ms{175.0f};              ///< off-board vision measurement delay relative to the IMU (mSec)
+
 	float ev_vel_noise{0.1f};               ///< minimum allowed observation noise for EV velocity fusion (m/sec)
 	float ev_pos_noise{0.1f};               ///< minimum allowed observation noise for EV position fusion (m)
 	float ev_att_noise{0.1f};               ///< minimum allowed observation noise for EV attitude fusion (rad/sec)
@@ -395,15 +416,24 @@ struct parameters {
 	float ev_pos_innov_gate{5.0f};          ///< vision position fusion innovation consistency gate size (STD)
 	float ev_hgt_bias_nsd{0.13f};           ///< process noise for vision height bias estimation (m/s/sqrt(Hz))

+	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
 	// gravity fusion
 	float gravity_noise{1.0f};              ///< accelerometer measurement gaussian noise (m/s**2)

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	float flow_delay_ms{5.0f};              ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval
+
 	// optical flow fusion
 	float flow_noise{0.15f};                ///< observation noise for optical flow LOS rate measurements (rad/sec)
 	float flow_noise_qual_min{0.5f};        ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec)
 	int32_t flow_qual_min{1};               ///< minimum acceptable quality integer from  the flow sensor
 	float flow_innov_gate{3.0f};            ///< optical flow fusion innovation consistency gate size (STD)

+	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	// these parameters control the strictness of GPS quality checks used to determine if the GPS is
 	// good enough to set a local origin and commence aiding
 	int32_t gps_check_mask{21};             ///< bitmask used to control which GPS quality checks are used
@@ -418,9 +448,6 @@ struct parameters {
 	// XYZ offset of sensors in body axes (m)
 	Vector3f imu_pos_body{};                ///< xyz position of IMU in body frame (m)
 	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
-	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
-	Vector3f flow_pos_body{};               ///< xyz position of range sensor focal point in body frame (m)
-	Vector3f ev_pos_body{};                 ///< xyz position of VI-sensor focal point in body frame (m)

 	// accel bias learning control
 	float acc_bias_lim{0.4f};               ///< maximum accel bias magnitude (m/sec**2)
@@ -436,6 +463,7 @@ struct parameters {

 	int32_t valid_timeout_max{5'000'000};     ///< amount of time spent inertial dead reckoning before the estimator reports the state estimates as invalid (uSec)

+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	// static barometer pressure position error coefficient along body axes
 	float static_pressure_coef_xp{0.0f};    // (-)
 	float static_pressure_coef_xn{0.0f};    // (-)
@@ -445,21 +473,27 @@ struct parameters {

 	// upper limit on airspeed used for correction  (m/s**2)
 	float max_correction_airspeed {20.0f};
+#endif // CONFIG_EKF2_BARO_COMPENSATION

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	// multi-rotor drag specific force fusion
 	float drag_noise{2.5f};                 ///< observation noise variance for drag specific force measurements (m/sec**2)**2
 	float bcoef_x{100.0f};                  ///< bluff body drag ballistic coefficient for the X-axis (kg/m**2)
 	float bcoef_y{100.0f};                  ///< bluff body drag ballistic coefficient for the Y-axis (kg/m**2)
 	float mcoef{0.1f};                      ///< rotor momentum drag coefficient for the X and Y axes (1/s)
+#endif // CONFIG_EKF2_DRAG_FUSION

 	// control of accel error detection and mitigation (IMU clipping)
 	const float vert_innov_test_lim{3.0f};          ///< Number of standard deviations of vertical vel/pos innovations allowed before triggering a vertical acceleration failure
 	const float vert_innov_test_min{1.0f};          ///< Minimum number of standard deviations of vertical vel/pos innovations required to trigger a vertical acceleration failure
 	const int bad_acc_reset_delay_us{500000};       ///< Continuous time that the vertical position and velocity innovation test must fail before the states are reset (uSec)

+#if defined(CONFIG_EKF2_AUXVEL)
 	// auxiliary velocity fusion
+	float auxvel_delay_ms{5.0f};            ///< auxiliary velocity measurement delay relative to the IMU (mSec)
 	const float auxvel_noise{0.5f};         ///< minimum observation noise, uses reported noise if greater (m/s)
 	const float auxvel_gate{5.0f};          ///< velocity fusion innovation consistency gate size (STD)
+#endif // CONFIG_EKF2_AUXVEL

 	// compute synthetic magnetomter Z value if possible
 	int32_t synthesize_mag_z{0};
@@ -593,6 +627,7 @@ union ekf_solution_status_u {
 	uint16_t value;
 };

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 union terrain_fusion_status_u {
 	struct {
 		bool range_finder : 1;  ///< 0 - true if we are fusing range finder data
@@ -600,6 +635,7 @@ union terrain_fusion_status_u {
 	} flags;
 	uint8_t value;
 };
+#endif // CONFIG_EKF2_RANGE_FINDER

 // define structure used to communicate information events
 union information_event_status_u {
@@ -104,19 +104,37 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)

 	// control use of observations for aiding
 	controlMagFusion();
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	controlOpticalFlowFusion(imu_delayed);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	controlGpsFusion(imu_delayed);
+
+#if defined(CONFIG_EKF2_AIRSPEED)
 	controlAirDataFusion(imu_delayed);
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
 	controlBetaFusion(imu_delayed);
+#endif // CONFIG_EKF2_SIDESLIP
+
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	controlDragFusion();
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 	controlHeightFusion(imu_delayed);
 	controlGravityFusion(imu_delayed);

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// Additional data odometry data from an external estimator can be fused.
 	controlExternalVisionFusion();
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_AUXVEL)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
 	controlAuxVelFusion();
+#endif // CONFIG_EKF2_AUXVEL

 	controlZeroInnovationHeadingUpdate();

@@ -69,16 +69,17 @@ void Ekf::initialiseCovariance()
 	// position
 	P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f));
 	P(8,8) = P(7,7);
+	P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));

+	if (_control_status.flags.gps_hgt) {
+		P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
+	}
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
 		P(9,9) = sq(fmaxf(_params.range_noise, 0.01f));
-
-	} else if (_control_status.flags.gps_hgt) {
-		P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f));
-
-	} else {
-		P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f));
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER

 	// gyro bias
 	_prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt);
@@ -444,14 +445,27 @@ void Ekf::fixCovarianceErrors(bool force_symmetry)
 		if (fabsf(down_dvel_bias) > dVel_bias_lim) {

 			bool bad_vz_gps = _control_status.flags.gps    && (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.0f);
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 			bool bad_vz_ev  = _control_status.flags.ev_vel && (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.0f);
+#else
+			bool bad_vz_ev  = false;
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 			if (bad_vz_gps || bad_vz_ev) {
 				bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f);
 				bool bad_z_gps  = _control_status.flags.gps_hgt  && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f);
-				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation  < 0.0f);
-				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _aid_src_ev_hgt.innovation   < 0.0f);

+#if defined(CONFIG_EKF2_RANGE_FINDER)
+				bool bad_z_rng  = _control_status.flags.rng_hgt  && (down_dvel_bias * _aid_src_rng_hgt.innovation  < 0.0f);
+#else
+				bool bad_z_rng  = false;
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+				bool bad_z_ev   = _control_status.flags.ev_hgt   && (down_dvel_bias * _aid_src_ev_hgt.innovation   < 0.0f);
+#else
+				bool bad_z_ev   = false;
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 				if (bad_z_baro || bad_z_gps || bad_z_rng || bad_z_ev) {
 					bad_acc_bias = true;
@@ -63,9 +63,11 @@ void Ekf::reset()
 	_state.wind_vel.setZero();
 	_state.quat_nominal.setIdentity();

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+#endif // CONFIG_EKF2_RANGE_FINDER

 	_control_status.value = 0;
 	_control_status_prev.value = 0;
@@ -94,17 +96,13 @@ void Ekf::reset()
 	_time_last_hor_vel_fuse = 0;
 	_time_last_ver_vel_fuse = 0;
 	_time_last_heading_fuse = 0;
-
-	_time_last_flow_terrain_fuse = 0;
 	_time_last_zero_velocity_fuse = 0;
-	_time_last_healthy_rng_data = 0;

 	_last_known_pos.setZero();

 	_time_acc_bias_check = 0;

 	_gps_checks_passed = false;
-
 	_gps_alt_ref = NAN;

 	_baro_counter = 0;
@@ -115,30 +113,46 @@ void Ekf::reset()
 	_clip_counter = 0;

 	resetEstimatorAidStatus(_aid_src_baro_hgt);
-	resetEstimatorAidStatus(_aid_src_rng_hgt);
+#if defined(CONFIG_EKF2_AIRSPEED)
 	resetEstimatorAidStatus(_aid_src_airspeed);
+#endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_SIDESLIP)
 	resetEstimatorAidStatus(_aid_src_sideslip);
+#endif // CONFIG_EKF2_SIDESLIP

 	resetEstimatorAidStatus(_aid_src_fake_pos);
 	resetEstimatorAidStatus(_aid_src_fake_hgt);

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	resetEstimatorAidStatus(_aid_src_ev_hgt);
 	resetEstimatorAidStatus(_aid_src_ev_pos);
 	resetEstimatorAidStatus(_aid_src_ev_vel);
 	resetEstimatorAidStatus(_aid_src_ev_yaw);
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	resetEstimatorAidStatus(_aid_src_gnss_hgt);
 	resetEstimatorAidStatus(_aid_src_gnss_pos);
 	resetEstimatorAidStatus(_aid_src_gnss_vel);
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	resetEstimatorAidStatus(_aid_src_gnss_yaw);
+#endif // CONFIG_EKF2_GNSS_YAW

 	resetEstimatorAidStatus(_aid_src_mag_heading);
 	resetEstimatorAidStatus(_aid_src_mag);

+#if defined(CONFIG_EKF2_AUXVEL)
 	resetEstimatorAidStatus(_aid_src_aux_vel);
+#endif // CONFIG_EKF2_AUXVEL

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	resetEstimatorAidStatus(_aid_src_optical_flow);
 	resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	resetEstimatorAidStatus(_aid_src_rng_hgt);
+#endif // CONFIG_EKF2_RANGE_FINDER
 }

 bool Ekf::update()
@@ -166,8 +180,10 @@ bool Ekf::update()
 		// control fusion of observation data
 		controlFusionModes(imu_sample_delayed);

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		// run a separate filter for terrain estimation
 		runTerrainEstimator(imu_sample_delayed);
+#endif // CONFIG_EKF2_RANGE_FINDER

 		_output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(),
 							_state.quat_nominal, _state.vel, _state.pos);
@@ -205,8 +221,10 @@ bool Ekf::initialiseFilter()
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Initialise the terrain estimator
 	initHagl();
+#endif // CONFIG_EKF2_RANGE_FINDER

 	// reset the output predictor state history to match the EKF initial values
 	_output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos);
@@ -64,7 +64,6 @@ public:
 	typedef matrix::Vector<float, _k_num_states> Vector24f;
 	typedef matrix::SquareMatrix<float, _k_num_states> SquareMatrix24f;
 	typedef matrix::SquareMatrix<float, 2> Matrix2f;
-	typedef matrix::Vector<float, 4> Vector4f;
 	template<int ... Idxs>

 	using SparseVector24f = matrix::SparseVectorf<24, Idxs...>;
@@ -86,21 +85,50 @@ public:
 	void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	void getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const;
 	void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const;
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; }
 	void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; }
 	void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; }

+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
+	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }
+
 	void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; }
 	void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; }
 	void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; }

-	void getAuxVelInnov(float aux_vel_innov[2]) const;
-	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
-	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
+	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
+	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
+	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
+
+	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
+	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
+	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+
+	// terrain estimate
+	bool isTerrainEstimateValid() const;
+
+	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
+
+	// get the estimated terrain vertical position relative to the NED origin
+	float getTerrainVertPos() const { return _terrain_vpos; };
+
+	// get the number of times the vertical terrain position has been reset
+	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
+
+	// get the terrain variance
+	float get_terrain_var() const { return _terrain_var; }
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }

 	void getFlowInnov(float flow_innov[2]) const;
 	void getFlowInnovVar(float flow_innov_var[2]) const;
@@ -119,77 +147,117 @@ public:
 	void getTerrainFlowInnovVar(float flow_innov_var[2]) const;
 	void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); }

+	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_AUXVEL)
+	void getAuxVelInnov(float aux_vel_innov[2]) const;
+	void getAuxVelInnovVar(float aux_vel_innov[2]) const;
+	void getAuxVelInnovRatio(float &aux_vel_innov_ratio) const { aux_vel_innov_ratio = math::max(_aid_src_aux_vel.test_ratio[0], _aid_src_aux_vel.test_ratio[1]); }
+#endif // CONFIG_EKF2_AUXVEL
+
 	void getHeadingInnov(float &heading_innov) const
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov = _aid_src_mag_heading.innovation;
-
-		} else if (_control_status.flags.mag_3D) {
-			heading_innov = Vector3f(_aid_src_mag.innovation).max();
-
-		} else if (_control_status.flags.gps_yaw) {
-			heading_innov = _aid_src_gnss_yaw.innovation;
-
-		} else if (_control_status.flags.ev_yaw) {
-			heading_innov = _aid_src_ev_yaw.innovation;
+			return;
 		}
+
+		if (_control_status.flags.mag_3D) {
+			heading_innov = Vector3f(_aid_src_mag.innovation).max();
+			return;
+		}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
+			heading_innov = _aid_src_gnss_yaw.innovation;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_control_status.flags.ev_yaw) {
+			heading_innov = _aid_src_ev_yaw.innovation;
+			return;
+		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}

 	void getHeadingInnovVar(float &heading_innov_var) const
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_var = _aid_src_mag_heading.innovation_variance;
-
-		} else if (_control_status.flags.mag_3D) {
-			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
-
-		} else if (_control_status.flags.gps_yaw) {
-			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
-
-		} else if (_control_status.flags.ev_yaw) {
-			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+			return;
 		}
+
+		if (_control_status.flags.mag_3D) {
+			heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max();
+			return;
+		}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
+			heading_innov_var = _aid_src_gnss_yaw.innovation_variance;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_control_status.flags.ev_yaw) {
+			heading_innov_var = _aid_src_ev_yaw.innovation_variance;
+			return;
+		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}

 	void getHeadingInnovRatio(float &heading_innov_ratio) const
 	{
 		if (_control_status.flags.mag_hdg) {
 			heading_innov_ratio = _aid_src_mag_heading.test_ratio;
-
-		} else if (_control_status.flags.mag_3D) {
-			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
-
-		} else if (_control_status.flags.gps_yaw) {
-			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
-
-		} else if (_control_status.flags.ev_yaw) {
-			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+			return;
 		}
+
+		if (_control_status.flags.mag_3D) {
+			heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max();
+			return;
+		}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+		if (_control_status.flags.gps_yaw) {
+			heading_innov_ratio = _aid_src_gnss_yaw.test_ratio;
+			return;
+		}
+#endif // CONFIG_EKF2_GNSS_YAW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_control_status.flags.ev_yaw) {
+			heading_innov_ratio = _aid_src_ev_yaw.test_ratio;
+			return;
+		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}

 	void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); }
 	void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); }
 	void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); }

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); }
 	void getDragInnovVar(float drag_innov_var[2]) const { _drag_innov_var.copyTo(drag_innov_var); }
 	void getDragInnovRatio(float drag_innov_ratio[2]) const { _drag_test_ratio.copyTo(drag_innov_ratio); }
+#endif // CONFIG_EKF2_DRAG_FUSION

+#if defined(CONFIG_EKF2_AIRSPEED)
 	void getAirspeedInnov(float &airspeed_innov) const { airspeed_innov = _aid_src_airspeed.innovation; }
 	void getAirspeedInnovVar(float &airspeed_innov_var) const { airspeed_innov_var = _aid_src_airspeed.innovation_variance; }
 	void getAirspeedInnovRatio(float &airspeed_innov_ratio) const { airspeed_innov_ratio = _aid_src_airspeed.test_ratio; }
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_SIDESLIP)
 	void getBetaInnov(float &beta_innov) const { beta_innov = _aid_src_sideslip.innovation; }
 	void getBetaInnovVar(float &beta_innov_var) const { beta_innov_var = _aid_src_sideslip.innovation_variance; }
 	void getBetaInnovRatio(float &beta_innov_ratio) const { beta_innov_ratio = _aid_src_sideslip.test_ratio; }
-
-	void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; }
-	void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; }
-	void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; }
-
-	void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); }
-	void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); }
-	void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); }
+#endif // CONFIG_EKF2_SIDESLIP

 	void getGravityInnov(float grav_innov[3]) const { memcpy(grav_innov, _aid_src_gravity.innovation, sizeof(_aid_src_gravity.innovation)); }
 	void getGravityInnovVar(float grav_innov_var[3]) const { memcpy(grav_innov_var, _aid_src_gravity.innovation_variance, sizeof(_aid_src_gravity.innovation_variance)); }
@@ -204,9 +272,6 @@ public:
 	// get the wind velocity var
 	Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); }

-	// get the true airspeed in m/s
-	float getTrueAirspeed() const;
-
 	// get the full covariance matrix
 	const matrix::SquareMatrix<float, 24> &covariances() const { return P; }

@@ -287,8 +352,6 @@ public:
 		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
 	}

-	bool isTerrainEstimateValid() const;
-
 	bool isYawFinalAlignComplete() const
 	{
 		const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg);
@@ -299,17 +362,6 @@ public:
 		       && (is_mag_alignment_in_flight_complete || !is_using_mag);
 	}

-	uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; }
-
-	// get the estimated terrain vertical position relative to the NED origin
-	float getTerrainVertPos() const { return _terrain_vpos; };
-
-	// get the number of times the vertical terrain position has been reset
-	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
-
-	// get the terrain variance
-	float get_terrain_var() const { return _terrain_var; }
-
 	// gyro bias (states 10, 11, 12)
 	Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s
 	Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s
@@ -410,39 +462,49 @@ public:
 	uint8_t getHeightSensorRef() const { return _height_sensor_ref; }
 	const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 	const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); }
-	const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); }
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	const BiasEstimator::status &getEvHgtBiasEstimatorStatus() const { return _ev_hgt_b_est.getStatus(); }

 	const BiasEstimator::status &getEvPosBiasEstimatorStatus(int i) const { return _ev_pos_b_est.getStatus(i); }
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_AIRSPEED)
 	const auto &aid_src_airspeed() const { return _aid_src_airspeed; }
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
 	const auto &aid_src_sideslip() const { return _aid_src_sideslip; }
+#endif // CONFIG_EKF2_SIDESLIP

 	const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; }
-	const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; }

 	const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; }
 	const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; }

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	const auto &aid_src_ev_hgt() const { return _aid_src_ev_hgt; }
 	const auto &aid_src_ev_pos() const { return _aid_src_ev_pos; }
 	const auto &aid_src_ev_vel() const { return _aid_src_ev_vel; }
 	const auto &aid_src_ev_yaw() const { return _aid_src_ev_yaw; }
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	const auto &aid_src_gnss_hgt() const { return _aid_src_gnss_hgt; }
 	const auto &aid_src_gnss_pos() const { return _aid_src_gnss_pos; }
 	const auto &aid_src_gnss_vel() const { return _aid_src_gnss_vel; }
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; }
+#endif // CONFIG_EKF2_GNSS_YAW

 	const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; }
 	const auto &aid_src_mag() const { return _aid_src_mag; }

 	const auto &aid_src_gravity() const { return _aid_src_gravity; }

+#if defined(CONFIG_EKF2_AUXVEL)
 	const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; }
-
-	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
-	const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; }
+#endif // CONFIG_EKF2_AUXVEL

 private:

@@ -483,13 +545,8 @@ private:

 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised

-	float _ev_yaw_pred_prev{};                 ///< previous value of yaw state used by odometry fusion (m)
-	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
-
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
-	bool _flow_for_terrain_data_ready{false}; /// same flag as "_flow_data_ready" but used for separate terrain estimator

 	uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec)
 	uint64_t _time_last_v_pos_aiding{0};
@@ -500,15 +557,7 @@ private:
 	uint64_t _time_last_hor_vel_fuse{0};	///< time the last fusion of horizontal velocity measurements was performed (uSec)
 	uint64_t _time_last_ver_vel_fuse{0};	///< time the last fusion of verticalvelocity measurements was performed (uSec)
 	uint64_t _time_last_heading_fuse{0};
-
-	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
 	uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec)
-	uint64_t _time_last_healthy_rng_data{0};
-	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
-
-	uint8_t _nb_ev_pos_reset_available{0};
-	uint8_t _nb_ev_vel_reset_available{0};
-	uint8_t _nb_ev_yaw_reset_available{0};

 	Vector3f _last_known_pos{};		///< last known local position vector (m)

@@ -537,11 +586,35 @@ private:
 	Vector3f _delta_angle_bias_var_accum{};	///< kahan summation algorithm accumulator for delta angle bias variance
 	Vector3f _delta_vel_bias_var_accum{};   ///< kahan summation algorithm accumulator for delta velocity bias variance

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	Vector2f _drag_innov{};		///< multirotor drag measurement innovation (m/sec**2)
 	Vector2f _drag_innov_var{};	///< multirotor drag measurement innovation variance ((m/sec**2)**2)
+#endif // CONFIG_EKF2_DRAG_FUSION
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	estimator_aid_source1d_s _aid_src_rng_hgt{};
+
+	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};

 	float _hagl_innov{0.0f};		///< innovation of the last height above terrain measurement (m)
 	float _hagl_innov_var{0.0f};		///< innovation variance for the last height above terrain measurement (m**2)
+	float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio
+
+	uint64_t _time_last_healthy_rng_data{0};
+
+	// Terrain height state estimation
+	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
+	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
+	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
+	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
+	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
+
+	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	estimator_aid_source2d_s _aid_src_optical_flow{};
+	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};

 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
@@ -554,33 +627,52 @@ private:
 	uint64_t _time_good_motion_us{0};	///< last system time that on-ground motion was within limits (uSec)
 	Vector2f _flow_compensated_XY_rad{};	///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive

+	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
+	uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
 	estimator_aid_source1d_s _aid_src_baro_hgt{};
-	estimator_aid_source1d_s _aid_src_rng_hgt{};
+#if defined(CONFIG_EKF2_AIRSPEED)
 	estimator_aid_source1d_s _aid_src_airspeed{};
+#endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_SIDESLIP)
 	estimator_aid_source1d_s _aid_src_sideslip{};
+#endif // CONFIG_EKF2_SIDESLIP

 	estimator_aid_source2d_s _aid_src_fake_pos{};
 	estimator_aid_source1d_s _aid_src_fake_hgt{};

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	estimator_aid_source1d_s _aid_src_ev_hgt{};
 	estimator_aid_source2d_s _aid_src_ev_pos{};
 	estimator_aid_source3d_s _aid_src_ev_vel{};
 	estimator_aid_source1d_s _aid_src_ev_yaw{};

+	float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
+
+	uint8_t _nb_ev_pos_reset_available{0};
+	uint8_t _nb_ev_vel_reset_available{0};
+	uint8_t _nb_ev_yaw_reset_available{0};
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+	bool _inhibit_ev_yaw_use{false};	///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North)
+
 	estimator_aid_source1d_s _aid_src_gnss_hgt{};
 	estimator_aid_source2d_s _aid_src_gnss_pos{};
 	estimator_aid_source3d_s _aid_src_gnss_vel{};
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	estimator_aid_source1d_s _aid_src_gnss_yaw{};
+	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
+#endif // CONFIG_EKF2_GNSS_YAW

 	estimator_aid_source1d_s _aid_src_mag_heading{};
 	estimator_aid_source3d_s _aid_src_mag{};

 	estimator_aid_source3d_s _aid_src_gravity{};

+#if defined(CONFIG_EKF2_AUXVEL)
 	estimator_aid_source2d_s _aid_src_aux_vel{};
-
-	estimator_aid_source2d_s _aid_src_optical_flow{};
-	estimator_aid_source2d_s _aid_src_terrain_optical_flow{};
+#endif // CONFIG_EKF2_AUXVEL

 	// variables used for the GPS quality checks
 	Vector3f _gps_pos_deriv_filt{};	///< GPS NED position derivative (m/sec)
@@ -608,7 +700,6 @@ private:
 	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)

 	// Variables used to control activation of post takeoff functionality
-	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 	uint64_t _flt_mag_align_start_time{0};	///< time that inflight magnetic field alignment started (uSec)
 	uint64_t _time_last_mov_3d_mag_suitable{0};	///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec)
 	Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2)
@@ -626,13 +717,6 @@ private:
 	Vector3f _prev_delta_ang_bias_var{};	///< saved delta angle XYZ bias variances (rad/sec)
 	Vector3f _prev_dvel_bias_var{};		///< saved delta velocity XYZ bias variances (m/sec)**2

-	// Terrain height state estimation
-	float _terrain_vpos{0.0f};		///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m)
-	float _terrain_var{1e4f};		///< variance of terrain position estimate (m**2)
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-	uint64_t _time_last_hagl_fuse{0};		///< last system time that a range sample was fused by the terrain estimator
-	terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground
-
 	// height sensor status
 	bool _baro_hgt_faulty{false};		///< true if baro data have been declared faulty TODO: move to fault flags
 	bool _gps_intermittent{true};           ///< true if data into the buffer is intermittent
@@ -664,6 +748,9 @@ private:
 	bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const;

+#if defined(CONFIG_EKF2_GNSS_YAW)
+	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
+
 	// fuse the yaw angle obtained from a dual antenna GPS unit
 	void fuseGpsYaw();

@@ -671,6 +758,13 @@ private:
 	// return true if the reset was successful
 	bool resetYawToGps(const float gnss_yaw);

+	void updateGpsYaw(const gpsSample &gps_sample);
+
+	void startGpsYawFusion(const gpsSample &gps_sample);
+
+#endif // CONFIG_EKF2_GNSS_YAW
+	void stopGpsYawFusion();
+
 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
 	bool fuseDeclination(float decl_sigma);
@@ -678,15 +772,38 @@ private:
 	// apply sensible limits to the declination and length of the NE mag field states estimates
 	void limitDeclination();

+#if defined(CONFIG_EKF2_AIRSPEED)
+	// control fusion of air data observations
+	void controlAirDataFusion(const imuSample &imu_delayed);
+
 	void updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const;
 	void fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src);

+	void stopAirspeedFusion();
+
+	// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
+	void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
+
+	// perform a limited reset of the wind state covariances
+	void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_SIDESLIP)
+	// control fusion of synthetic sideslip observations
+	void controlBetaFusion(const imuSample &imu_delayed);
+
 	// fuse synthetic zero sideslip measurement
 	void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const;
 	void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip);
+#endif // CONFIG_EKF2_SIDESLIP
+
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	// control fusion of multi-rotor drag specific force observations
+	void controlDragFusion();

 	// fuse body frame drag specific forces for multi-rotor wind estimation
 	void fuseDrag(const dragSample &drag_sample);
+#endif // CONFIG_EKF2_DRAG_FUSION

 	// fuse single velocity and position measurement
 	bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
@@ -710,12 +827,6 @@ private:

 	void resetVerticalVelocityToZero();

-	// fuse optical flow line of sight rate measurements
-	void updateOptFlow(estimator_aid_source2d_s &aid_src);
-	void fuseOptFlow();
-	float predictFlowRange();
-	Vector2f predictFlowVelBody();
-
 	// horizontal and vertical position aid source
 	void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
 	void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
@@ -732,18 +843,19 @@ private:
 	void fuseVelocity(estimator_aid_source2d_s &vel_aid_src);
 	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);

-	void updateGpsYaw(const gpsSample &gps_sample);
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	// range height
+	void controlRangeHeightFusion();
+	bool isConditionalRangeAidSuitable();
+	void stopRngHgtFusion();

-	// calculate optical flow body angular rate compensation
-	// returns false if bias corrected body rate data is unavailable
-	bool calcOptFlowBodyRateComp();
-
-	// initialise the terrain vertical position estimator
+	// terrain vertical position estimator
 	void initHagl();
-
 	void runTerrainEstimator(const imuSample &imu_delayed);
 	void predictHagl(const imuSample &imu_delayed);

+	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
+
 	// update the terrain vertical position estimate using a height above ground measurement from the range finder
 	void controlHaglRngFusion();
 	void fuseHaglRng();
@@ -753,14 +865,37 @@ private:
 	void stopHaglRngFusion();
 	float getRngVar();

+	void controlHaglFakeFusion();
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	// control fusion of optical flow observations
+	void controlOpticalFlowFusion(const imuSample &imu_delayed);
+	void stopFlowFusion();
+
+	void updateOnGroundMotionForOpticalFlowChecks();
+	void resetOnGroundMotionForOpticalFlowChecks();
+
+	// calculate the measurement variance for the optical flow sensor
+	float calcOptFlowMeasVar(const flowSample &flow_sample);
+
+	// calculate optical flow body angular rate compensation
+	// returns false if bias corrected body rate data is unavailable
+	bool calcOptFlowBodyRateComp();
+
+	// fuse optical flow line of sight rate measurements
+	void updateOptFlow(estimator_aid_source2d_s &aid_src);
+	void fuseOptFlow();
+	float predictFlowRange();
+	Vector2f predictFlowVelBody();
+
 	// update the terrain vertical position estimate using an optical flow measurement
 	void controlHaglFlowFusion();
 	void startHaglFlowFusion();
 	void resetHaglFlow();
 	void stopHaglFlowFusion();
 	void fuseFlowForTerrain(estimator_aid_source2d_s &flow);
-
-	void controlHaglFakeFusion();
+#endif // CONFIG_EKF2_OPTICAL_FLOW

 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
@@ -862,36 +997,30 @@ private:
 	// Control the filter fusion modes
 	void controlFusionModes(const imuSample &imu_delayed);

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// control fusion of external vision observations
 	void controlExternalVisionFusion();
-
 	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-
 	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
-	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
-	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
-	void stopEvPosFusion();
-
 	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
 	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);

-	// control fusion of optical flow observations
-	void controlOpticalFlowFusion(const imuSample &imu_delayed);
-	void updateOnGroundMotionForOpticalFlowChecks();
-	void resetOnGroundMotionForOpticalFlowChecks();
+	void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src);
+	void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src);
+	void stopEvPosFusion();
+	void stopEvHgtFusion();
+	void stopEvVelFusion();
+	void stopEvYawFusion();
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
 	bool shouldResetGpsFusion() const;
 	bool isYawFailure() const;

-	void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing);
-
 	// control fusion of magnetometer observations
 	void controlMagFusion();

-	float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; }
-
 	bool magReset();
 	bool haglYawResetReq();

@@ -908,15 +1037,6 @@ private:
 	void runMagAndMagDeclFusions(const Vector3f &mag);
 	void run3DMagAndDeclFusions(const Vector3f &mag);

-	// control fusion of air data observations
-	void controlAirDataFusion(const imuSample &imu_delayed);
-
-	// control fusion of synthetic sideslip observations
-	void controlBetaFusion(const imuSample &imu_delayed);
-
-	// control fusion of multi-rotor drag specific force observations
-	void controlDragFusion();
-
 	// control fusion of fake position observations to constrain drift
 	void controlFakePosFusion();

@@ -929,8 +1049,11 @@ private:

 	void controlZeroInnovationHeadingUpdate();

+#if defined(CONFIG_EKF2_AUXVEL)
 	// control fusion of auxiliary velocity observations
 	void controlAuxVelFusion();
+	void stopAuxVelFusion();
+#endif // CONFIG_EKF2_AUXVEL

 	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;
@@ -940,9 +1063,6 @@ private:
 	void checkHeightSensorRefFallback();
 	void controlBaroHeightFusion();
 	void controlGnssHeightFusion(const gpsSample &gps_sample);
-	void controlRangeHeightFusion();
-
-	bool isConditionalRangeAidSuitable();

 	void stopMagFusion();
 	void stopMag3DFusion();
@@ -952,17 +1072,12 @@ private:

 	void stopBaroHgtFusion();
 	void stopGpsHgtFusion();
-	void stopRngHgtFusion();
-	void stopEvHgtFusion();

 	void updateGroundEffect();

 	// gravity fusion: heuristically enable / disable gravity fusion
 	void controlGravityFusion(const imuSample &imu_delayed);

-	// calculate the measurement variance for the optical flow sensor
-	float calcOptFlowMeasVar(const flowSample &flow_sample);
-
 	// initialise the quaternion covariances using rotation vector variances
 	// do not call before quaternion states are initialised
 	void initialiseQuatCovariances(Vector3f &rot_vec_var);
@@ -976,18 +1091,8 @@ private:

 	// perform a reset of the wind states and related covariances
 	void resetWind();
-
-	// Reset the wind states using the current airspeed measurement, ground relative nav velocity, yaw angle and assumption of zero sideslip
-	void resetWindUsingAirspeed(const airspeedSample &airspeed_sample);
-
-	// perform a limited reset of the wind state covariances
-	void resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample);
-
 	void resetWindToZero();

-	// check that the range finder data is continuous
-	void updateRangeDataContinuity();
-
 	// Increase the yaw error variance of the quaternions
 	// Argument is additional yaw variance in rad**2
 	void increaseQuatYawErrVariance(float yaw_variance);
@@ -1022,22 +1127,10 @@ private:
 		return (sensor_timestamp != 0) && (sensor_timestamp + acceptance_interval > _time_latest_us);
 	}

-	void stopAirspeedFusion();
-
 	void stopGpsFusion();
 	void stopGpsPosFusion();
 	void stopGpsVelFusion();

-	void startGpsYawFusion(const gpsSample &gps_sample);
-	void stopGpsYawFusion();
-
-	void stopEvVelFusion();
-	void stopEvYawFusion();
-
-	void stopAuxVelFusion();
-
-	void stopFlowFusion();
-
 	void resetFakePosFusion();
 	void stopFakePosFusion();

@@ -1058,9 +1151,11 @@ private:

 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
 	HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref};
-	HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref};
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{static_cast<uint8_t>(PositionSensor::EV), _position_sensor_ref};
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
 	// Resets the horizontal velocity and position to the default navigation sensor
@@ -150,7 +150,9 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f

 	_state_reset_status.reset_count.posNE++;

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	//_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change);

 	// Reset the timout timer
@@ -196,9 +198,13 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_state_reset_status.reset_count.posD++;

 	_baro_b_est.setBias(_baro_b_est.getBias() + delta_z);
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z);
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
+#endif // CONFIG_EKF2_RANGE_FINDER

 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
@@ -231,6 +237,7 @@ void Ekf::constrainStates()

 float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
 {
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
 		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
@@ -258,6 +265,7 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
 		// correct baro measurement using pressure error estimate and assuming sea level gravity
 		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
 	}
+#endif // CONFIG_EKF2_BARO_COMPENSATION

 	// otherwise return the uncorrected baro measurement
 	return baro_alt_uncompensated;
@@ -302,6 +310,7 @@ void Ekf::getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &v
 	vpos = _aid_src_gnss_hgt.test_ratio;
 }

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void Ekf::getEvVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const
 {
 	hvel[0] = _aid_src_ev_vel.innovation[0];
@@ -332,7 +341,9 @@ void Ekf::getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vp
 	hpos = fmaxf(_aid_src_ev_pos.test_ratio[0], _aid_src_ev_pos.test_ratio[1]);
 	vpos = _aid_src_ev_hgt.test_ratio;
 }
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_AUXVEL)
 void Ekf::getAuxVelInnov(float aux_vel_innov[2]) const
 {
 	aux_vel_innov[0] = _aid_src_aux_vel.innovation[0];
@@ -344,7 +355,9 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const
 	aux_vel_innov_var[0] = _aid_src_aux_vel.innovation_variance[0];
 	aux_vel_innov_var[1] = _aid_src_aux_vel.innovation_variance[1];
 }
+#endif // CONFIG_EKF2_AUXVEL

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void Ekf::getFlowInnov(float flow_innov[2]) const
 {
 	flow_innov[0] = _aid_src_optical_flow.innovation[0];
@@ -368,6 +381,7 @@ void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const
 	flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0];
 	flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1];
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW

 // get the state vector at the delayed time horizon
 matrix::Vector<float, 24> Ekf::getStateAtFusionHorizonAsVector() const
@@ -462,9 +476,11 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}

 	*ekf_eph = hpos_err;
@@ -485,9 +501,11 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm());
 		}

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 		if (_control_status.flags.ev_pos) {
 			hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
 	}

 	*ekf_eph = hpos_err;
@@ -505,21 +523,26 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	if (_horizontal_deadreckon_time_exceeded) {
 		float vel_err_conservative = 0.0f;

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
 			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm();
 		}
+#endif // CONFIG_EKF2_OPTICAL_FLOW

 		if (_control_status.flags.gps) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm());
+		}

-		} else if (_control_status.flags.ev_pos) {
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+		if (_control_status.flags.ev_pos) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm());
 		}

 		if (_control_status.flags.ev_vel) {
 			vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm());
 		}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 		hvel_err = math::max(hvel_err, vel_err_conservative);
 	}
@@ -543,6 +566,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 	*hagl_min = NAN;
 	*hagl_max = NAN;

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// Calculate range finder limits
 	const float rangefinder_hagl_min = _range_sensor.getValidMinVal();

@@ -551,15 +575,18 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl

 	// TODO : calculate visual odometry limits
 	const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt);
-	const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);

 	// Keep within range sensor limit when using rangefinder as primary height source
 	if (relying_on_rangefinder) {
 		*hagl_min = rangefinder_hagl_min;
 		*hagl_max = rangefinder_hagl_max;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// Keep within flow AND range sensor limits when exclusively using optical flow
+	const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow);
+
 	if (relying_on_optical_flow) {
 		// Calculate optical flow limits
 		const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance);
@@ -574,6 +601,7 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 		*hagl_min = flow_hagl_min;
 		*hagl_max = flow_hagl_max;
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
 }

 void Ekf::resetImuBias()
@@ -617,19 +645,28 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 	status = _innov_check_fail_status.value;

 	// return the largest magnetometer innovation test ratio
+	mag = 0.f;
+
 	if (_control_status.flags.mag_hdg) {
-		mag = sqrtf(_aid_src_mag_heading.test_ratio);
-
-	} else if (_control_status.flags.mag_3D) {
-		mag = sqrtf(Vector3f(_aid_src_mag.test_ratio).max());
-
-	} else if (_control_status.flags.gps_yaw) {
-		mag = sqrtf(_aid_src_gnss_yaw.test_ratio);
-
-	} else {
-		mag = NAN;
+		mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio));
 	}

+	if (_control_status.flags.mag_3D) {
+		mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max()));
+	}
+
+#if defined(CONFIG_EKF2_GNSS_YAW)
+	if (_control_status.flags.gps_yaw) {
+		mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio));
+	}
+#endif // CONFIG_EKF2_GNSS_YAW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_yaw) {
+		mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio));
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
 	// return the largest velocity and position innovation test ratio
 	vel = NAN;
 	pos = NAN;
@@ -642,6 +679,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		pos = math::max(gps_pos, FLT_MIN);
 	}

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_vel) {
 		float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max());
 		vel = math::max(vel, ev_vel, FLT_MIN);
@@ -651,11 +689,14 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max());
 		pos = math::max(pos, ev_pos, FLT_MIN);
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) {
 		float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max());
 		vel = math::max(of_vel, FLT_MIN);
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW

 	// return the combined vertical position innovation test ratio
 	float hgt_sum = 0.f;
@@ -671,15 +712,19 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		n_hgt_sources++;
 	}

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_control_status.flags.rng_hgt) {
 		hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio);
 		n_hgt_sources++;
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_hgt) {
 		hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio);
 		n_hgt_sources++;
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	if (n_hgt_sources > 0) {
 		hgt = math::max(hgt_sum / static_cast<float>(n_hgt_sources), FLT_MIN);
@@ -688,20 +733,26 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 		hgt = NAN;
 	}

+#if defined(CONFIG_EKF2_AIRSPEED)
 	// return the airspeed fusion innovation test ratio
 	tas = sqrtf(_aid_src_airspeed.test_ratio);
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// return the terrain height innovation test ratio
 	hagl = sqrtf(_hagl_test_ratio);
+#endif // CONFIG_EKF2_RANGE_FINDER

+#if defined(CONFIG_EKF2_SIDESLIP)
 	// return the synthetic sideslip innovation test ratio
 	beta = sqrtf(_aid_src_sideslip.test_ratio);
+#endif // CONFIG_EKF2_SIDESLIP
 }

 // return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
-	ekf_solution_status_u soln_status;
+	ekf_solution_status_u soln_status{};
 	// TODO: Is this accurate enough?
 	soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
 	soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
@@ -709,7 +760,9 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const
 	soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0);
 	soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0);
 	soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	soln_status.flags.pos_vert_agl = isTerrainEstimateValid();
+#endif // CONFIG_EKF2_RANGE_FINDER
 	soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz;
 	soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel;
 	soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs;
@@ -768,13 +821,23 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 				  && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max)
 				      || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max));

-	const bool optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+	bool optFlowAiding = false;
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max);
+#endif // CONFIG_EKF2_OPTICAL_FLOW

-	const bool airDataAiding = _control_status.flags.wind &&
+	bool airDataAiding = false;
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+	airDataAiding = _control_status.flags.wind &&
 				   isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) &&
 				   isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max);

 	_control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding;
+#else
+	_control_status.flags.wind_dead_reckoning = false;
+#endif // CONFIG_EKF2_AIRSPEED
+
 	_control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;

 	if (!_control_status.flags.inertial_dead_reckoning) {
@@ -963,12 +1026,15 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
 void Ekf::updateGroundEffect()
 {
 	if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 		if (isTerrainEstimateValid()) {
 			// automatically set ground effect if terrain is valid
 			float height = _terrain_vpos - _state.pos(2);
 			_control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt);

-		} else if (_control_status.flags.gnd_effect) {
+		} else
+#endif // CONFIG_EKF2_RANGE_FINDER
+		if (_control_status.flags.gnd_effect) {
 			// Turn off ground effect compensation if it times out
 			if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) {
 				_control_status.flags.gnd_effect = false;
@@ -1118,9 +1184,12 @@ bool Ekf::resetYawToEKFGSF()
 		_control_status.flags.gps_yaw_fault = true;
 		_warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true;

-	} else if (_control_status.flags.ev_yaw) {
+	}
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	if (_control_status.flags.ev_yaw) {
 		_inhibit_ev_yaw_use = true;
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	return true;
 }
@@ -1151,3 +1220,26 @@ void Ekf::resetGpsDriftCheckFilters()
 	_gps_vertical_position_drift_rate_m_s = NAN;
 	_gps_filtered_horizontal_velocity_m_s = NAN;
 }
+
+void Ekf::resetWind()
+{
+#if defined(CONFIG_EKF2_AIRSPEED)
+	if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) {
+		resetWindUsingAirspeed(_airspeed_sample_delayed);
+		return;
+	}
+#endif // CONFIG_EKF2_AIRSPEED
+
+	resetWindToZero();
+}
+
+void Ekf::resetWindToZero()
+{
+	ECL_INFO("reset wind to zero");
+
+	// If we don't have an airspeed measurement, then assume the wind is zero
+	_state.wind_vel.setZero();
+
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty);
+}
@@ -49,12 +49,24 @@ EstimatorInterface::~EstimatorInterface()
 	delete _gps_buffer;
 	delete _mag_buffer;
 	delete _baro_buffer;
+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	delete _range_buffer;
+#endif // CONFIG_EKF2_RANGE_FINDER
+#if defined(CONFIG_EKF2_AIRSPEED)
 	delete _airspeed_buffer;
+#endif // CONFIG_EKF2_AIRSPEED
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	delete _flow_buffer;
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	delete _ext_vision_buffer;
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	delete _drag_buffer;
+#endif // CONFIG_EKF2_DRAG_FUSION
+#if defined(CONFIG_EKF2_AUXVEL)
 	delete _auxvel_buffer;
+#endif // CONFIG_EKF2_AUXVEL
 }

 // Accumulate imu data and store to buffer at desired rate
@@ -85,7 +97,9 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 		_min_obs_interval_us = (imu_sample.time_us - _time_delayed_us) / (_obs_buffer_length - 1);
 	}

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	setDragData(imu_sample);
+#endif // CONFIG_EKF2_DRAG_FUSION
 }

 void EstimatorInterface::setMagData(const magSample &mag_sample)
@@ -164,7 +178,17 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)

 		gps_sample_new.hgt = (float)gps.alt * 1e-3f;

-		gps_sample_new.yaw = gps.yaw;
+#if defined(CONFIG_EKF2_GNSS_YAW)
+
+		if (PX4_ISFINITE(gps.yaw)) {
+			_time_last_gps_yaw_buffer_push = _time_latest_us;
+			gps_sample_new.yaw = gps.yaw;
+			gps_sample_new.yaw_acc = PX4_ISFINITE(gps.yaw_accuracy) ? gps.yaw_accuracy : 0.f;
+
+		} else {
+			gps_sample_new.yaw = NAN;
+			gps_sample_new.yaw_acc = 0.f;
+		}

 		if (PX4_ISFINITE(gps.yaw_offset)) {
 			_gps_yaw_offset = gps.yaw_offset;
@@ -173,12 +197,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 			_gps_yaw_offset = 0.0f;
 		}

-		if (PX4_ISFINITE(gps.yaw_accuracy)) {
-			gps_sample_new.yaw_acc = gps.yaw_accuracy;
-
-		} else {
-			gps_sample_new.yaw_acc = 0.f;
-		}
+#endif // CONFIG_EKF2_GNSS_YAW

 		// Only calculate the relative position if the WGS-84 location of the origin is set
 		if (collect_gps(gps)) {
@@ -192,9 +211,6 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps)
 		_gps_buffer->push(gps_sample_new);
 		_time_last_gps_buffer_push = _time_latest_us;

-		if (PX4_ISFINITE(gps.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
-		}

 	} else {
 		ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us);
@@ -237,6 +253,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample)
 	}
 }

+#if defined(CONFIG_EKF2_AIRSPEED)
 void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 {
 	if (!_initialised) {
@@ -271,7 +288,9 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 		ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
 	if (!_initialised) {
@@ -307,7 +326,9 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 		ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_RANGE_FINDER

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 {
 	if (!_initialised) {
@@ -342,8 +363,9 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow)
 		ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_OPTICAL_FLOW

-// set attitude and position data derived from an external vision system
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 {
 	if (!_initialised) {
@@ -380,7 +402,9 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata)
 		ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_AUXVEL)
 void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 {
 	if (!_initialised) {
@@ -415,6 +439,7 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample)
 		ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us);
 	}
 }
+#endif // CONFIG_EKF2_AUXVEL

 void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 {
@@ -452,6 +477,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags)
 	}
 }

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 void EstimatorInterface::setDragData(const imuSample &imu)
 {
 	// down-sample the drag specific force data by accumulating and calculating the mean when
@@ -502,45 +528,59 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 		}
 	}
 }
+#endif // CONFIG_EKF2_DRAG_FUSION

 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
 	// find the maximum time delay the buffers are required to handle

 	// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
-	float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms);
+	float max_time_delay_ms = _params.sensor_interval_max_ms;
+
+	// aux vel
+#if defined(CONFIG_EKF2_AUXVEL)
+	max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms);
+#endif // CONFIG_EKF2_AUXVEL

 	// using baro
 	if (_params.baro_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
 	}

+#if defined(CONFIG_EKF2_AIRSPEED)
 	// using airspeed
 	if (_params.arsp_thr > FLT_EPSILON) {
 		max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_AIRSPEED

 	// mag mode
 	if (_params.mag_fusion_type != MagFuseType::NONE) {
 		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
 	}

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	// using range finder
 	if ((_params.rng_ctrl != RngCtrl::DISABLED)) {
 		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER

 	if (_params.gnss_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
 	}

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) {
 		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_params.ev_ctrl > 0) {
 		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;

@@ -666,25 +706,35 @@ void EstimatorInterface::print_status()
 		printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
 	}

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	if (_range_buffer) {
 		printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_RANGE_FINDER

+#if defined(CONFIG_EKF2_AIRSPEED)
 	if (_airspeed_buffer) {
 		printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	if (_flow_buffer) {
 		printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW

+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_ext_vision_buffer) {
 		printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_DRAG_FUSION)
 	if (_drag_buffer) {
 		printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
 	}
+#endif // CONFIG_EKF2_DRAG_FUSION

 	_output_predictor.print_status();
 }
@@ -64,11 +64,14 @@
 #include "common.h"
 #include "RingBuffer.h"
 #include "imu_down_sampler.hpp"
-#include "range_finder_consistency_check.hpp"
-#include "sensor_range_finder.hpp"
 #include "utils.hpp"
 #include "output_predictor.h"

+#if defined(CONFIG_EKF2_RANGE_FINDER)
+# include "range_finder_consistency_check.hpp"
+# include "sensor_range_finder.hpp"
+#endif // CONFIG_EKF2_RANGE_FINDER
+
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
 #include <mathlib/mathlib.h>
@@ -90,17 +93,43 @@ public:

 	void setBaroData(const baroSample &baro_sample);

+#if defined(CONFIG_EKF2_AIRSPEED)
 	void setAirspeedData(const airspeedSample &airspeed_sample);
+#endif // CONFIG_EKF2_AIRSPEED

+#if defined(CONFIG_EKF2_RANGE_FINDER)
 	void setRangeData(const rangeSample &range_sample);

+	// set sensor limitations reported by the rangefinder
+	void set_rangefinder_limits(float min_distance, float max_distance)
+	{
+		_range_sensor.setLimits(min_distance, max_distance);
+	}
+
+	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// if optical flow sensor gyro delta angles are not available, set gyro_xyz vector fields to NaN and the EKF will use its internal delta angle data instead
 	void setOpticalFlowData(const flowSample &flow);

+	// set sensor limitations reported by the optical flow sensor
+	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
+	{
+		_flow_max_rate = max_flow_rate;
+		_flow_min_distance = min_distance;
+		_flow_max_distance = max_distance;
+	}
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// set external vision position and attitude data
 	void setExtVisionData(const extVisionSample &evdata);
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_AUXVEL)
 	void setAuxVelData(const auxVelSample &auxvel_sample);
+#endif // CONFIG_EKF2_AUXVEL

 	void setSystemFlagData(const systemFlagUpdate &system_flags);

@@ -147,20 +176,6 @@ public:
 	// set air density used by the multi-rotor specific drag force fusion
 	void set_air_density(float air_density) { _air_density = air_density; }

-	// set sensor limitations reported by the rangefinder
-	void set_rangefinder_limits(float min_distance, float max_distance)
-	{
-		_range_sensor.setLimits(min_distance, max_distance);
-	}
-
-	// set sensor limitations reported by the optical flow sensor
-	void set_optical_flow_limits(float max_flow_rate, float min_distance, float max_distance)
-	{
-		_flow_max_rate = max_flow_rate;
-		_flow_min_distance = min_distance;
-		_flow_max_distance = max_distance;
-	}
-
 	// the flags considered are opt_flow, gps, ev_vel and ev_pos
 	bool isOnlyActiveSourceOfHorizontalAiding(bool aiding_flag) const;

@@ -242,7 +257,6 @@ public:
 	const uint64_t &time_delayed_us() const { return _time_delayed_us; }

 	const gpsSample &get_gps_sample_delayed() const { return _gps_sample_delayed; }
-	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }

 	const bool &global_origin_valid() const { return _NED_origin_initialised; }
 	const MapProjection &global_origin() const { return _pos_ref; }
@@ -290,20 +304,36 @@ protected:

 	// measurement samples capturing measurements on the delayed time horizon
 	gpsSample _gps_sample_delayed{};
-	sensor::SensorRangeFinder _range_sensor{};
+
+
+#if defined(CONFIG_EKF2_AIRSPEED)
 	airspeedSample _airspeed_sample_delayed{};
-	flowSample _flow_sample_delayed{};
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	extVisionSample _ev_sample_prev{};
-	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
+#endif // CONFIG_EKF2_EXTERNAL_VISION

+#if defined(CONFIG_EKF2_RANGE_FINDER)
+	RingBuffer<rangeSample> *_range_buffer{nullptr};
+	uint64_t _time_last_range_buffer_push{0};
+
+	sensor::SensorRangeFinder _range_sensor{};
 	RangeFinderConsistencyCheck _rng_consistency_check;
+#endif // CONFIG_EKF2_RANGE_FINDER

-	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
+
+	flowSample _flow_sample_delayed{};

 	// Sensor limitations
 	float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s)
 	float _flow_min_distance{0.0f};	///< minimum distance that the optical flow sensor can operate at (m)
 	float _flow_max_distance{10.f};	///< maximum distance that the optical flow sensor can operate at (m)
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};		// air density (kg/m**3)

 	bool _imu_updated{false};      // true if the ekf should update (completed downsampling process)
 	bool _initialised{false};      // true if the ekf interface instance (data buffering) is initialized
@@ -314,13 +344,19 @@ protected:
 	MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin
 	MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message
 	float _gps_alt_prev{0.0f};	// height from the previous GPS message (m)
+#if defined(CONFIG_EKF2_GNSS_YAW)
 	float _gps_yaw_offset{0.0f};	// Yaw offset angle for dual GPS antennas used for yaw estimation (radians).
-
 	// innovation consistency check monitoring ratios
 	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
+	uint64_t _time_last_gps_yaw_buffer_push{0};
+#endif // CONFIG_EKF2_GNSS_YAW

-	float _hagl_test_ratio{};		// height above terrain measurement innovation consistency check ratio
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	RingBuffer<dragSample> *_drag_buffer{nullptr};
+	dragSample _drag_down_sampled{};	// down sampled drag specific force data (filter prediction rate -> observation rate)
 	Vector2f _drag_test_ratio{};		// drag innovation consistency check ratio
+#endif // CONFIG_EKF2_DRAG_FUSION
+
 	innovation_fault_status_u _innov_check_fail_status{};

 	bool _horizontal_deadreckon_time_exceeded{true};
@@ -341,20 +377,23 @@ protected:
 	RingBuffer<gpsSample> *_gps_buffer{nullptr};
 	RingBuffer<magSample> *_mag_buffer{nullptr};
 	RingBuffer<baroSample> *_baro_buffer{nullptr};
-	RingBuffer<rangeSample> *_range_buffer{nullptr};
+
+#if defined(CONFIG_EKF2_AIRSPEED)
 	RingBuffer<airspeedSample> *_airspeed_buffer{nullptr};
-	RingBuffer<flowSample> 	*_flow_buffer{nullptr};
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	RingBuffer<extVisionSample> *_ext_vision_buffer{nullptr};
-	RingBuffer<dragSample> *_drag_buffer{nullptr};
+	uint64_t _time_last_ext_vision_buffer_push{0};
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+#if defined(CONFIG_EKF2_AUXVEL)
 	RingBuffer<auxVelSample> *_auxvel_buffer{nullptr};
+#endif // CONFIG_EKF2_AUXVEL
 	RingBuffer<systemFlagUpdate> *_system_flag_buffer{nullptr};

 	uint64_t _time_last_gps_buffer_push{0};
-	uint64_t _time_last_gps_yaw_buffer_push{0};
 	uint64_t _time_last_mag_buffer_push{0};
 	uint64_t _time_last_baro_buffer_push{0};
-	uint64_t _time_last_range_buffer_push{0};
-	uint64_t _time_last_ext_vision_buffer_push{0};

 	uint64_t _time_last_gnd_effect_on{0};

@@ -380,16 +419,18 @@ protected:

 private:

-	inline void setDragData(const imuSample &imu);
+#if defined(CONFIG_EKF2_DRAG_FUSION)
+	void setDragData(const imuSample &imu);
+
+	// Used by the multi-rotor specific drag force fusion
+	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
+	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
+#endif // CONFIG_EKF2_DRAG_FUSION

 	void printBufferAllocationFailed(const char *buffer_name);

 	ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};

 	unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
-
-	// Used by the multi-rotor specific drag force fusion
-	uint8_t _drag_sample_count{0};	// number of drag specific force samples assumulated at the filter prediction rate
-	float _drag_sample_time_dt{0.0f};	// time integral across all samples used to form _drag_down_sampled (sec)
 };
 #endif // !EKF_ESTIMATOR_INTERFACE_H
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Thomas Debrunner	6f1fd4b6e6	uORB: Add subscription with single field selection	2023-03-27 09:44:20 +02:00
Junwoo Hwang	9c001f2e40	Fix SITL Test failure: Place PX4 instance runner after Gazebo server runner (#21230 ) * MAVSDK_Test_Runner: Place PX4 instance runner after Gazebo server runner - This was a nasty bug where starting PX4 instance first, then starting Gazebo server was causing PX4 instance' EKF to freak out, probably because it doesn't like getting data a while after it is started - Detailed breakdown is given here: https://github.com/PX4/PX4-Autopilot/issues/21229#issuecomment-1450761542 - This now guarantees that such edge case won't occur and MAVSDK test will run as it should * MAVSDK Test Runner: Retain comment within 79 character limit - To pass flake8 python style check * Update test/mavsdk_tests/mavsdk_test_runner.py Co-authored-by: Julian Oes <julian@oes.ch> --------- Co-authored-by: Julian Oes <julian@oes.ch>	2023-03-26 11:27:01 +09:00
PX4 BuildBot	58a5aa26a0	Update submodule mavlink to latest Sat Mar 25 00:39:09 UTC 2023 - mavlink in PX4/Firmware (73104cab2e49118903cf32bdf11f6ea917fa394e): https://github.com/mavlink/mavlink/commit/2bdcab78b53d1e349079b43c9d726036abe0617c - mavlink current upstream: https://github.com/mavlink/mavlink/commit/e1e68da5e6f2e44c3126cafc0780f6fee2c2b4f2 - Changes: https://github.com/mavlink/mavlink/compare/2bdcab78b53d1e349079b43c9d726036abe0617c...e1e68da5e6f2e44c3126cafc0780f6fee2c2b4f2 e1e68da5 2023-03-08 Hamish Willee - Add quickstart to the README (#1960) 9b92bf2e 2023-03-08 Hamish Willee - Fix enum value for payload battery (#1959) 81e9fcd5 2023-03-08 Hamish Willee - Allocate range for ras-a and indicate next free range (#1957) abbda253 2023-03-02 Hamish Willee - Indicate preference for COMMAND_INT (#1955) d3a8e4b8 2023-02-08 Julian Oes - development: add command to configure SiK radio	2023-03-24 21:43:59 -04:00
PX4 BuildBot	8306fb96ea	boards: update all NuttX defconfigs	2023-03-24 10:03:14 -04:00
PX4 BuildBot	efab9be488	update all px4board kconfig	2023-03-24 10:01:58 -04:00
Daniel Agar	4d9369dc67	Update world_magnetic_model to latest Fri Mar 24 11:14:06 UTC 2023	2023-03-24 09:39:26 -04:00
Silvan Fuhrer	e8a8e7c677	VTOL: increase deafult of VT_QC_T_ALT_LOSS from 10 to 20m Testing has shown that 10m is a bit too tight, most of all as this check is also run in not height-controlled flight modes (eg Stabilized), and there 10m altitude loss during transitions can happen easily. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-24 13:33:52 +01:00
Silvan Fuhrer	e58d33284a	vtol: correct/improve quad-chute param description Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-24 13:33:52 +01:00
Matthias Grob	937d27f8ee	Helicopter: add offset for yaw compensation based on collective pitch	2023-03-24 07:16:36 +01:00
Beniamino Pozzan	eeb90ac70a	gazebo-classic/sitl_mutliple_run.sh: start spawning drones from index 1 (#21183 ) Fix microdds key session collision when drones with index 0 and 1 are used together Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-24 05:58:26 +01:00
Antonio Sanjurjo C	1ea993880c	Fix typo in WORK_ITEM_TYPE (#21311 ) Signed-off-by: Antonio Sanjurjo C <74329840+antonio-sc66@users.noreply.github.com>	2023-03-24 05:49:14 +01:00
Julian Oes	09a0089c80	README: add Cube Orange+ (#21361 ) This adds the Cube Orange+ to the list, and also changes the Hex naming to CubePilot as that is how it is sold/marketed now. Signed-off-by: Julian Oes <julian@oes.ch>	2023-03-24 05:21:43 +01:00
Daniel Agar	cc2b367c0b	drivers/uavcannode: fix typo (hydrometer->hygrometer)	2023-03-23 22:06:55 -04:00
Silvan Fuhrer	5b6d1e9290	TECS: change default for FW_T_I_GAIN_THR from 0.3 to more appropriate 0.1 Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-23 15:00:36 +01:00
Silvan Fuhrer	ac6c9c857a	Gimbal: remove shutter and retraction handling Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-23 06:59:02 +01:00
Frederic Taillandier	18898f1876	updating sitl-gazebo classic (#21352 ) Co-authored-by: root <root@DESKTOP-OINCOKE>	2023-03-22 14:04:02 +01:00
Silvan Fuhrer	d532578ecc	EKF2: only check for EKF2_BETA_FUSION when corresponding kconfig option is set Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	98d07ad1f3	disable side slip fusion for tailsitters The attitude frame is wrong for tailsitters doing side slip fusion for wind estimation. It doesn't take into account that the frames is 90deg tilted in FW flight. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	07531d29b7	FailureDetector: fix attitude check for tailsitter -set roll/pitch used for failure detection during transition to 0 -rotate estimated attitude 90° in FW flight Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
Silvan Fuhrer	edc570adff	AirspeedSelector: fix attitude rotation for tailsitter Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-22 10:05:32 +01:00
bresch	6bee0893de	add circle intersection unit tests	2023-03-22 10:03:48 +01:00
bresch	af345c88e9	TrajMath: use fmaxf instead of max	2023-03-22 10:03:48 +01:00
bresch	c7bddda1db	MC auto: add maximum RC assist distance during landing	2023-03-22 10:03:48 +01:00
bresch	d6fa42fefd	mc att: add unit tests for AttitudeControlMath helper	2023-03-22 10:01:27 +01:00
bresch	edaf5bf0cd	mc att: extract tilt yaw error compensation to lib	2023-03-22 10:01:27 +01:00
bresch	f65daf7259	mc att: do not pass by euler angles to generate att sp from sticks	2023-03-22 10:01:27 +01:00
bresch	3e3307c0d0	ekf2: fix mag declination innovation angle wrapping	2023-03-21 21:07:28 -04:00
David Sidrane	64e90b91aa	px4_fmu-v6c:Add Mini & fix Rev 1 (#21226 )	2023-03-21 09:16:35 -07:00
Daniel Agar	32a5bd32ad	ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)	2023-03-21 11:25:34 -04:00
Daniel Agar	0784901a66	ekf2: add kconfig option to enable/disable optical flow fusion	2023-03-21 11:25:34 -04:00
Matthias Grob	9f8fa99d70	matrix: use stack allocation for debug output string	2023-03-21 15:01:26 +01:00
Matthias Grob	b1435c6e34	matrix: use output stream function template to enable proper automatic output in gtest unit tests that compare two `Matrix`, `Vector` or two `Dual` objects. Credits to @jwidauer for showing me this trick.	2023-03-21 15:01:26 +01:00
Matthias Grob	b0189d95af	Use new Vector4 class	2023-03-21 15:01:26 +01:00
Matthias Grob	08d6465ce5	matrix: add explicit Vector4 type	2023-03-21 15:01:26 +01:00
Matthias Grob	da0e40c72b	googletest: switch to latest version 1.12.1 -> 1.13	2023-03-21 15:01:26 +01:00
bresch	91364ba901	mc land detect: use z_deriv and vz for vert movement check z_deriv is less prone to bias errors	2023-03-21 09:22:58 -04:00
Beat Küng	22c94f805a	component information: add translation support	2023-03-20 21:57:25 -04:00
Jukka Laitinen	631046b962	src/drivers/sw_crypto: Correct RSA lengths in the example sw crypto backend driver Maximum plaintext length was wrong, it is just k - 2 * hLen -2, where k is RSA key modulus length and hLen is hash length Also minimum block that can be encrypted didn't make sense for RSA-OAEP, it is just 1 byte, the rest will be padded. Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-03-20 21:54:40 -04:00
Jukka Laitinen	b26669b085	Add crypto_deinit function for crypto_backend module Bootloader needs to have a mechanism to de-initialize crypto, in case some HW accelerator is being used. This adds the needed function for it Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2023-03-20 21:54:40 -04:00
Beat Küng	8497d3388f	fix mavlink_ulog: use hrt_abstime instead of float The float inaccuracy was leading to problems on SITL with large timestamps.	2023-03-20 21:47:41 -04:00
Daniel Agar	1524bd39b5	.gitignore: ignore generate top level log/ folder (colcon)	2023-03-20 11:06:12 -04:00
Daniel Agar	4363b09421	ekf2: add kconfig option to enable/disable external vision fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	d47f96f1a5	ekf2: add kconfig option to enable/disable AUX velocity fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	4270a303ab	ekf2: add kconfig option to enable/disable airspeed and sideslip fusion	2023-03-20 10:12:17 -04:00
Daniel Agar	98ff1afc19	ekf2: add kconfig option to enable/disable GNSS yaw	2023-03-20 10:12:17 -04:00
Daniel Agar	8b2205810b	ekf2: add kconfig option to enable/disable baro compensation	2023-03-20 10:12:17 -04:00
Daniel Agar	fe0e3acf09	ekf2: add kconfig option to enable/disable drag fusion	2023-03-20 10:12:17 -04:00
frederic@auterion.com	9a56b0a70d	refactoring jinja-gen call in a cleaner way	2023-03-20 14:50:56 +01:00
numan	ed174d54e9	fix typo in LeddarOne.cpp added missing "~"	2023-03-20 07:51:00 +01:00
alexklimaj	e2de62e38b	GPS only warn if jamming state is critical	2023-03-17 21:38:28 -04:00
Silvan Fuhrer	0afda910d1	VTOL: make sure that all virtual topics are updated Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-17 19:43:01 +01:00
Andrew Brahim	71c746b9bf	drivers/distance_sensor/lightware_sf45_serial: fix path to crc lib (no longer in systemlib) Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2023-03-17 09:35:46 -04:00