Log camera topics

Eliminate intermediate variable
Add fw state for path following
2026-08-07 12:40:34 +08:00 · 2023-03-30 19:05:50 +02:00 · 2023-03-28 18:48:26 +02:00 · 2023-03-28 17:13:07 +02:00 · 2023-03-27 10:28:43 -07:00 · 2023-03-27 10:28:43 -07:00
167 changed files with 7756 additions and 6257 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/
@@ -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
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -25,7 +25,6 @@ param set-default FW_W_EN 1

 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -27,7 +27,6 @@ param set-default FW_W_EN 1
 param set-default MIS_LTRMIN_ALT 30
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_DIST_1WP 2500
-param set-default MIS_DIST_WPS 10000

 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2
@@ -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
@@ -277,7 +277,7 @@ then
 	# Override namespace if environment variable is defined
 	microdds_ns="-n $PX4_MICRODDS_NS"
 fi
-microdds_client start -t udp -p 8888 $microdds_ns
+microdds_client start -t udp -h 127.0.0.1 -p 8888 $microdds_ns

 if param greater -s MNT_MODE_IN -1
 then
@@ -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
@@ -79,7 +79,6 @@ param set-default MC_YAWRATE_MAX 20
 param set-default MC_AIRMODE 1

 param set-default MIS_DIST_1WP 100
-param set-default MIS_DIST_WPS 100000
 param set-default MIS_TAKEOFF_ALT 15

 param set-default MPC_XY_P 0.8
@@ -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
@@ -46,7 +46,6 @@ param set-default RTL_LAND_DELAY -1
 #
 param set-default NAV_ACC_RAD 10

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TAKEOFF_ALT 25
 param set-default MIS_TKO_LAND_REQ 2

@@ -23,7 +23,6 @@ param set-default EKF2_FUSE_BETA 1

 param set-default HTE_VXY_THR 2.0

-param set-default MIS_DIST_WPS 5000
 param set-default MIS_TKO_LAND_REQ 2

 param set-default MPC_ACC_HOR_MAX 2
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+"""
+Converts IP address given in decimal dot notation to int32 to be used in XRCE_DDS_0_CFG parameter
+and vice-versa
+
+@author: Beniamino Pozzan (beniamino.pozzan@phd.unipd.it)
+"""
+
+
+import argparse
+
+parser = argparse.ArgumentParser(
+    prog = 'convert_ip',
+    description = 'converts IP to int32 and viceversa'
+    )
+parser.add_argument('input',
+                    type=str,
+                    help='IP address to convert')
+parser.add_argument('-r','--reverse',
+                    action='store_true',
+                    help='converts from int32 to dot decimal')
+
+args = parser.parse_args()
+
+if( args.reverse == False ):
+
+    try:
+        ip_parts = [int(x) for x in args.input.split('.')]
+    except:
+        raise ValueError("Not a valid IP")
+    if( len(ip_parts)!=4 ):
+        raise ValueError("Not a valid IP")
+    if( not all(x>=0 and x<255 for x in ip_parts) ):
+        raise ValueError("Not a valid IP")
+
+    ip = (ip_parts[0]<<24) + (ip_parts[1]<<16) + (ip_parts[2]<<8) + ip_parts[3]
+
+    if(ip & 0x80000000):
+        ip = -0x100000000 + ip
+
+    print(ip)
+
+else:
+    try:
+        ip = int(args.input)
+    except:
+        raise ValueError("Not a valid IP")
+    if(ip < 0):
+        ip = ip + 0x100000000
+    print('{}.{}.{}.{}'.format(ip>>24, (ip>>16)&0xff, (ip>>8)&0xff, ip&0xff))
@@ -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
@@ -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);
 			}
@@ -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;
@@ -70,27 +70,28 @@ CameraFeedback::Run()

 	camera_trigger_s trig{};

-	while (_trigger_sub.update(&trig)) {
+	vehicle_attitude_s att{};

+	while (_att_sub.update(&att)) {
 		// update geotagging subscriptions
 		vehicle_global_position_s gpos{};
 		_gpos_sub.copy(&gpos);

-		vehicle_attitude_s att{};
 		_att_sub.copy(&att);
+		_trigger_sub.copy(&trig);

-		if (trig.timestamp == 0 ||
-		    gpos.timestamp == 0 ||
-		    att.timestamp == 0) {
+		// if (trig.timestamp == 0 ||
+		//     gpos.timestamp == 0 ||
+		//     att.timestamp == 0) {

-			// reject until we have valid data
-			continue;
-		}
+		// 	// reject until we have valid data
+		// 	continue;
+		// }

-		if ((_cam_cap_fback >= 1) && !trig.feedback) {
-			// Ignore triggers that are not feedback when camera capture feedback is enabled
-			continue;
-		}
+		// if ((_cam_cap_fback >= 1) && !trig.feedback) {
+		// 	// Ignore triggers that are not feedback when camera capture feedback is enabled
+		// 	continue;
+		// }

 		camera_capture_s capture{};

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

--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
JaeyoungLim	21b4052d82	Log camera topics	2023-03-30 19:05:50 +02:00
Jaeyoung Lim	8296d02b1e	Eliminate intermediate variable	2023-03-28 18:48:26 +02:00
Jaeyoung Lim	e3db31f5ab	Add fw state for path following	2023-03-28 17:13:07 +02:00
Ramon Roche	319f3db5e1	tools: ip converter remove verbose output	2023-03-27 10:28:43 -07:00
Beniamino Pozzan	51dd6b783c	microdds_client: Mark parameter as used only if truly used Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-27 10:28:43 -07:00
Beniamino Pozzan	c7f67a3328	microdds_client: remove multi-instances supports, add agent IP as PX4 parameter - multi-instances support is removed from the parameter definitions. - XRCE_DDS_AG_IP allows to define the agent IP when udp transport is used. The parameter is used by default if the module is started without the -h argument. - XRCE_DDS_PRT allows to define the agent listning udp port when the udp transport is uded. The parameter is used by default if the module is started without the -p argument. - Tools/convert_ip.py assists in converting ips in decimal dot notation into int32 notation. Signed-off-by: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>	2023-03-27 10:28:43 -07:00
Daniel Mesham	6fddbea3e4	microdds_client: fix check for length of participant XML (#21374 )	2023-03-27 10:03:09 -07:00
Silvan Fuhrer	ba17a137e1	Navigator: MissionFeasibiltyCheck first WP: check only to current vehicle position Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	465ddddbfd	Navigator: take min of vehicle position or Home distance for first WP check To prevent mission rejections when vehicle is currently far away from Mission start, but planned home is close to it. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	d8ed35c422	Navigator: take vehicle position for first-WP-distance-check And only use Home position if current vehicle position is unknown. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +02:00
Silvan Fuhrer	4d2a31afe3	Navigator: remove max distance between WP mission feasibility check (MIS_DIST_WPS) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-03-27 17:43:33 +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