mavlink: return unsupported frame result when that's the case

mavlink: spacing, naming of command frame handling
Update MAVLink to contain _INT frame deprecation warning
2026-05-25 21:07:36 +08:00 · 2024-05-02 17:32:37 +02:00 · 2024-05-02 17:30:59 +02:00 · 2024-05-02 17:19:31 +02:00 · 2024-05-02 16:49:37 +02:00 · 2024-05-02 16:49:37 +02:00
134 changed files with 1290 additions and 2556 deletions
@@ -1,7 +1,7 @@
 root = true

 [*]
-insert_final_newline = true
+insert_final_newline = false

 [{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
 indent_style = tab
@@ -72,6 +72,8 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL)
 	px4_add_romfs_files(
 		rc.rover_apps
 		rc.rover_defaults
+
+		rc.boat_defaults # hack
 	)
 endif()

@@ -82,13 +84,6 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
 	)
 endif()

-if(CONFIG_MODULES_BOAT)
-	px4_add_romfs_files(
-		rc.boat_apps
-		rc.boat_defaults
-	)
-endif()
-
 if(CONFIG_MODULES_UUV_ATT_CONTROL)
 	px4_add_romfs_files(
 		rc.uuv_apps
@@ -1,59 +0,0 @@
-#!/bin/sh
-#
-# @name RC Boat
-#
-# @type Boat
-# @class Boat
-#
-# @output Servo1 Steering
-# @output Motor1 Throttle
-
-. ${R}etc/init.d/rc.boat_defaults
-
-# Failsafe
-param set-default NAV_RCL_ACT 6 # Disarm on stick input loss
-param set-default NAV_DLL_ACT 6 # Disarm on ground station loss
-param set-default COM_DL_LOSS_T 5 # timeout for ground station connection
-param set-default COM_RC_LOSS_T 10 # timeout for stick input
-param set-default COM_RCL_EXCEPT 1 # Continue mission without stick input
-
-param set-default COM_ARM_WO_GPS 1 # Disable GPS prearm check
-param set-default COM_DISARM_LAND -1 # Don't disarm automatically
-param set-default COM_DISARM_PRFLT -1
-
-# Steering PWM output
-param set-default PWM_AUX_FUNC1 201
-param set-default PWM_AUX_DIS1 1500
-param set-default PWM_AUX_MIN1 1000
-param set-default PWM_AUX_MAX1 2000
-# Engine throttle PWM output
-param set-default PWM_AUX_FUNC2 101
-param set-default PWM_AUX_DIS2 900
-param set-default PWM_AUX_MIN2 940 # Engine doesn't rev below that
-param set-default PWM_AUX_MAX2 2000 # Maximum trhottle at which it's not so easy to flip the boat
-
-param set-default CA_AIRFRAME 5
-
-# Estimation
-param set-default EKF2_MULTI_IMU 0 # Disable multi-IMU
-param set-default SENS_IMU_MODE 1
-param set-default CAL_MAG0_PRIO 0 # Disable internal Magnetometer
-param set-default MBE_ENABLE 0 # Disable online mag calibration when disarmed
-param set-default EKF2_GPS_CTRL 15 # Enable GPS heading fusion
-param set-default SENS_GPS_MASK 0 # Disable multi-GPS blending
-
-# Estimation workarounds, should be double-checked if necessary
-param set-default EKF2_GBIAS_INIT 0.01
-param set-default EKF2_SYNT_MAG_Z 1 # Why? Who said magnetometer Z is not usable?
-
-param set-default EKF2_GPS_V_NOISE 0.751 # Lower certainty for GPS
-param set-default EKF2_GPS_POS_X 2.5 # GPS position, accurate?
-param set-default EKF2_REQ_EPV 10 # Accept GPS easier
-param set-default EKF2_REQ_HDRIFT 1
-param set-default EKF2_REQ_SACC 2
-param set-default EKF2_REQ_VDRIFT 20
-
-param set-default EKF2_MAG_CHK_STR 0.5 # Accept mag easier
-param set-default COM_ARM_MAG_STR 0 # Disable magnetic field strength prearm check
-param set-default COM_ARM_MAG_ANG 360 # Disable internal/external magnetometer consistency check
-param set-default EKF2_ABL_LIM 1 # Higher accelerometer bias limit, was sometimes above the threshold, GPS reporting 0 altitude issue?
@@ -157,9 +157,3 @@ if(CONFIG_MODULES_UUV_ATT_CONTROL)
 		60002_uuv_bluerov2_heavy
 	)
 endif()
-
-if(CONFIG_MODULES_BOAT)
-	px4_add_romfs_files(
-		70001_boat
-	)
-endif()
@@ -1,11 +0,0 @@
-#!/bin/sh
-# Standard apps for a differential drive rover.
-
-# Start the attitude and position estimator.
-ekf2 start &
-
-# Start rover differential drive controller.
-boat start
-
-# Start Land Detector.
-land_detector start rover
@@ -5,7 +5,7 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #

-set VEHICLE_TYPE boat
+set VEHICLE_TYPE rover

 # MAV_TYPE_SURFACE_BOAT 11
 param set-default MAV_TYPE 11
@@ -32,15 +32,6 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi

-#
-# Boat setup.
-#
-if [ $VEHICLE_TYPE = boat ]
-then
-	# Start standard Boat apps.
-	. ${R}etc/init.d/rc.boat_apps
-fi
-
 #
 # Differential Rover setup.
 #
@@ -6,7 +6,7 @@ function check_git_submodule {
 if [[ -f $1"/.git" || -d $1"/.git" ]]; then

 	# always update within CI environment or configuring withing VSCode CMake where you can't interact
-	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ] || [ -n "${CLION_IDE+set}" ]; then
+	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then
 		git submodule --quiet sync --recursive -- $1
 		git submodule --quiet update --init --recursive --jobs=8 -- $1  || true
 		git submodule --quiet sync --recursive -- $1
@@ -14,7 +14,6 @@ CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_COMMON_HYGROMETERS=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y
-CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IRLOCK=y
@@ -1,13 +1,13 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
+CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
-CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
@@ -8,7 +8,4 @@ icm42688p -R 0 -s start

 bmp388 -I -b 1 start

-if ! iis2mdc -R 4 -I -b 1 start
-then
-	bmm150 -I -b 1 start
-fi
+bmm150 -I -b 1 start
@@ -9,7 +9,7 @@ When running PX4 directly on the QRB5165 SoC it runs partially on the Sensor Low
 The portion running on the DSP hosts the flight critical portions of PX4 such as
 the IMU, barometer, magnetometer, GPS, ESC, and power management drivers, and the
 state estimation. The DSP acts as the real time portion of the system. Non flight
-critical applications such as Mavlink, and logging are running on the
+critical applications such as Mavlink, logging, and commander are running on the
 ARM CPU cluster (aka apps proc). The DSP and ARM CPU cluster communicate via a
 Qualcomm proprietary shared memory interface.

@@ -27,7 +27,6 @@ The full instructions are available here:
 ```
 px4$ boards/modalai/voxl2/scripts/run-docker.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
-root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-deps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
 root@9373fa1401b8:/usr/local/workspace# exit
@@ -70,15 +69,16 @@ pxh>
 ## Notes

 You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have
-to power cycle the board and restart everything. Starting with SDK 1.3.0 it is possible
-to cleanly shutdown PX4 on VOXL 2.
+to power cycle the board and restart everything.

 ## Tips

-Always use the latest SDK release
+Start with a VOXL 2 that only has the system image installed, not the SDK
+
+Run the command ```voxl-px4 -s``` on target to run the self-test

 In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK
-can be used (Most messages are passed to the apps proc but certain low level messages are not):
+can be used:
 ```
 modalai@modalai-XPS-15-9570:/local/mnt/workspace/Qualcomm/Hexagon_SDK/4.1.0.4/tools/debug/mini-dm/Ubuntu18$ sudo ./mini-dm
 [sudo] password for modalai:
@@ -20,6 +20,7 @@ adb shell chmod a+x /usr/bin/voxl-px4-hitl-start

 # Push configuration file
 adb shell mkdir -p /etc/modalai
+adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai

@@ -129,6 +129,12 @@ else
    DAEMON="-d"
 fi

+if [ ! -f /data/px4/param/parameters ]; then
+   echo "[INFO] Setting default parameters for PX4 on voxl"
+   px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config
+   /bin/sync
+fi
+
 if [ $SENSOR_CAL == "FAKE" ]; then
   /bin/echo "[INFO] Setting up fake sensor calibration values"
   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
@@ -0,0 +1,192 @@
+#!/bin/sh
+# PX4 commands need the 'px4-' prefix in bash.
+# (px4-alias.sh is expected to be in the PATH)
+. px4-alias.sh
+
+param select /data/px4/param/parameters
+
+# Make sure we are running at 800Hz on IMU
+param set IMU_GYRO_RATEMAX 800
+
+# EKF2 Parameters
+param set EKF2_IMU_POS_X 0.027
+param set EKF2_IMU_POS_Y 0.009
+param set EKF2_IMU_POS_Z -0.019
+param set EKF2_EV_DELAY 5
+param set EKF2_AID_MASK 280
+param set EKF2_ABL_LIM 0.8
+param set EKF2_TAU_POS 0.25
+param set EKF2_TAU_VEL 0.25
+
+param set MC_AIRMODE 0
+
+param set MC_YAW_P 2.0
+param set MC_YAWRATE_P 0.15
+param set MC_YAWRATE_I 0.1
+param set MC_YAWRATE_D 0.0
+param set MC_YAWRATE_K 1.0
+
+param set MC_PITCH_P 5.5
+param set MC_PITCHRATE_P 0.08
+param set MC_PITCHRATE_I 0.2
+param set MC_PITCHRATE_D 0.0013
+param set MC_PITCHRATE_K 1.0
+
+param set MC_ROLL_P 5.5
+param set MC_ROLLRATE_P 0.08
+param set MC_ROLLRATE_I 0.2
+param set MC_ROLLRATE_D 0.0013
+param set MC_ROLLRATE_K 1.0
+
+param set MPC_VELD_LP 5.0
+
+# tweak MPC_THR_MIN to prevent roll/pitch losing control
+# authority under rapid downward acceleration
+param set MPC_THR_MAX 0.75
+param set MPC_THR_MIN 0.08
+param set MPC_THR_HOVER 0.42
+param set MPC_MANTHR_MIN 0.05
+
+# default position mode with a little expo, smooth mode is terrible
+param set MPC_POS_MODE 0
+param set MPC_YAW_EXPO 0.20
+param set MPC_XY_MAN_EXPO 0.20
+param set MPC_Z_MAN_EXPO 0.20
+
+# max velocities
+param set MPC_VEL_MANUAL 5.0
+param set MPC_XY_VEL_MAX 5.0
+param set MPC_XY_CRUISE 5.0
+param set MPC_Z_VEL_MAX_DN 1.5
+param set MPC_Z_VEL_MAX_UP 4.0
+param set MPC_LAND_SPEED 1.0
+
+# Horizontal position PID
+param set MPC_XY_P 0.95
+param set MPC_XY_VEL_P_ACC 3.00
+param set MPC_XY_VEL_I_ACC 0.10
+param set MPC_XY_VEL_D_ACC 0.00
+
+# Vertical position PID
+# PX4 Defaults
+param set MPC_Z_P 1.0
+param set MPC_Z_VEL_P_ACC 8.0
+param set MPC_Z_VEL_I_ACC 2.0
+param set MPC_Z_VEL_D_ACC 0.0
+
+param set MPC_TKO_RAMP_T 1.50
+param set MPC_TKO_SPEED	1.50
+
+# Set the ESC outputs to function as motors
+param set VOXL_ESC_FUNC1 101
+param set VOXL_ESC_FUNC2 103
+param set VOXL_ESC_FUNC3 104
+param set VOXL_ESC_FUNC4 102
+
+param set VOXL_ESC_SDIR1 0
+param set VOXL_ESC_SDIR2 0
+param set VOXL_ESC_SDIR3 0
+param set VOXL_ESC_SDIR4 0
+
+param set VOXL_ESC_CONFIG 1
+param set VOXL_ESC_REV 0
+param set VOXL_ESC_MODE 0
+param set VOXL_ESC_BAUD 2000000
+param set VOXL_ESC_RPM_MAX 10500
+param set VOXL_ESC_RPM_MIN 1000
+
+# Set the Voxl2 IO outputs to function as motors
+param set VOXL2_IO_FUNC1 101
+param set VOXL2_IO_FUNC2 102
+param set VOXL2_IO_FUNC3 103
+param set VOXL2_IO_FUNC4 104
+
+param set VOXL2_IO_BAUD 921600
+param set VOXL2_IO_MIN 1000
+param set VOXL2_IO_MAX 2000
+
+# Some parameters for control allocation
+param set CA_ROTOR_COUNT 4
+param set CA_R_REV 0
+param set CA_AIRFRAME 0
+param set CA_ROTOR_COUNT 4
+param set CA_ROTOR0_PX 0.15
+param set CA_ROTOR0_PY 0.15
+param set CA_ROTOR1_PX -0.15
+param set CA_ROTOR1_PY -0.15
+param set CA_ROTOR2_PX 0.15
+param set CA_ROTOR2_PY -0.15
+param set CA_ROTOR2_KM -0.05
+param set CA_ROTOR3_PX -0.15
+param set CA_ROTOR3_PY 0.15
+param set CA_ROTOR3_KM -0.05
+
+# Some parameter settings to disable / ignore certain preflight checks
+
+# No GPS driver yet so disable it
+param set SYS_HAS_GPS 0
+
+# Allow arming wihtout a magnetometer (Need magnetometer driver)
+param set SYS_HAS_MAG 0
+param set EKF2_MAG_TYPE	5
+
+# Allow arming without battery check (Need voxlpm driver)
+param set CBRK_SUPPLY_CHK 894281
+
+# Allow arming without an SD card
+param set COM_ARM_SDCARD 0
+
+# Allow arming wihtout CPU load information
+param set COM_CPU_MAX 0.0
+
+# Disable auto disarm. This is number of seconds to wait for takeoff
+# after arming. If no takeoff happens then it will disarm. A negative
+# value disables this.
+param set COM_DISARM_PRFLT -1
+
+# This parameter doesn't exist anymore. Need to see what the new method is.
+# param set MAV_BROADCAST 0
+
+# Doesn't work without setting this to Quadcopter
+param set MAV_TYPE 2
+
+# Parameters that we don't use but QGC complains about if they aren't there
+param set SYS_AUTOSTART 4001
+
+# Default RC channel mappings
+param set RC_MAP_ACRO_SW 0
+param set RC_MAP_ARM_SW 0
+param set RC_MAP_AUX1 0
+param set RC_MAP_AUX2 0
+param set RC_MAP_AUX3 0
+param set RC_MAP_AUX4 0
+param set RC_MAP_AUX5 0
+param set RC_MAP_AUX6 0
+param set RC_MAP_FAILSAFE 0
+param set RC_MAP_FLAPS 0
+param set RC_MAP_FLTMODE 6
+param set RC_MAP_GEAR_SW 0
+param set RC_MAP_KILL_SW 7
+param set RC_MAP_LOITER_SW 0
+param set RC_MAP_MAN_SW 0
+param set RC_MAP_MODE_SW 0
+param set RC_MAP_OFFB_SW 0
+param set RC_MAP_PARAM1 0
+param set RC_MAP_PARAM2 0
+param set RC_MAP_PARAM3 0
+param set RC_MAP_PITCH 2
+param set RC_MAP_POSCTL_SW 0
+param set RC_MAP_RATT_SW 0
+param set RC_MAP_RETURN_SW 0
+param set RC_MAP_ROLL 1
+param set RC_MAP_STAB_SW 0
+param set RC_MAP_THROTTLE 3
+param set RC_MAP_TRANS_SW 0
+param set RC_MAP_YAW 4
+
+param save
+
+sleep 2
+
+# Need px4-shutdown otherwise Linux system shutdown is called
+px4-shutdown
@@ -6,5 +6,4 @@
 param set-default BAT1_V_DIV 10.1
 param set-default BAT1_A_PER_V 17

-param set-default GPS_2_CONFIG 202
 param set-default TEL_FRSKY_CONFIG 103
@@ -1,17 +0,0 @@
-CONFIG_DRIVERS_IRLOCK=n
-CONFIG_MODULES_AIRSPEED_SELECTOR=n
-CONFIG_MODULES_BOAT=y
-CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
-CONFIG_MODULES_FW_ATT_CONTROL=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
-CONFIG_MODULES_FW_RATE_CONTROL=n
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
-CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
-CONFIG_MODULES_MC_POS_CONTROL=n
-CONFIG_MODULES_MC_RATE_CONTROL=n
-CONFIG_MODULES_VTOL_ATT_CONTROL=n
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
-# CONFIG_EKF2_WIND is not set
@@ -1,17 +0,0 @@
-CONFIG_DRIVERS_IRLOCK=n
-CONFIG_MODULES_AIRSPEED_SELECTOR=n
-CONFIG_MODULES_BOAT=y
-CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
-CONFIG_MODULES_FW_ATT_CONTROL=n
-CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
-CONFIG_MODULES_FW_RATE_CONTROL=n
-CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
-CONFIG_MODULES_MC_ATT_CONTROL=n
-CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
-CONFIG_MODULES_MC_POS_CONTROL=n
-CONFIG_MODULES_MC_RATE_CONTROL=n
-CONFIG_MODULES_VTOL_ATT_CONTROL=n
-CONFIG_EKF2_AUX_GLOBAL_POSITION=y
-# CONFIG_EKF2_WIND is not set
@@ -66,7 +66,6 @@ CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
-CONFIG_MAVLINK_DIALECT="development"
 CONFIG_MODULES_MC_ATT_CONTROL=y
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
@@ -33,7 +33,6 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
-CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
@@ -85,7 +84,6 @@ CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
-CONFIG_SYSTEMCMDS_I2C_LAUNCHER=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_IO_BYPASS_CONTROL=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -12,11 +12,9 @@ param set-default MAV_2_RATE 100000
 param set-default MAV_2_REMOTE_PRT 14550
 param set-default MAV_2_UDP_PRT 14550

-# By disabling all 3 INA modules, we use the
-# i2c_launcher instead.
 param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
-param set-default SENS_EN_INA226 0
+param set-default SENS_EN_INA226 1

 safety_button start

@@ -17,7 +17,6 @@
 #------------------------------------------------------------------------------

 set HAVE_PM2 yes
-set INA_CONFIGURED no

 if mft query -q -k MFT -s MFT_PM2 -v 0
 then
@@ -40,8 +39,6 @@ then
 	then
 		ina226 -X -b 2 -t 2 -k start
 	fi
-
-	set INA_CONFIGURED yes
 fi

 if param compare SENS_EN_INA228 1
@@ -52,8 +49,6 @@ then
 	then
 		ina228 -X -b 2 -t 2 -k start
 	fi
-
-	set INA_CONFIGURED yes
 fi

 if param compare SENS_EN_INA238 1
@@ -64,25 +59,6 @@ then
 	then
 		ina238 -X -b 2 -t 2 -k start
 	fi
-
-	set INA_CONFIGURED yes
-fi
-
-#Start Auterion Power Module selector for Skynode boards
-if ver hwbasecmp 009 010
-then
-	pm_selector_auterion start
-else
-	if [ $INA_CONFIGURED = no ]
-	then
-		# INA226, INA228, INA238 auto-start
-		i2c_launcher start -b 1
-		if [ $HAVE_PM2 = yes ]
-		then
-			i2c_launcher start -b 2
-		fi
-	fi
-fi
 fi

 # Internal SPI bus ICM42686p (hard-mounted)
@@ -112,5 +88,4 @@ fi

 bmp388 -X -b 2 start

-unset INA_CONFIGURED
 unset HAVE_PM2
@@ -46,10 +46,7 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3643
 CONFIG_CDCACM_VENDORSTR="Dronecode Project, Inc."
-CONFIG_DEBUG_ERROR=y
-CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
-CONFIG_DEBUG_MEMFAULT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEV_FIFO_SIZE=0
@@ -260,6 +257,7 @@ CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=2032
+CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -278,7 +276,6 @@ CONFIG_SYSTEM_CLE=y
 CONFIG_SYSTEM_DHCPC_RENEW=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
-CONFIG_SYSTEM_SYSTEM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
@@ -308,7 +308,7 @@ class MavrosMissionTest(MavrosTestCommon):
        self.assertTrue(res['pitch_error_std'] < 5.0, str(res))

        # TODO: fix by excluding initial heading init and reset preflight
-        self.assertTrue(res['yaw_error_std'] < 13.0, str(res))
+        self.assertTrue(res['yaw_error_std'] < 10.0, str(res))


 if __name__ == '__main__':
@@ -1,10 +0,0 @@
-uint64 timestamp # time since system start (microseconds)
-
-float32 speed # [m/s] collective roll-off speed in body x-axis
-bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward
-float32 yaw_rate # [rad] yaw_rate
-bool closed_loop_yaw_rate_control # true if rudder angle is controlled using feedback, false if direct feed-forward
-float32 heading_setpoint # [rad] desired heading
-bool closed_loop_heading_control # true if heading is controlled using feedback, false if direct feed-forward
-
-# TOPICS boat_setpoint boat_control_output
@@ -53,7 +53,6 @@ set(msg_files
 	ArmingCheckRequest.msg
 	AutotuneAttitudeControlStatus.msg
 	BatteryStatus.msg
-	BoatSetpoint.msg
 	Buffer128.msg
 	ButtonEvent.msg
 	CameraCapture.msg
@@ -165,7 +165,10 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1

-uint8 ORB_QUEUE_LENGTH = 8
+uint8 frame # The coordinate system of the Command
+uint8 FRAME_UNKNOWN = 0 # Not specified
+uint8 FRAME_GLOBAL = 1 # Global WGS84 coordinate frame + altitude relative to mean sea level
+uint8 FRAME_GLOBAL_RELATIVE_ALTITUDE = 2 # Global WGS84 coordinate frame + altitude relative to the home position

 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
 float32 param2			# Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum.
@@ -184,4 +187,6 @@ bool from_external

 uint16 COMPONENT_MODE_EXECUTOR_START = 1000

+uint8 ORB_QUEUE_LENGTH = 8
+
 # TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
@@ -5,13 +5,16 @@
 uint64 timestamp		# time since system start (microseconds)

 # Result cases. This follows the MAVLink MAV_RESULT enum definition
-uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
-uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
-uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
-uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
-uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
-uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
-uint8 VEHICLE_CMD_RESULT_CANCELLED = 6			# Command Canceled
+uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command is valid (is supported and has valid parameters), and was executed
+uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command is valid, but cannot be executed at this time
+uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command is invalid (is supported but has invalid parameters)
+uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command is not supported (unknown)
+uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command is valid, but execution has failed
+uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command is valid and is being executed
+uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command has been cancelled (as a result of receiving a COMMAND_CANCEL message)
+uint8 VEHICLE_CMD_RESULT_COMMAND_LONG_ONLY = 7 # Command is only accepted when sent as a COMMAND_LONG
+uint8 VEHICLE_CMD_RESULT_COMMAND_INT_ONLY = 8 # Command is only accepted when sent as a COMMAND_INT
+uint8 VEHICLE_CMD_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME = 9 # Command is invalid because a frame is required and the specified frame is not supported

 # Arming denied specific cases
 uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0
@@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
 static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
 static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};

-static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50};
+static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};

 static constexpr wq_config_t test1{"wq:test1", 2000, 0};
 static constexpr wq_config_t test2{"wq:test2", 2000, 0};
@@ -728,8 +728,7 @@ GPS::run()
 		int32_t gps_ubx_mode = 0;
 		param_get(handle, &gps_ubx_mode);

-		switch (gps_ubx_mode) {
-		case 1:  // heading
+		if (gps_ubx_mode == 1) { // heading
 			if (_instance == Instance::Main) {
 				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase;

@@ -737,13 +736,10 @@ GPS::run()
 				ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
 			}

-			break;
-
-		case 2:
+		} else if (gps_ubx_mode == 2) {
 			ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
-			break;

-		case 3:
+		} else if (gps_ubx_mode == 3) {
 			if (_instance == Instance::Main) {
 				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1;

@@ -751,18 +747,11 @@ GPS::run()
 				ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
 			}

-			break;
-
-		case 4:
+		} else if (gps_ubx_mode == 4) {
 			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
-			break;

-		case 5:  // rover with static base on Uart2
+		} else if (gps_ubx_mode == 5) { // rover with static base on Uart2
 			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
-			break;
-
-		default:
-			break;

 		}
 	}
@@ -1572,4 +1561,4 @@ int
 gps_main(int argc, char *argv[])
 {
 	return GPS::main(argc, argv);
-}
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2016-2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016 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
@@ -152,9 +152,8 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
 *
 * The offset angle increases clockwise.
 *
- * Set this to 90 if the rover (or Unicore primary, or Septentrio Mosaic Aux)
- * antenna is placed on the right side of the vehicle and the moving base
- * antenna is on the left side.
+ * Set this to 90 if the rover (or Unicore primary) antenna is placed on the
+ * right side of the vehicle and the moving base antenna is on the left side.
 *
 * (Note: the Unicore primary antenna is the one connected on the right as seen
 *        from the top).
@@ -172,9 +172,7 @@ void ADIS16507::RunImpl()
 			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);

 			if (DIAG_STAT != 0) {
-				PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT);
-				_state = STATE::RESET;
-				ScheduleDelayed(3_s);
+				PX4_ERR("DIAG_STAT: %#X", DIAG_STAT);

 			} else {
 				PX4_DEBUG("self test passed");
@@ -832,7 +832,7 @@ Serial bus driver for the VectorNav VN-100, VN-200, VN-300.

 Most boards are configured to enable/start the driver on a specified UART using the SENS_VN_CFG parameter.

-Setup/usage information: https://docs.px4.io/main/en/sensor/vectornav.html
+Setup/usage information: https://docs.px4.io/master/en/sensor/vectornav.html

 ### Examples

@@ -231,8 +231,6 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param

 	AltitudePitchControl control_setpoint;

-	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
-
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);

 	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
@@ -276,7 +274,6 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &

 	AltitudePitchControl control_setpoint;

-	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);

 	if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) {
@@ -323,11 +320,9 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
 	if (flag.airspeed_enabled) {
 		// Calculate limits for the demanded rate of change of speed based on physical performance limits
-		// with a 50% margin to allow the total energy controller to correct for errors. Increase it in case of fast descend
-		const float max_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_max / math::max(input.tas,
-					      FLT_EPSILON);
-		const float min_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_min / math::max(input.tas,
-					      FLT_EPSILON);
+		// with a 50% margin to allow the total energy controller to correct for errors.
+		const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON);
+		const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON);
 		airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp,
 						 max_tas_rate_sp);
 	}
@@ -353,7 +348,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
 			CONSTANTS_ONE_G; // potential energy rate of change
-	specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint *
+	specific_energy_rates.ske_rate.setpoint = input.tas *
 			control_setpoint.tas_rate_setpoint; // kinetic energy rate of change

 	// Calculate specific energy rates in units of (m**2/sec**3)
@@ -399,20 +394,19 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;

-	} else if (param.fast_descend > FLT_EPSILON) {
-		// pitch loop controls the airspeed to max
-		pitch_speed_weight = 1.f + param.fast_descend;
-
 	}

-	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 2.f);
-	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 2.f);
+	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
+	// loop time constant and therefore can lead to a destabilization of that control loop
+	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
+	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f);

 	return weight;
 }

 void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
-				    const Param &param, const Flag &flag)
+				    const Param &param,
+				    const Flag &flag)
 {
 	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
@@ -520,17 +514,10 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
 	const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
 	_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
 	_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
+
 	ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
-	float throttle_setpoint{param.throttle_min};
-
-	if (1.f - param.fast_descend < FLT_EPSILON) {
-		// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
-		throttle_setpoint = param.throttle_min;
-
-	} else {
-		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
-	}
+	_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
+	float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};

 	// Rate limit the throttle demand
 	if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
@@ -664,7 +651,6 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_reference_param.target_sinkrate = target_sinkrate;
 	// Control
 	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
-	_control_param.tas_max = eas_to_tas * _equivalent_airspeed_max;
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
@@ -719,11 +705,6 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);

 	} else {
-		/* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control
-		loop to control the speed to the maximum airspeed. */
-		_setFastDescend(hgt_setpoint, altitude);
-		_control_param.fast_descend = _fast_descend;
-
 		// Update airspeedfilter submodule
 		const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@@ -731,25 +712,15 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);

 		// Update Reference model submodule
-		if (1.f - _fast_descend < FLT_EPSILON) {
-			// Reset the altitude reference model, while we are in fast descend.
-			const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{
-				.alt = altitude,
-				.alt_rate = hgt_rate};
-			_altitude_reference_model.initialize(init_state);
+		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+				.alt_rate = hgt_rate_sp};

-		} else {
-			const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
-					.alt_rate = hgt_rate_sp};
-
-			_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
-		}
+		_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);

 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
-						EAS_setpoint;
+		control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint;

 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -769,17 +740,3 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_update_timestamp = now;
 }

-void TECS::_setFastDescend(const float alt_setpoint, const float alt)
-{
-	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
-	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
-		_fast_descend = 1.f;
-
-	} else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) {
-		// Were in fast descend, scale it down. up until 5m above target altitude
-		_fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f);
-
-	} else {
-		_fast_descend = 0.f;
-	}
-}
@@ -202,7 +202,6 @@ public:
 		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
-		float tas_max;				///< True airspeed demand upper limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
@@ -234,8 +233,6 @@ public:

 		float load_factor_correction;				///< Gain from normal load factor increase to total energy rate demand [m²/s³].
 		float load_factor;					///< Additional normal load factor.
-
-		float fast_descend;
 	};

 	/**
@@ -366,7 +363,6 @@ private:
 	struct AltitudePitchControl {
 		float altitude_rate_setpoint;	///< Controlled altitude rate setpoint [m/s].
 		float tas_rate_setpoint;	///< Controlled true airspeed rate setpoint [m/s²].
-		float tas_setpoint; 		///< Controller true airspeed setpoint [m/s]
 	};

 	/**
@@ -397,7 +393,7 @@ private:
 	 * @brief calculate airspeed control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input	is the current input measurement of the UAS.
+	 * @param input	is the current input measurment of the UAS.
 	 * @param param	is the control parameters.
 	 * @param flag	is the control flags.
 	 * @return controlled airspeed rate setpoint in [m/s²].
@@ -408,7 +404,7 @@ private:
 	 * @brief calculate altitude control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input is the current input measurement of the UAS.
+	 * @param input is the current input measurment of the UAS.
 	 * @param param is the control parameters.
 	 * @return controlled altitude rate setpoint in [m/s].
 	 */
@@ -417,14 +413,14 @@ private:
 	 * @brief Calculate specific energy rates.
 	 *
 	 * @param control_setpoint is the controlles altitude and airspeed rate setpoints.
-	 * @param input is the current input measurement of the UAS.
+	 * @param input is the current input measurment of the UAS.
 	 * @return Specific energy rates in [m²/s³].
 	 */
 	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
 	/**
 	 * @brief Detect underspeed.
 	 *
-	 * @param input is the current input measurement of the UAS.
+	 * @param input is the current input measurment of the UAS.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
@@ -606,11 +602,9 @@ public:
 	void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };

 	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
-	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); };
-	void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; };
+	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };

 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }

 	void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
@@ -671,10 +665,7 @@ private:

 	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.

-	float _equivalent_airspeed_min{10.0f};				///< equivalent airspeed demand lower limit (m/sec)
-	float _equivalent_airspeed_max{20.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _fast_descend_alt_err{-1.f};	 				///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m].
-	float _fast_descend{0.f};					///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude.
+	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)

 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
@@ -706,7 +697,6 @@ private:
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
 		.tas_min = 10.0f,
-		.tas_max = 20.0f,
 		.pitch_max = 0.5f,
 		.pitch_min = -0.5f,
 		.throttle_trim = 0.0f,
@@ -726,20 +716,11 @@ private:
 		.throttle_slewrate = 0.0f,
 		.load_factor_correction = 0.0f,
 		.load_factor = 1.0f,
-		.fast_descend = 0.f
 	};

 	TECSControl::Flag _control_flag{
 		.airspeed_enabled = false,
 		.detect_underspeed_enabled = false,
 	};
-
-	/**
-	 * @brief Set fast descend value
-	 *
-	 * @param alt_setpoint is the altitude setpoint
-	 * @param alt is the current altitude
-	 */
-	void _setFastDescend(float alt_setpoint, float alt);
 };

@@ -54,7 +54,7 @@ def getData(log, topic_name, variable_name, instance=0):
 def us2s(time_ms):
    return time_ms * 1e-6

-def run(logfile, use_gnss, scale_init):
+def run(logfile, use_gnss):
    log = ULog(logfile)

    if use_gnss:
@@ -75,10 +75,7 @@ def run(logfile, use_gnss, scale_init):
    dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom')
    t_dist_bottom = us2s(getData(log, 'vehicle_local_position', 'timestamp'))

-    if scale_init is None:
-        scale_init = 1.0
-
-    state = np.array([0.0, 0.0, scale_init])
+    state = np.array([0.0, 0.0, 1.0])
    P = np.diag([1.0, 1.0, 1e-4])
    wind_nsd = 1e-2
    scale_nsd = 1e-4
@@ -106,12 +103,12 @@ def run(logfile, use_gnss, scale_init):

            P += Q * dt

-            if i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now:
+            if t_true_airspeed[i_airspeed] < t_now:
                while i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now:
                    i_airspeed += 1
                i_airspeed -= 1

-                (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P, true_airspeed[i_airspeed], R, epsilon)
+                (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P.flatten(), true_airspeed[i_airspeed], R, epsilon)
                state += np.array(K) * innov
                P -= K * H * P
                i_airspeed += 1
@@ -148,9 +145,8 @@ if __name__ == '__main__':
    parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str)
    parser.add_argument('--gnss', help='Use GNSS velocity instead of local velocity estimate',
                        action='store_true')
-    parser.add_argument('--scale_init', help='Initial airsped scale factor (1.0 if not specified)', type=float)
    args = parser.parse_args()

    logfile = os.path.abspath(args.logfile) # Convert to absolute path

-    run(logfile, args.gnss, args.scale_init)
+    run(logfile, args.gnss)
@@ -1,189 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "Boat.hpp"
-
-Boat::Boat() :
-	ModuleParams(nullptr),
-	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
-{
-	updateParams();
-}
-
-bool Boat::init()
-{
-	ScheduleOnInterval(10_ms); // 100 Hz
-	return true;
-}
-
-void Boat::updateParams()
-{
-	ModuleParams::updateParams();
-
-	_max_speed = _param_bt_spd_max.get();
-	_boat_guidance.setMaxSpeed(_max_speed);
-	_boat_kinematics.setMaxSpeed(_max_speed);
-
-	_max_angular_velocity = _param_bt_ang_max.get();
-	_boat_guidance.setMaxAngularVelocity(_max_angular_velocity);
-	_boat_kinematics.setMaxAngularVelocity(_max_angular_velocity);
-}
-
-void Boat::Run()
-{
-	if (should_exit()) {
-		ScheduleClear();
-		exit_and_cleanup();
-	}
-
-	hrt_abstime now = hrt_absolute_time();
-	const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f;
-	_time_stamp_last = now;
-
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&parameter_update);
-		updateParams();
-	}
-
-	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode{};
-
-		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_manual_driving = vehicle_control_mode.flag_control_manual_enabled;
-			_mission_driving = vehicle_control_mode.flag_control_auto_enabled;
-		}
-	}
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status{};
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s);
-			_boat_kinematics.setArmed(spooled_up);
-			_acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO);
-		}
-	}
-
-	if (_manual_driving) {
-		// Manual mode
-		// Directly produce setpoints from the manual control setpoint (joystick)
-		if (_manual_control_setpoint_sub.updated()) {
-			manual_control_setpoint_s manual_control_setpoint{};
-
-			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
-				boat_setpoint_s setpoint{};
-				setpoint.speed = ((manual_control_setpoint.throttle + 1.f) * 0.5f) * math::max(0.f, _param_bt_spd_scale.get());
-				setpoint.yaw_rate = manual_control_setpoint.roll * _param_bt_ang_vel_scale.get();
-
-				// if acro mode, we activate the yaw rate control
-				if (_acro_driving) {
-					setpoint.closed_loop_speed_control = false;
-					setpoint.closed_loop_yaw_rate_control = true;
-
-				} else {
-					setpoint.closed_loop_speed_control = false;
-					setpoint.closed_loop_yaw_rate_control = false;
-				}
-
-
-				setpoint.timestamp = now;
-				_boat_setpoint_pub.publish(setpoint);
-			}
-		}
-
-	} else if (_mission_driving) {
-		// Mission mode
-		// Directly receive setpoints from the guidance library
-		_boat_guidance.computeGuidance(
-			_boat_control.getVehicleYaw(),
-			_boat_control.getLocalPosition(),
-			dt
-		);
-	}
-
-	_boat_control.control(dt);
-	_boat_kinematics.allocate();
-}
-
-int Boat::task_spawn(int argc, char *argv[])
-{
-	Boat *instance = new Boat();
-
-	if (instance) {
-		_object.store(instance);
-		_task_id = task_id_is_work_queue;
-
-		if (instance->init()) {
-			return PX4_OK;
-		}
-
-	} else {
-		PX4_ERR("alloc failed");
-	}
-
-	delete instance;
-	_object.store(nullptr);
-	_task_id = -1;
-
-	return PX4_ERROR;
-}
-
-int Boat::custom_command(int argc, char *argv[])
-{
-	return print_usage("unknown command");
-}
-
-int Boat::print_usage(const char *reason)
-{
-	if (reason) {
-		PX4_ERR("%s\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION(
-		R"DESCR_STR(
-### Description
-Boat Drive controller.
-)DESCR_STR");
-
-	PRINT_MODULE_USAGE_NAME("boat", "controller");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-	return 0;
-}
-
-extern "C" __EXPORT int boat_main(int argc, char *argv[])
-{
-	return Boat::main(argc, argv);
-}
@@ -1,109 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include "BoatControl/BoatControl.hpp"
-#include "BoatGuidance/BoatGuidance.hpp"
-#include "BoatKinematics/BoatKinematics.hpp"
-
-using namespace time_literals;
-
-class Boat : public ModuleBase<Boat>, public ModuleParams,
-	public px4::ScheduledWorkItem
-{
-public:
-	Boat();
-	~Boat() override = default;
-
-	/** @see ModuleBase */
-	static int task_spawn(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int custom_command(int argc, char *argv[]);
-
-	/** @see ModuleBase */
-	static int print_usage(const char *reason = nullptr);
-
-	bool init();
-
-protected:
-	void updateParams() override;
-
-private:
-	void Run() override;
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-
-	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
-
-	bool _manual_driving = false;
-	bool _mission_driving = false;
-	bool _acro_driving = false;
-
-
-	BoatGuidance _boat_guidance{this};
-	BoatControl _boat_control{this};
-	BoatKinematics _boat_kinematics{this};
-
-	float _max_speed{0.f};
-	float _max_angular_velocity{0.f};
-
-	hrt_abstime _time_stamp_last{0};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
-		(ParamFloat<px4::params::BT_ANG_VEL_SCALE>) _param_bt_ang_vel_scale,
-		(ParamFloat<px4::params::BT_SPD_SCALE>) _param_bt_spd_scale,
-		(ParamFloat<px4::params::BT_ANG_MAX>) _param_bt_ang_max,
-		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max
-	)
-};
@@ -1,110 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "BoatControl.hpp"
-
-using namespace matrix;
-
-BoatControl::BoatControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f);
-	pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f);
-}
-
-void BoatControl::updateParams()
-{
-	ModuleParams::updateParams();
-
-	pid_set_parameters(&_pid_angular_velocity,
-			   _param_bt_ang_p.get(),
-			   _param_bt_ang_i.get(),
-			   0,
-			   _param_bt_ang_imax.get(),
-			   _param_bt_ang_outlim.get());
-
-	pid_set_parameters(&_pid_speed,
-			   _param_bt_spd_p.get(),
-			   _param_bt_spd_i.get(),
-			   0,
-			   _param_bt_spd_imax.get(),
-			   _param_bt_spd_outlim.get());
-}
-
-void BoatControl::control(float dt)
-{
-	if (_vehicle_angular_velocity_sub.updated()) {
-		vehicle_angular_velocity_s vehicle_angular_velocity{};
-
-		if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) {
-			_vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2];
-		}
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-
-		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
-			_vehicle_attitude_quaternion = Quatf(vehicle_attitude.q);
-			_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-		}
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-
-		if (_vehicle_local_position_sub.copy(&vehicle_local_position)) {
-			_vehicle_local_position = vehicle_local_position;
-			Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-			Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-			_vehicle_forward_speed = velocity_in_body_frame(0);
-		}
-	}
-
-	_boat_setpoint_sub.update(&_boat_setpoint);
-
-	// PID to reach setpoint using control_output
-	boat_setpoint_s boat_control_output = _boat_setpoint;
-
-	if (_boat_setpoint.closed_loop_speed_control) {
-		boat_control_output.speed +=
-			pid_calculate(&_pid_speed, _boat_setpoint.speed, _vehicle_forward_speed, 0, dt);
-	}
-
-	if (_boat_setpoint.closed_loop_yaw_rate_control) {
-		boat_control_output.yaw_rate +=
-			pid_calculate(&_pid_angular_velocity, _boat_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt);
-	}
-
-	boat_control_output.timestamp = hrt_absolute_time();
-	_boat_control_output_pub.publish(boat_control_output);
-}
@@ -1,101 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file BoatControl.hpp
- *
- * Controller for heading rate and forward speed.
- */
-
-#pragma once
-
-#include <lib/pid/pid.h>
-#include <matrix/matrix/math.hpp>
-#include <px4_platform_common/module_params.h>
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-using namespace matrix;
-
-class BoatControl : public ModuleParams
-{
-public:
-	BoatControl(ModuleParams *parent);
-	~BoatControl() = default;
-
-	void control(float dt);
-	float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; }
-	float getVehicleYaw() const { return _vehicle_yaw; }
-	vehicle_local_position_s getLocalPosition() const { return _vehicle_local_position; }
-
-protected:
-	void updateParams() override;
-
-private:
-	uORB::Subscription _boat_setpoint_sub{ORB_ID(boat_setpoint)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-
-	uORB::Publication<boat_setpoint_s> _boat_control_output_pub{ORB_ID(boat_control_output)};
-
-	boat_setpoint_s _boat_setpoint{};
-
-	matrix::Quatf _vehicle_attitude_quaternion{};
-	float _vehicle_yaw{0.f};
-
-
-	// States
-	float _vehicle_body_yaw_rate{0.f};
-	float _vehicle_forward_speed{0.f};
-	Vector2f _vehicle_speed{0.f, 0.f};
-	vehicle_local_position_s _vehicle_local_position{};
-
-	PID_t _pid_angular_velocity;
-	PID_t _pid_speed;
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BT_SPD_P>) _param_bt_spd_p,
-		(ParamFloat<px4::params::BT_SPD_I>) _param_bt_spd_i,
-		(ParamFloat<px4::params::BT_SPD_IMAX>) _param_bt_spd_imax,
-		(ParamFloat<px4::params::BT_SPD_OUTLIM>) _param_bt_spd_outlim,
-		(ParamFloat<px4::params::BT_ANG_P>) _param_bt_ang_p,
-		(ParamFloat<px4::params::BT_ANG_I>) _param_bt_ang_i,
-		(ParamFloat<px4::params::BT_ANG_IMAX>) _param_bt_ang_imax,
-		(ParamFloat<px4::params::BT_ANG_OUTLIM>) _param_bt_ang_outlim
-	)
-};
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(BoatControl
-	BoatControl.cpp
-)
-
-target_link_libraries(BoatControl PUBLIC pid)
-target_include_directories(BoatControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -1,264 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "BoatGuidance.hpp"
-
-#include <mathlib/math/Limits.hpp>
-
-using namespace matrix;
-
-BoatGuidance::BoatGuidance(ModuleParams *parent) : ModuleParams(parent)
-{
-	updateParams();
-
-	_currentState = GuidanceState::kDriving;
-}
-
-void BoatGuidance::computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position,
-				   float dt)
-{
-	if (_position_setpoint_triplet_sub.updated()) {
-		_position_setpoint_triplet_sub.copy(&_position_setpoint_triplet);
-	}
-
-	if (_vehicle_global_position_sub.updated()) {
-		_vehicle_global_position_sub.copy(&_vehicle_global_position);
-	}
-
-	const matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon);
-	const matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon);
-	const matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon);
-	const matrix::Vector2d previous_waypoint(_position_setpoint_triplet.previous.lat,
-			_position_setpoint_triplet.previous.lon);
-
-	if (!_global_local_proj_ref.isInitialized()
-	    || (_global_local_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.timestamp)) {
-		_global_local_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
-						     vehicle_local_position.timestamp);
-	}
-
-	const Vector2f current_waypoint_local_frame = _global_local_proj_ref.project(
-				current_waypoint(0),
-				current_waypoint(1));
-	const Vector2f previous_waypoint_local_frame = _global_local_proj_ref.project(
-				previous_waypoint(0),
-				previous_waypoint(1));
-	const Vector2f local_frame_position = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-
-	const float distance_to_next_wp = get_distance_to_next_waypoint(
-			global_position(0),
-			global_position(1),
-			current_waypoint(0),
-			current_waypoint(1));
-
-	float heading_error = 0.f;
-	float speed_interpolation = 0.f;
-	float heading_to_next_waypoint = 0.f;
-	float heading_error_to_next_waypoint = 0.f;
-	float desired_speed = _param_bt_spd_cruise.get();
-
-	// Go back to driving, when the waypoint has been reached
-	if (_current_waypoint != current_waypoint) {
-		_currentState = GuidanceState::kDriving;
-	}
-
-	// Make boat stop when it arrives at the last waypoint
-	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
-		_currentState = GuidanceState::kGoalReached;
-	}
-
-	switch (_currentState) {
-	case GuidanceState::kDriving: {
-			if (PX4_ISFINITE(previous_waypoint(0)) && PX4_ISFINITE(previous_waypoint(1))) {
-
-				float look_ahead_distance = getLookAheadDistance(
-								    current_waypoint_local_frame,
-								    previous_waypoint_local_frame,
-								    local_frame_position
-							    );
-
-				float desired_heading = calcDesiredHeading(
-								current_waypoint_local_frame,
-								previous_waypoint_local_frame,
-								local_frame_position,
-								look_ahead_distance
-							);
-
-				heading_error = matrix::wrap_pi(desired_heading - yaw);
-				_desired_angular_velocity = heading_error;
-
-				heading_to_next_waypoint = get_bearing_to_next_waypoint(
-								   previous_waypoint(0),
-								   previous_waypoint(1),
-								   current_waypoint(0),
-								   current_waypoint(1)
-							   );
-
-			} else {
-				_previous_local_position = local_frame_position;
-				_previous_position = global_position;
-				_currentState = GuidanceState::kDrivingToAPoint;
-			}
-
-			break;
-		}
-
-	// Control logic if there is no previous waypoint
-	case GuidanceState::kDrivingToAPoint: {
-			float look_ahead_distance = getLookAheadDistance(
-							    current_waypoint_local_frame,
-							    _previous_local_position,
-							    local_frame_position
-						    );
-
-			float desired_heading = calcDesiredHeading(
-							current_waypoint_local_frame,
-							_previous_local_position,
-							local_frame_position,
-							look_ahead_distance
-						);
-
-			heading_error = matrix::wrap_pi(desired_heading - yaw);
-			_desired_angular_velocity = heading_error;
-
-			heading_to_next_waypoint = get_bearing_to_next_waypoint(
-							   _previous_position(0),
-							   _previous_position(1),
-							   current_waypoint(0),
-							   current_waypoint(1)
-						   );
-			break;
-		}
-
-	case GuidanceState::kGoalReached:
-		desired_speed = 0.f;
-		heading_error = 0.f;
-		_desired_angular_velocity = 0.f;
-		break;
-	}
-
-	heading_error_to_next_waypoint = matrix::wrap_pi(heading_to_next_waypoint - yaw);
-
-	// Interpolate the speed based on the heading error
-	if (PX4_ISFINITE(heading_error_to_next_waypoint) && desired_speed > 0.1f) {
-
-		speed_interpolation = math::interpolate<float>(abs(heading_error_to_next_waypoint),
-				      _param_bt_min_heading_error.get() * M_PI_F / 180.f,
-				      _param_bt_max_heading_error.get() * M_PI_F / 180.f,
-				      _param_bt_spd_cruise.get(),
-				      _param_bt_spd_min.get());
-		desired_speed = math::constrain(speed_interpolation, _param_bt_spd_min.get(), _max_speed);
-
-	}
-
-	boat_setpoint_s output{};
-	output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);
-	output.yaw_rate = math::constrain(_desired_angular_velocity, -_max_angular_velocity, _max_angular_velocity);
-	output.closed_loop_speed_control = true;
-	output.closed_loop_yaw_rate_control = true;
-	output.timestamp = hrt_absolute_time();
-
-	_boat_setpoint_pub.publish(output);
-
-	_current_waypoint = current_waypoint;
-}
-
-float BoatGuidance::getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-		const Vector2f &curr_pos_local)
-{
-	// Calculate crosstrack error
-	const Vector2f prev_wp_to_curr_wp_local = curr_wp_local - prev_wp_local;
-
-	if (prev_wp_to_curr_wp_local.norm() < FLT_EPSILON) { // Avoid division by 0 (this case should not happen)
-		return 0.f;
-	}
-
-	const Vector2f prev_wp_to_curr_pos_local = curr_pos_local - prev_wp_local;
-	const Vector2f distance_on_line_segment = ((prev_wp_to_curr_pos_local * prev_wp_to_curr_wp_local) /
-			prev_wp_to_curr_wp_local.norm()) * prev_wp_to_curr_wp_local.normalized();
-	const Vector2f crosstrack_error = (prev_wp_local + distance_on_line_segment) - curr_pos_local;
-
-	if (crosstrack_error.length() < _param_look_ahead_distance.get()) {
-		return _param_look_ahead_distance.get();
-
-	} else {
-		return crosstrack_error.length();
-	}
-}
-
-float BoatGuidance::calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-				       Vector2f const &curr_pos_local, float const &lookahead_distance)
-{
-	// Setup variables
-	const float line_segment_slope = (curr_wp_local(1) - prev_wp_local(1)) / (curr_wp_local(0) - prev_wp_local(0));
-	const float line_segment_rover_offset = prev_wp_local(1) - curr_pos_local(1) + line_segment_slope * (curr_pos_local(
-			0) - prev_wp_local(0));
-	const float a = -line_segment_slope;
-	const float c = -line_segment_rover_offset;
-	const float r = lookahead_distance;
-	const float x0 = -a * c / (a * a + 1.0f);
-	const float y0 = -c / (a * a + 1.0f);
-
-	// Calculate intersection points
-	if (c * c > r * r * (a * a + 1.0f) + FLT_EPSILON) { // No intersection points exist
-		return 0.f;
-
-	} else if (abs(c * c - r * r * (a * a + 1.0f)) < FLT_EPSILON) { // One intersection point exists
-		return atan2f(y0, x0);
-
-	} else { // Two intersetion points exist
-		const float d = r * r - c * c / (a * a + 1.0f);
-		const float mult = sqrt(d / (a * a + 1.0f));
-		const float ax = x0 + mult;
-		const float bx = x0 - mult;
-		const float ay = y0 - a * mult;
-		const float by = y0 + a * mult;
-		const Vector2f point1(ax, ay);
-		const Vector2f point2(bx, by);
-		const Vector2f distance1 = (curr_wp_local - curr_pos_local) - point1;
-		const Vector2f distance2 = (curr_wp_local - curr_pos_local) - point2;
-
-		// Return intersection point closer to current waypoint
-		if (distance1.norm_squared() < distance2.norm_squared()) {
-			return atan2f(ay, ax);
-
-		} else {
-			return atan2f(by, bx);
-		}
-	}
-}
-
-void BoatGuidance::updateParams()
-{
-	ModuleParams::updateParams();
-}
@@ -1,170 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/module_params.h>
-
-#include <matrix/matrix/math.hpp>
-#include <matrix/math.hpp>
-#include <mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-#include <math.h>
-
-#include <lib/motion_planning/PositionSmoothing.hpp>
-#include <lib/motion_planning/VelocitySmoothing.hpp>
-
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/boat_setpoint.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-
-#include <lib/pid/pid.h>
-#include <lib/l1/ECL_L1_Pos_Controller.hpp>
-
-using namespace matrix;
-/**
- * @brief Enum class for the different states of guidance.
- */
-enum class GuidanceState {
-	kDriving, ///< The vehicle is currently driving straight.
-	kDrivingToAPoint, ///< The vehicle is currently driving to the next waypoint.
-	kGoalReached ///< The vehicle has reached its goal.
-};
-
-/**
- * @brief Class for boat drive guidance.
- */
-class BoatGuidance : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for BoatGuidance.
-	 * @param parent The parent ModuleParams object.
-	 */
-	BoatGuidance(ModuleParams *parent);
-	~BoatGuidance() = default;
-
-	/**
-	 * @brief Compute guidance for the vehicle.
-	 * @param global_pos The global position of the vehicle in degrees.
-	 * @param current_waypoint The current waypoint the vehicle is heading towards in degrees.
-	 * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees.
-	 * @param vehicle_yaw The yaw orientation of the vehicle in radians.
-	 * @param body_velocity The velocity of the vehicle in m/s.
-	 * @param angular_velocity The angular velocity of the vehicle in rad/s.
-	 * @param dt The time step in seconds.
-	 */
-	void computeGuidance(float yaw, vehicle_local_position_s vehicle_local_position, float dt);
-
-	/**
-	 * @brief Calculate the lookahead distance based on the current and previous waypoints and the current position.
-	 * @param curr_wp_local The current waypoint in local coordinates.
-	 * @param prev_wp_local The previous waypoint in local coordinates.
-	 * @param curr_pos_local The current position in local coordinates.
-	 * @return The calculated lookahead distance.
-	 */
-	float getLookAheadDistance(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local,
-				   const Vector2f &curr_pos_local);
-
-	/**
-	 * @brief Calculate the desired heading based on the current and previous waypoints, current position, and lookahead distance.
-	 * @param curr_wp_local The current waypoint in local coordinates.
-	 * @param prev_wp_local The previous waypoint in local coordinates.
-	 * @param curr_pos_local The current position in local coordinates.
-	 * @param lookahead_distance The lookahead distance.
-	 * @return The calculated desired heading.
-	 */
-	float calcDesiredHeading(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local,
-				 const float &lookahead_distance);
-
-	/**
-	 * @brief Set the maximum speed for the vehicle.
-	 * @param max_speed The maximum speed in m/s.
-	 * @return The set maximum speed in m/s.
-	 */
-	float setMaxSpeed(float max_speed) { return _max_speed = max_speed; }
-
-
-	/**
-	 * @brief Set the maximum angular velocity for the boat.
-	 * @param max_angular_velocity The maximum angular velocity in rad/s.
-	 * @return The set maximum angular velocity in rad/s.
-	 */
-	float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; }
-
-
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
-
-	uORB::Publication<boat_setpoint_s> _boat_setpoint_pub{ORB_ID(boat_setpoint)};
-
-	position_setpoint_triplet_s _position_setpoint_triplet{};
-	vehicle_global_position_s _vehicle_global_position{};
-
-	GuidanceState _currentState;
-
-	float _desired_angular_velocity{};
-	float _max_angular_velocity{};
-	float _look_ahead_distance{};
-	float _max_speed{};
-
-	VelocitySmoothing _forwards_velocity_smoothing{};
-	PositionSmoothing _position_smoothing{};
-	MapProjection _global_local_proj_ref{};
-	matrix::Vector2d _current_waypoint{};
-	ECL_L1_Pos_Controller _l1_guidance{};
-	Vector2f _previous_local_position{};
-	Vector2d _previous_position{};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BT_MAX_HERR>) _param_bt_max_heading_error,
-		(ParamFloat<px4::params::BT_MIN_HERR>) _param_bt_min_heading_error,
-		(ParamFloat<px4::params::BT_LOOKAHEAD>) _param_look_ahead_distance,
-		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,
-		(ParamFloat<px4::params::BT_SPD_CRUISE>) _param_bt_spd_cruise,
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
-		(ParamFloat<px4::params::BT_SPD_MAX>) _param_bt_spd_max,
-		(ParamFloat<px4::params::BT_SPD_MIN>) _param_bt_spd_min
-	)
-};
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(BoatGuidance
-	BoatGuidance.cpp
-	BoatGuidance.hpp
-)
-
-target_include_directories(BoatGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "BoatKinematics.hpp"
-
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
-using namespace time_literals;
-
-BoatKinematics::BoatKinematics(ModuleParams *parent) : ModuleParams(parent)
-{}
-
-void BoatKinematics::allocate()
-{
-	hrt_abstime now = hrt_absolute_time();
-
-	if (_boat_control_output_sub.updated()) {
-		_boat_control_output_sub.copy(&_boat_control_output);
-	}
-
-	const bool setpoint_timeout = (_boat_control_output.timestamp + 100_ms) < now;
-
-	Vector2f boat_output =
-		computeInverseKinematics(_boat_control_output.speed, _boat_control_output.yaw_rate);
-
-	if (!_armed || setpoint_timeout) {
-		boat_output = {}; // stop
-	}
-
-	boat_output = matrix::constrain(boat_output, -1.f, 1.f);
-
-	actuator_motors_s actuator_motors{};
-	actuator_motors.control[0] = boat_output(0);
-	actuator_motors.control[1] = boat_output(0);
-	actuator_motors.timestamp = now;
-	_actuator_motors_pub.publish(actuator_motors);
-
-	actuator_servos_s actuator_servos{};
-	actuator_servos.control[0] = boat_output(1);
-	actuator_servos.control[1] = boat_output(1);
-	actuator_servos.timestamp = now;
-	_actuator_servos_pub.publish(actuator_servos);
-}
-
-matrix::Vector2f BoatKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const
-{
-	if (_max_speed < FLT_EPSILON) {
-		return Vector2f();
-	}
-
-	// Room for more advanced dynamics, if required
-
-	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
-	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
-
-	float throttle = linear_velocity_x / _max_speed;
-	float rudder_angle = yaw_rate / _max_angular_velocity;
-
-	return Vector2f(throttle, rudder_angle);
-}
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <matrix/matrix/math.hpp>
-#include <px4_platform_common/module_params.h>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/boat_setpoint.h>
-
-
-/**
- * @brief Boat Drive Kinematics class for computing the kinematics of a boat drive robot.
- *
- * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
- * given linear velocity and yaw rate.
- */
-class BoatKinematics : public ModuleParams
-{
-public:
-	BoatKinematics(ModuleParams *parent);
-	~BoatKinematics() = default;
-
-	/**
-	 * @brief Sets the wheel base of the robot.
-	 *
-	 * @param wheel_base The distance between the centers of the wheels.
-	 */
-	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
-
-	/**
-	 * @brief Sets the maximum speed of the robot.
-	 *
-	 * @param max_speed The maximum speed of the robot.
-	 */
-	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
-
-	/**
-	 * @brief Sets the maximum angular speed of the robot.
-	 *
-	 * @param max_angular_speed The maximum angular speed of the robot.
-	 */
-	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
-
-	void setArmed(const bool armed) { _armed = armed; };
-
-	void allocate();
-
-	/**
-	 * @brief Computes the inverse kinematics for boat drive.
-	 *
-	 * @param linear_velocity_x Linear velocity along the x-axis.
-	 * @param yaw_rate Yaw rate of the robot.
-	 * @return matrix::Vector2f Motor velocities for the right and left motors.
-	 */
-	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const;
-
-private:
-	uORB::Subscription _boat_control_output_sub{ORB_ID(boat_control_output)};
-	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::PublicationMulti<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
-
-	boat_setpoint_s _boat_control_output{};
-
-	bool _armed = false;
-
-	float _wheel_base{0.f};
-	float _max_speed{0.f};
-	float _max_angular_velocity{0.f};
-
-	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
-	)
-};
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_library(BoatKinematics
-	BoatKinematics.cpp
-)
-
-target_include_directories(BoatKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
-
-px4_add_functional_gtest(SRC BoatKinematicsTest.cpp LINKLIBS BoatKinematics)
@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-add_subdirectory(BoatControl)
-add_subdirectory(BoatGuidance)
-add_subdirectory(BoatKinematics)
-
-px4_add_module(
-	MODULE modules__boat
-	MAIN boat
-	SRCS
-		Boat.cpp
-		Boat.hpp
-	DEPENDS
-		BoatControl
-		BoatGuidance
-		BoatKinematics
-		px4_work_queue
-		l1
-		# modules__control_allocator # for parameter CA_R_REV
-	MODULE_CONFIG
-		module.yaml
-)
@@ -1,6 +0,0 @@
-menuconfig MODULES_BOAT
-	bool "boat"
-	default n
-	depends on MODULES_CONTROL_ALLOCATOR
-	---help---
-		Enable support for control of boats
@@ -1,149 +0,0 @@
-module_name: Boat Drive
-
-parameters:
-  - group: Boat Drive
-    definitions:
-      BT_SPD_SCALE:
-        description:
-          short: Manual speed scale
-        type: float
-        min: 0
-        max: 1
-        increment: 0.01
-        decimal: 2
-        default: 1
-      BT_ANG_VEL_SCALE:
-        description:
-          short: Manual angular velocity scale
-        type: float
-        min: 0
-        max: 1
-        increment: 0.01
-        decimal: 2
-        default: 1
-      BT_SPD_CRUISE:
-        description:
-          short: Default cruise speed
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 50.0
-        decimal: 1
-        default: 7.0
-      BT_SPD_MAX:
-        description:
-          short: Maximum speed
-        type: float
-        unit: m/s
-        min: 1.0
-        max: 50.0
-        decimal: 1
-        default: 17.0
-      BT_SPD_MIN:
-        description:
-          short: Minimum speed
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 50.0
-        decimal: 1
-        default: 2.0
-      BT_SPD_P:
-        description:
-          short: Speed controller proportional gain
-        type: float
-        min: 0.0
-        max: 2.0
-        decimal: 2
-        default: 2.0
-      BT_SPD_I:
-        description:
-          short: Speed controller integral gain
-        type: float
-        min: 0.0
-        max: 20.0
-        decimal: 2
-        default: 1.0
-      BT_SPD_IMAX:
-        description:
-          short: Speed integral maximum value
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 20.0
-      BT_SPD_OUTLIM:
-        description:
-          short: Speed PI controller output limit
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 200.0
-        decimal: 1
-        default: 50.0
-      BT_ANG_MAX:
-        description:
-          short: Maximum angular velocity
-        type: float
-        unit: rad/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 1.0
-      BT_ANG_P:
-        description:
-          short: Angular velocity controller proportional gain
-        type: float
-        min: 0.0
-        max: 2.0
-        decimal: 2
-        default: 0.2
-      BT_ANG_I:
-        description:
-          short: Angular velocity controller integral gain
-        type: float
-        min: 0.0
-        max: 20.0
-        decimal: 2
-        default: 0.0
-      BT_ANG_IMAX:
-        description:
-          short: Angular velocity controller integral maximum value
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 20.0
-        decimal: 1
-        default: 20.0
-      BT_ANG_OUTLIM:
-        description:
-          short: Angular velocity controller PI controller output limit
-        type: float
-        unit: m/s
-        min: 0.0
-        max: 200.0
-        decimal: 1
-        default: 50.0
-      BT_MAX_HERR:
-        description:
-          short: Max heading error for slowdown
-        type: float
-        min: 0.0
-        decimal: 2
-        default: 90.0
-      BT_MIN_HERR:
-        description:
-          short: Min heading error for full speed
-        type: float
-        min: 0.0
-        decimal: 2
-        default: 30.0
-      BT_LOOKAHEAD:
-        description:
-          short: Lookahead distance for heading error
-        type: float
-        unit: m
-        min: 0.0
-        decimal: 2
-        default: 10.0
@@ -72,7 +72,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit

 	// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
 	if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
-		_currentState = GuidanceState::kGoalReached;
+		_currentState = GuidanceState::GOAL_REACHED;
 	}

 	float desired_speed = 0.f;
@@ -82,12 +82,12 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 		desired_speed = 0.f;

 		if (fabsf(heading_error) < 0.05f) {
-			_currentState = GuidanceState::kDriving;
+			_currentState = GuidanceState::DRIVING;
 		}

 		break;

-	case GuidanceState::kDriving: {
+	case GuidanceState::DRIVING: {
 			const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
 						   _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
 			_forwards_velocity_smoothing.updateDurations(max_velocity);
@@ -97,7 +97,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
 			break;
 		}

-	case GuidanceState::kGoalReached:
+	case GuidanceState::GOAL_REACHED:
 		// temporary till I find a better way to stop the vehicle
 		desired_speed = 0.f;
 		heading_error = 0.f;
@@ -58,8 +58,8 @@
 */
 enum class GuidanceState {
 	TURNING, ///< The vehicle is currently turning.
-	kDriving, ///< The vehicle is currently driving straight.
-	kGoalReached ///< The vehicle has reached its goal.
+	DRIVING, ///< The vehicle is currently driving straight.
+	GOAL_REACHED ///< The vehicle has reached its goal.
 };

 /**
@@ -116,107 +116,111 @@ endif()
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
+	EKF/bias_estimator.cpp
 	EKF/control.cpp
 	EKF/covariance.cpp
 	EKF/ekf.cpp
 	EKF/ekf_helper.cpp
 	EKF/estimator_interface.cpp
+	EKF/fake_height_control.cpp
+	EKF/fake_pos_control.cpp
 	EKF/height_control.cpp
+	EKF/imu_down_sampler.cpp
+	EKF/output_predictor.cpp
 	EKF/velocity_fusion.cpp
 	EKF/position_fusion.cpp
 	EKF/yaw_fusion.cpp
+	EKF/zero_innovation_heading_update.cpp

-	EKF/imu_down_sampler/imu_down_sampler.cpp
-
-	EKF/aid_sources/fake_height_control.cpp
-	EKF/aid_sources/fake_pos_control.cpp
 	EKF/aid_sources/ZeroGyroUpdate.cpp
 	EKF/aid_sources/ZeroVelocityUpdate.cpp
-	EKF/aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS EKF/aid_sources/airspeed/airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
+	list(APPEND EKF_SRCS EKF/aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS EKF/aid_sources/auxvel/auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/barometer/baro_height_control.cpp
+		EKF/baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS EKF/aid_sources/drag/drag_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/external_vision/ev_control.cpp
-		EKF/aid_sources/external_vision/ev_height_control.cpp
-		EKF/aid_sources/external_vision/ev_pos_control.cpp
-		EKF/aid_sources/external_vision/ev_vel_control.cpp
-		EKF/aid_sources/external_vision/ev_yaw_control.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
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/gnss/gnss_height_control.cpp
-		EKF/aid_sources/gnss/gps_checks.cpp
-		EKF/aid_sources/gnss/gps_control.cpp
+		EKF/gnss_height_control.cpp
+		EKF/gps_checks.cpp
+		EKF/gps_control.cpp
 	)

-	if(CONFIG_EKF2_GNSS_YAW)
-		list(APPEND EKF_SRCS EKF/aid_sources/gnss/gps_yaw_fusion.cpp)
-	endif()
-
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
+endif()
+
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS EKF/aid_sources/gravity/gravity_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/magnetometer/mag_control.cpp
-		EKF/aid_sources/magnetometer/mag_fusion.cpp
+		EKF/mag_3d_control.cpp
+		EKF/mag_control.cpp
+		EKF/mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/optical_flow/optical_flow_control.cpp
-		EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
+		EKF/optical_flow_control.cpp
+		EKF/optflow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/range_height_control.cpp
-		EKF/aid_sources/range_finder/sensor_range_finder.cpp
+		EKF/range_finder_consistency_check.cpp
+		EKF/range_height_control.cpp
+		EKF/sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
 endif()

 add_subdirectory(EKF)

+
+
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -256,10 +260,7 @@ px4_add_module(
 		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
-
 		${EKF_LIBS}
-		bias_estimator
-		output_predictor
 	UNITY_BUILD
 	)

@@ -31,110 +31,109 @@
 #
 ############################################################################

-add_subdirectory(bias_estimator)
-add_subdirectory(output_predictor)
-
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
+	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	ekf.cpp
 	ekf_helper.cpp
 	estimator_interface.cpp
+	fake_height_control.cpp
+	fake_pos_control.cpp
 	height_control.cpp
+	imu_down_sampler.cpp
+	output_predictor.cpp
 	velocity_fusion.cpp
 	position_fusion.cpp
 	yaw_fusion.cpp
+	zero_innovation_heading_update.cpp

-	imu_down_sampler/imu_down_sampler.cpp
-
-	aid_sources/fake_height_control.cpp
-	aid_sources/fake_pos_control.cpp
 	aid_sources/ZeroGyroUpdate.cpp
 	aid_sources/ZeroVelocityUpdate.cpp
-	aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
+	list(APPEND EKF_SRCS aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		aid_sources/barometer/baro_height_control.cpp
+		baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS aid_sources/drag/drag_fusion.cpp)
+	list(APPEND EKF_SRCS drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		aid_sources/external_vision/ev_control.cpp
-		aid_sources/external_vision/ev_height_control.cpp
-		aid_sources/external_vision/ev_pos_control.cpp
-		aid_sources/external_vision/ev_vel_control.cpp
-		aid_sources/external_vision/ev_yaw_control.cpp
+		ev_control.cpp
+		ev_height_control.cpp
+		ev_pos_control.cpp
+		ev_vel_control.cpp
+		ev_yaw_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		aid_sources/gnss/gnss_height_control.cpp
-		aid_sources/gnss/gps_checks.cpp
-		aid_sources/gnss/gps_control.cpp
+		gnss_height_control.cpp
+		gps_checks.cpp
+		gps_control.cpp
 	)

-	if(CONFIG_EKF2_GNSS_YAW)
-		list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp)
-	endif()
-
 	add_subdirectory(yaw_estimator)
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

+if(CONFIG_EKF2_GNSS_YAW)
+	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
+endif()
+
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS aid_sources/gravity/gravity_fusion.cpp)
+	list(APPEND EKF_SRCS gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		aid_sources/magnetometer/mag_control.cpp
-		aid_sources/magnetometer/mag_fusion.cpp
+		mag_3d_control.cpp
+		mag_control.cpp
+		mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		aid_sources/optical_flow/optical_flow_control.cpp
-		aid_sources/optical_flow/optical_flow_fusion.cpp
+		optical_flow_control.cpp
+		optflow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/range_height_control.cpp
-		aid_sources/range_finder/sensor_range_finder.cpp
+		range_finder_consistency_check.cpp
+		range_height_control.cpp
+		sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS aid_sources/sideslip/sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS terrain_estimator.cpp)
 endif()

 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -147,9 +146,7 @@ target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PAT

 target_link_libraries(ecl_EKF
 	PRIVATE
-		bias_estimator
 		geo
-		output_predictor
 		world_magnetic_model
 		${EKF_LIBS}
 )
@@ -42,7 +42,7 @@
 #ifndef EKF_SENSOR_HPP
 #define EKF_SENSOR_HPP

-#include <cstdint>
+#include "common.h"

 namespace estimator
 {
@@ -33,7 +33,7 @@

 #include "ekf.h"

-#include "aid_sources/aux_global_position/aux_global_position.hpp"
+#include "aux_global_position.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -42,8 +42,8 @@
 //  WelfordMean for init?
 //  WelfordMean for rate

-#include "../../common.h"
-#include "../../RingBuffer.h"
+#include "common.h"
+#include "RingBuffer.h"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -196,39 +196,3 @@ void Ekf::stopBaroHgtFusion()
 		_control_status.flags.baro_hgt = false;
 	}
 }
-
-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
-{
-	if (_control_status.flags.wind && local_position_is_valid()) {
-		// calculate static pressure error = Pmeas - Ptruth
-		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
-		// negative X and Y directions. Used to correct baro data for positional errors
-
-		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
-		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
-
-		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
-
-		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
-
-		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
-
-		const Vector3f K_pstatic_coef(
-			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
-			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
-			_params.static_pressure_coef_z);
-
-		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
-
-		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
-
-		// correct baro measurement using pressure error estimate and assuming sea level gravity
-		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
-	}
-
-	// otherwise return the uncorrected baro measurement
-	return baro_alt_uncompensated;
-}
-#endif // CONFIG_EKF2_BARO_COMPENSATION
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-add_library(bias_estimator
-	bias_estimator.cpp
-	bias_estimator.hpp
-	height_bias_estimator.hpp
-	position_bias_estimator.hpp
-)
-
-add_dependencies(bias_estimator prebuild_targets)
@@ -70,6 +70,7 @@ static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable
 static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
 static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
+static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL      = 500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)

 // bad accelerometer detection and mitigation
@@ -196,6 +197,12 @@ struct baroSample {
 	bool        reset{false};
 };

+struct rangeSample {
+	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
+	float       rng{};      ///< range (distance to ground) measurement (m)
+	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
+};
+
 struct airspeedSample {
 	uint64_t    time_us{};          ///< timestamp of the measurement (uSec)
 	float       true_airspeed{};    ///< true airspeed measurement (m/sec)
@@ -282,25 +282,6 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 	P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
 }

-void Ekf::resetGyroBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
-}
-
-void Ekf::resetGyroBiasZCov()
-{
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
-}
-
-void Ekf::resetAccelBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
-}
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void Ekf::resetMagCov()
 {
@@ -314,10 +295,7 @@ void Ekf::resetMagCov()
 }
 #endif // CONFIG_EKF2_MAGNETOMETER

-#if defined(CONFIG_EKF2_WIND)
-void Ekf::resetWindCov()
+void Ekf::resetGyroBiasZCov()
 {
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
+	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
 }
-#endif // CONFIG_EKF2_WIND
@@ -49,9 +49,9 @@
 # include "yaw_estimator/EKFGSF_yaw.h"
 #endif // CONFIG_EKF2_GNSS

-#include "bias_estimator/bias_estimator.hpp"
-#include "bias_estimator/height_bias_estimator.hpp"
-#include "bias_estimator/position_bias_estimator.hpp"
+#include "bias_estimator.hpp"
+#include "height_bias_estimator.hpp"
+#include "position_bias_estimator.hpp"

 #include <ekf_derivation/generated/state.h>

@@ -63,7 +63,7 @@
 #include "aid_sources/ZeroVelocityUpdate.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-# include "aid_sources/aux_global_position/aux_global_position.hpp"
+# include "aux_global_position.hpp"
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

 enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -265,13 +265,12 @@ public:
 	// get the 1-sigma horizontal and vertical velocity uncertainty
 	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;

-	// Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
-	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
-	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
-	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
+	// get the vehicle control limits required by the estimator to keep within sensor limitations
 	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;

+	// Reset all IMU bias states and covariances to initial alignment values.
+	void resetImuBias();
+
 	void resetGyroBias();
 	void resetGyroBiasCov();

@@ -391,7 +390,7 @@ public:
 	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 					float &hagl, float &beta) const;

-	// return a bitmask integer that describes which state estimates are valid
+	// return a bitmask integer that describes which state estimates can be used for flight control
 	void get_ekf_soln_status(uint16_t *status) const;

 	HeightSensor getHeightSensorRef() const { return _height_sensor_ref; }
@@ -758,7 +757,7 @@ private:

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// ekf sequential fusion of magnetometer measurements
-	bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states = false, bool update_tilt = false);
+	bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true, bool update_tilt = true);

 	// fuse magnetometer declination measurement
 	// argument passed in is the declination uncertainty in radians
@@ -948,6 +947,10 @@ private:
 	// and a scalar innovation value
 	void fuse(const VectorState &K, float innovation);

+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
+#endif // CONFIG_EKF2_BARO_COMPENSATION
+
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;

@@ -1025,6 +1028,7 @@ private:
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
 	void controlMagFusion();
+	void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src);

 	bool checkHaglYawResetReq() const;

@@ -1075,11 +1079,6 @@ private:
 	void stopBaroHgtFusion();

 	void updateGroundEffect();
-
-# if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
-# endif // CONFIG_EKF2_BARO_COMPENSATION
-
 #endif // CONFIG_EKF2_BAROMETER

 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
@@ -56,6 +56,43 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }

+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
+float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+{
+	if (_control_status.flags.wind && local_position_is_valid()) {
+		// calculate static pressure error = Pmeas - Ptruth
+		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
+		// negative X and Y directions. Used to correct baro data for positional errors
+
+		// Calculate airspeed in body frame
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
+
+		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
+
+		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
+
+		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
+
+		const Vector3f K_pstatic_coef(
+			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
+			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
+			_params.static_pressure_coef_z);
+
+		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+
+		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
+
+		// correct baro measurement using pressure error estimate and assuming sea level gravity
+		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
+	}
+
+	// otherwise return the uncorrected baro measurement
+	return baro_alt_uncompensated;
+}
+#endif // CONFIG_EKF2_BARO_COMPENSATION
+
+// calculate the earth rotation vector
 Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 {
 	return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
@@ -198,6 +235,7 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2));
 }

+// get the 1-sigma horizontal and vertical velocity uncertainty
 void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 {
 	float hvel_err = sqrtf(P.trace<2>(State::vel.idx));
@@ -238,6 +276,13 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }

+/*
+Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
+vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
+vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
+hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
+hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
+*/
 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
 {
 	// Do not require limiting by default
@@ -285,6 +330,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 #endif // CONFIG_EKF2_RANGE_FINDER
 }

+void Ekf::resetImuBias()
+{
+	resetGyroBias();
+	resetAccelBias();
+}
+
 void Ekf::resetGyroBias()
 {
 	// Zero the gyro bias states
@@ -293,6 +344,13 @@ void Ekf::resetGyroBias()
 	resetGyroBiasCov();
 }

+void Ekf::resetGyroBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+}
+
 void Ekf::resetAccelBias()
 {
 	// Zero the accel bias states
@@ -301,6 +359,18 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }

+void Ekf::resetAccelBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+}
+
+// get EKF innovation consistency check status information comprising of:
+// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
+// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
+// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
+// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
 void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 				     float &hagl, float &beta) const
 {
@@ -428,6 +498,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 #endif // CONFIG_EKF2_SIDESLIP
 }

+// return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
@@ -623,6 +694,53 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER

+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
+{
+	// save a copy of the quaternion state for later use in calculating the amount of reset change
+	const Quatf quat_before_reset = _state.quat_nominal;
+
+	// update the yaw angle variance
+	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
+		P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
+	}
+
+	// update transformation matrix from body to world frame using the current estimate
+	// update the rotation matrix using the new yaw value
+	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
+
+	// calculate the amount that the quaternion has changed by
+	const Quatf quat_after_reset(_R_to_earth);
+	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
+
+	// update quaternion states
+	_state.quat_nominal = quat_after_reset;
+
+	// add the reset amount to the output observer buffered data
+	_output_predictor.resetQuaternion(q_error);
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	// update EV attitude error filter
+	if (_ev_q_error_initialized) {
+		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
+		_ev_q_error_filt.reset(ev_q_error_updated);
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+	// record the state change
+	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
+		_state_reset_status.quat_change = q_error;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
+		_state_reset_status.quat_change.normalize();
+	}
+
+	_state_reset_status.reset_count.quat++;
+
+	_time_last_heading_fuse = _time_delayed_us;
+}
+
 #if defined(CONFIG_EKF2_WIND)
 void Ekf::resetWind()
 {
@@ -646,6 +764,11 @@ void Ekf::resetWindToZero()
 	resetWindCov();
 }

+void Ekf::resetWindCov()
+{
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
+}
 #endif // CONFIG_EKF2_WIND

 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
@@ -266,7 +266,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
+void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 {
 	if (!_initialised) {
 		return;
@@ -274,7 +274,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)

 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);

 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {

-		sensor::rangeSample range_sample_new{range_sample};
+		rangeSample range_sample_new{range_sample};
 		range_sample_new.time_us = time_us;

 		_range_buffer->push(range_sample_new);
@@ -63,12 +63,12 @@

 #include "common.h"
 #include "RingBuffer.h"
-#include "imu_down_sampler/imu_down_sampler.hpp"
-#include "output_predictor/output_predictor.h"
+#include "imu_down_sampler.hpp"
+#include "output_predictor.h"

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-# include "aid_sources/range_finder/range_finder_consistency_check.hpp"
-# include "aid_sources/range_finder/sensor_range_finder.hpp"
+# include "range_finder_consistency_check.hpp"
+# include "sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER

 #include <lib/atmosphere/atmosphere.h>
@@ -107,7 +107,7 @@ public:
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const estimator::sensor::rangeSample &range_sample);
+	void setRangeData(const rangeSample &range_sample);

 	// set sensor limitations reported by the rangefinder
 	void set_rangefinder_limits(float min_distance, float max_distance)
@@ -115,7 +115,7 @@ public:
 		_range_sensor.setLimits(min_distance, max_distance);
 	}

-	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
+	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -356,7 +356,7 @@ protected:
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<sensor::rangeSample> *_range_buffer{nullptr};
+	RingBuffer<rangeSample> *_range_buffer{nullptr};
 	uint64_t _time_last_range_buffer_push{0};

 	sensor::SensorRangeFinder _range_sensor{};
@@ -39,7 +39,7 @@
 #define EKF_HEIGHT_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
-#include "../common.h"
+#include "common.h"

 class HeightBiasEstimator: public BiasEstimator
 {
@@ -1,4 +1,4 @@
-#include "imu_down_sampler/imu_down_sampler.hpp"
+#include "imu_down_sampler.hpp"

 #include <lib/mathlib/mathlib.h>

@@ -41,7 +41,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>

-#include "../common.h"
+#include "common.h"

 using namespace estimator;

@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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 mag_3d_control.cpp
+ * Control functions for ekf mag 3D fusion
+ */
+
+#include "ekf.h"
+
+void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing,
+			     estimator_aid_source3d_s &aid_src)
+{
+	static constexpr const char *AID_SRC_NAME = "mag";
+
+	resetEstimatorAidStatus(aid_src);
+
+	const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion
+
+	// determine if we should use mag fusion
+	bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
+					     && _control_status.flags.tilt_align
+					     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && mag_sample.mag.longerThan(0.f)
+					     && mag_sample.mag.isAllFinite();
+
+	const bool starting_conditions_passing = common_starting_conditions_passing
+			&& continuing_conditions_passing;
+
+	// For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise
+	// before they are used to constrain heading drift
+	_control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO)
+				       && _control_status.flags.mag
+				       && _control_status.flags.mag_aligned_in_flight
+				       && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps)
+				       && !_control_status.flags.mag_fault
+				       && !_control_status.flags.ev_yaw
+				       && !_control_status.flags.gps_yaw;
+
+	const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
+
+	_control_status.flags.mag_hdg = ((_params.mag_fusion_type == MagFuseType::HEADING)
+					      || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D))
+					     && _control_status.flags.tilt_align
+					     && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
+					         || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
+					     && !_control_status.flags.mag_fault
+					     && !_control_status.flags.mag_field_disturbed
+					     && !_control_status.flags.ev_yaw
+					     && !_control_status.flags.gps_yaw;
+
+	// TODO: allow clearing mag_fault if mag_3d is good?
+
+	if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
+		ECL_INFO("starting mag 3D fusion");
+
+	} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
+		ECL_INFO("stopping mag 3D fusion");
+	}
+
+	// if we are using 3-axis magnetometer fusion, but without external NE aiding,
+	// then the declination must be fused as an observation to prevent long term heading drift
+	// fusing declination when gps aiding is available is optional.
+	const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
+	_control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight));
+
+	if (_control_status.flags.mag) {
+		aid_src.timestamp_sample = mag_sample.time_us;
+
+		if (continuing_conditions_passing && _control_status.flags.yaw_align) {
+
+			if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
+				ECL_INFO("reset to %s", AID_SRC_NAME);
+				resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+				aid_src.time_last_fuse = _time_delayed_us;
+
+			} else {
+				if (!_mag_decl_cov_reset) {
+					// After any magnetic field covariance reset event the earth field state
+					// covariances need to be corrected to incorporate knowledge of the declination
+					// before fusing magnetometer data to prevent rapid rotation of the earth field
+					// states for the first few observations.
+					fuseDeclination(0.02f);
+					_mag_decl_cov_reset = true;
+					fuseMag(mag_sample.mag, aid_src, false);
+
+				} else {
+					// The normal sequence is to fuse the magnetometer data first before fusing
+					// declination angle at a higher uncertainty to allow some learning of
+					// declination angle over time.
+					const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
+					const bool update_tilt = _control_status.flags.mag_3D;
+					fuseMag(mag_sample.mag, aid_src, update_all_states, update_tilt);
+
+					if (_control_status.flags.mag_dec) {
+						fuseDeclination(0.5f);
+					}
+				}
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
+
+			if (is_fusion_failing) {
+				if (_nb_mag_3d_reset_available > 0) {
+					// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
+					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
+					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
+					aid_src.time_last_fuse = _time_delayed_us;
+
+					if (_control_status.flags.in_air) {
+						_nb_mag_3d_reset_available--;
+					}
+
+				} else if (starting_conditions_passing) {
+					// Data seems good, but previous reset did not fix the issue
+					// something else must be wrong, declare the sensor faulty and stop the fusion
+					//_control_status.flags.mag_fault = true;
+					ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
+					stopMagFusion();
+
+				} else {
+					// A reset did not fix the issue but all the starting checks are not passing
+					// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+					ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
+					stopMagFusion();
+				}
+			}
+
+		} else {
+			// Stop fusion but do not declare it faulty
+			ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
+			stopMagFusion();
+		}
+
+	} else {
+		if (starting_conditions_passing) {
+
+			_control_status.flags.mag = true;
+
+			// activate fusion, reset mag states and initialize variance if first init or in flight reset
+			if (!_control_status.flags.yaw_align
+			    || wmm_updated
+			    || !_mag_decl_cov_reset
+			    || !_state.mag_I.longerThan(0.f)
+			    || (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
+			    || (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
+			   ) {
+				ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
+
+				bool reset_heading = !_control_status.flags.yaw_align;
+
+				resetMagStates(_mag_lpf.getState(), reset_heading);
+
+				if (reset_heading) {
+					_control_status.flags.yaw_align = true;
+				}
+
+			} else {
+				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+				fuseMag(mag_sample.mag, aid_src, false);
+			}
+
+			aid_src.time_last_fuse = _time_delayed_us;
+
+			_nb_mag_3d_reset_available = 2;
+		}
+	}
+}
+
+void Ekf::stopMagFusion()
+{
+	if (_control_status.flags.mag) {
+		ECL_INFO("stopping mag fusion");
+
+		_control_status.flags.mag = false;
+		_control_status.flags.mag_dec = false;
+
+		if (_control_status.flags.mag_3D) {
+			ECL_INFO("stopping mag 3D fusion");
+			_control_status.flags.mag_3D = false;
+		}
+
+		if (_control_status.flags.mag_hdg) {
+			ECL_INFO("stopping mag heading fusion");
+			_control_status.flags.mag_hdg = false;
+			_fault_status.flags.bad_hdg = false;
+		}
+
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+
+		_fault_status.flags.bad_mag_decl = false;
+	}
+}
@@ -39,13 +39,8 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>

-#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
-
 void Ekf::controlMagFusion()
 {
-	static constexpr const char *AID_SRC_NAME = "mag";
-	estimator_aid_source3d_s &aid_src = _aid_src_mag;
-
 	// reset the flight alignment flag so that the mag fields will be
 	//  re-initialised next time we achieve flight altitude
 	if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) {
@@ -71,7 +66,7 @@ void Ekf::controlMagFusion()
 	if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {

 		if (mag_sample.reset || (_mag_counter == 0)) {
-			// sensor or calibration has changed, reset low pass filter
+			// sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion)
 			_control_status.flags.mag_fault = false;

 			_state.mag_B.zero();
@@ -85,6 +80,13 @@ void Ekf::controlMagFusion()
 			_mag_counter++;
 		}

+		const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
+				&& checkMagField(mag_sample.mag)
+				&& (_mag_counter > 5) // wait until we have more than a few samples through the filter
+				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
+				&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
+				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
+
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
@@ -101,202 +103,8 @@ void Ekf::controlMagFusion()
 			_control_status.flags.synthetic_mag_z = false;
 		}

-		// reset flags
-		_fault_status.flags.bad_mag_x = false;
-		_fault_status.flags.bad_mag_y = false;
-		_fault_status.flags.bad_mag_z = false;
-
-
-		resetEstimatorAidStatus(aid_src);
-		aid_src.timestamp_sample = mag_sample.time_us;
-
-		// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
-		const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
-
-		// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
-		Vector3f mag_innov;
-		Vector3f innov_var;
-
-		// Observation jacobian and Kalman gain vectors
-		VectorState H;
-		sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
-
-		for (int i = 0; i < 3; i++) {
-			aid_src.observation[i] = mag_sample.mag(i);
-			aid_src.observation_variance[i] = R_MAG;
-			aid_src.innovation[i] = mag_innov(i);
-			aid_src.innovation_variance[i] = innov_var(i);
-		}
-
-		const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
-		setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-		// Perform an innovation consistency check and report the result
-		_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);
-		_innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f);
-		_innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f);
-
-		// determine if we should use mag fusion
-		bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE)
-						     && _control_status.flags.tilt_align
-						     && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-						     && mag_sample.mag.longerThan(0.f)
-						     && mag_sample.mag.isAllFinite();
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& checkMagField(mag_sample.mag)
-				&& (_mag_counter > 3) // wait until we have more than a few samples through the filter
-				&& (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame
-				&& (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset
-				&& isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL);
-
 		checkMagHeadingConsistency(mag_sample);
-
-		// WMM update can occur on the last epoch, just after mag fusion
-		const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
-
-		{
-		const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps;
-		const bool common_conditions_passing = _control_status.flags.mag
-						       && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss)
-						           || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align))
-						       && !_control_status.flags.mag_fault
-						       && !_control_status.flags.mag_field_disturbed
-						       && !_control_status.flags.ev_yaw
-						       && !_control_status.flags.gps_yaw;
-
-		_control_status.flags.mag_3D = common_conditions_passing
-					       && (_params.mag_fusion_type == MagFuseType::AUTO)
-					       && _control_status.flags.mag_aligned_in_flight;
-
-		_control_status.flags.mag_hdg = common_conditions_passing
-						&& ((_params.mag_fusion_type == MagFuseType::HEADING)
-						    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));
-		}
-
-		// TODO: allow clearing mag_fault if mag_3d is good?
-
-		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
-			ECL_INFO("starting mag 3D fusion");
-
-		} else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) {
-			ECL_INFO("stopping mag 3D fusion");
-		}
-
-		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
-		// then the declination must be fused as an observation to prevent long term heading drift
-		// fusing declination when gps aiding is available is optional.
-		const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
-		_control_status.flags.mag_dec = _control_status.flags.mag
-						&& (not_using_ne_aiding || !_control_status.flags.mag_aligned_in_flight);
-
-		if (_control_status.flags.mag) {
-
-			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
-
-				if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) {
-					ECL_INFO("reset to %s", AID_SRC_NAME);
-					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-					aid_src.time_last_fuse = _time_delayed_us;
-
-				} else {
-					if (!_mag_decl_cov_reset) {
-						// After any magnetic field covariance reset event the earth field state
-						// covariances need to be corrected to incorporate knowledge of the declination
-						// before fusing magnetometer data to prevent rapid rotation of the earth field
-						// states for the first few observations.
-						fuseDeclination(0.02f);
-						_mag_decl_cov_reset = true;
-						fuseMag(mag_sample.mag, R_MAG, H, aid_src);
-
-					} else {
-						// The normal sequence is to fuse the magnetometer data first before fusing
-						// declination angle at a higher uncertainty to allow some learning of
-						// declination angle over time.
-						const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg;
-						const bool update_tilt = _control_status.flags.mag_3D;
-						fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt);
-
-						// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
-						if (update_all_states && update_tilt) {
-							_fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]);
-							_fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]);
-							_fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]);
-						}
-
-						if (_control_status.flags.mag_dec) {
-							fuseDeclination(0.5f);
-						}
-					}
-				}
-
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					if (_nb_mag_3d_reset_available > 0) {
-						// Data seems good, attempt a reset (mag states only unless mag_3D currently active)
-						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
-						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-						aid_src.time_last_fuse = _time_delayed_us;
-
-						if (_control_status.flags.in_air) {
-							_nb_mag_3d_reset_available--;
-						}
-
-					} else if (starting_conditions_passing) {
-						// Data seems good, but previous reset did not fix the issue
-						// something else must be wrong, declare the sensor faulty and stop the fusion
-						//_control_status.flags.mag_fault = true;
-						ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME);
-						stopMagFusion();
-
-					} else {
-						// A reset did not fix the issue but all the starting checks are not passing
-						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
-						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
-						stopMagFusion();
-					}
-				}
-
-			} else {
-				// Stop fusion but do not declare it faulty
-				ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME);
-				stopMagFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-
-				_control_status.flags.mag = true;
-
-				// activate fusion, reset mag states and initialize variance if first init or in flight reset
-				if (!_control_status.flags.yaw_align
-				    || wmm_updated
-				    || !_mag_decl_cov_reset
-				    || !_state.mag_I.longerThan(0.f)
-				    || (getStateVariance<State::mag_I>().min() < kMagVarianceMin)
-				    || (getStateVariance<State::mag_B>().min() < kMagVarianceMin)
-				   ) {
-					ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME);
-
-					bool reset_heading = !_control_status.flags.yaw_align;
-
-					resetMagStates(_mag_lpf.getState(), reset_heading);
-					aid_src.time_last_fuse = _time_delayed_us;
-					_nb_mag_3d_reset_available = 2;
-
-					if (reset_heading) {
-						_control_status.flags.yaw_align = true;
-					}
-
-				} else {
-					if (fuseMag(mag_sample.mag, R_MAG, H, aid_src)) {
-						ECL_INFO("starting %s fusion", AID_SRC_NAME);
-						_nb_mag_3d_reset_available = 2;
-					}
-				}
-			}
-		}
+		controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag);

 	} else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
@@ -304,37 +112,9 @@ void Ekf::controlMagFusion()
 	}
 }

-void Ekf::stopMagFusion()
-{
-	if (_control_status.flags.mag) {
-		ECL_INFO("stopping mag fusion");
-
-		_control_status.flags.mag = false;
-		_control_status.flags.mag_dec = false;
-
-		if (_control_status.flags.mag_3D) {
-			ECL_INFO("stopping mag 3D fusion");
-			_control_status.flags.mag_3D = false;
-		}
-
-		if (_control_status.flags.mag_hdg) {
-			ECL_INFO("stopping mag heading fusion");
-			_control_status.flags.mag_hdg = false;
-			_fault_status.flags.bad_hdg = false;
-		}
-
-		_fault_status.flags.bad_mag_x = false;
-		_fault_status.flags.bad_mag_y = false;
-		_fault_status.flags.bad_mag_z = false;
-
-		_fault_status.flags.bad_mag_decl = false;
-	}
-}
-
 bool Ekf::checkHaglYawResetReq() const
 {
 #if defined(CONFIG_EKF2_TERRAIN)
-
 	// We need to reset the yaw angle after climbing away from the ground to enable
 	// recovery from ground level magnetic interference.
 	if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) {
@@ -344,7 +124,6 @@ bool Ekf::checkHaglYawResetReq() const
 		const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl;
 		return above_mag_anomalies;
 	}
-
 #endif // CONFIG_EKF2_TERRAIN

 	return false;
@@ -368,7 +147,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)

 		// mag_B: reset
 #if defined(CONFIG_EKF2_GNSS)
-
 		if (isYawEmergencyEstimateAvailable()) {

 			const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth);
@@ -381,7 +159,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)

 		} else if (!reset_heading && _control_status.flags.yaw_align) {
 #else
-
 		if (!reset_heading && _control_status.flags.yaw_align) {
 #endif
 			// mag_B: reset using WMM
@@ -43,6 +43,7 @@

 #include "ekf.h"

+#include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 #include <ekf_derivation/generated/compute_mag_y_innov_var_and_h.h>
 #include <ekf_derivation/generated/compute_mag_z_innov_var_and_h.h>

@@ -50,14 +51,71 @@

 #include <mathlib/mathlib.h>

-bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, bool update_all_states, bool update_tilt)
+bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states, bool update_tilt)
 {
-	// if any axis failed, abort the mag fusion
-	if (aid_src.innovation_rejected) {
+	// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
+	const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
+
+	// calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains
+	Vector3f mag_innov;
+	Vector3f innov_var;
+
+	// Observation jacobian and Kalman gain vectors
+	VectorState H;
+	const auto state_vector = _state.vector();
+	sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
+
+	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
+	if (_control_status.flags.synthetic_mag_z) {
+		mag_innov(2) = 0.0f;
+	}
+
+	for (int i = 0; i < 3; i++) {
+		aid_src_mag.observation[i] = mag(i) - _state.mag_B(i);
+		aid_src_mag.observation_variance[i] = R_MAG;
+		aid_src_mag.innovation[i] = mag_innov(i);
+		aid_src_mag.innovation_variance[i] = innov_var(i);
+	}
+
+	const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
+	setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate);
+
+	if (update_all_states) {
+		_fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]);
+		_fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]);
+		_fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]);
+
+	} else {
+		_fault_status.flags.bad_mag_x = false;
+		_fault_status.flags.bad_mag_y = false;
+		_fault_status.flags.bad_mag_z = false;
+	}
+
+	// Perform an innovation consistency check and report the result
+	_innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f);
+	_innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f);
+
+	const char *numerical_error_covariance_reset_string = "numerical error - covariance reset";
+
+	// check innovation variances for being badly conditioned
+	if (innov_var.min() < R_MAG) {
+		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+		// we need to re-initialise covariances and abort this fusion step
+		if (update_all_states) {
+			resetQuatCov(_params.mag_heading_noise);
+		}
+
+		resetMagCov();
+
+		ECL_ERR("mag %s", numerical_error_covariance_reset_string);
 		return false;
 	}

-	const auto state_vector = _state.vector();
+	// if any axis fails, abort the mag fusion
+	if (aid_src_mag.innovation_rejected) {
+		return false;
+	}

 	bool fused[3] {false, false, false};

@@ -65,14 +123,29 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	for (uint8_t index = 0; index <= 2; index++) {
 		// Calculate Kalman gains and observation jacobians
 		if (index == 0) {
-			// everything was already computed
+			// everything was already computed above

 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
+			sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);

 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
+			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
+
+			if (aid_src_mag.innovation_variance[index] < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_y = true;
+
+				// we need to re-initialise covariances and abort this fusion step
+				if (update_all_states) {
+					resetQuatCov(_params.mag_heading_noise);
+				}
+
+				resetMagCov();
+
+				ECL_ERR("magY %s", numerical_error_covariance_reset_string);
+				return false;
+			}

 		} else if (index == 2) {
 			// we do not fuse synthesized magnetomter measurements when doing 3D fusion
@@ -82,26 +155,28 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 			}

 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
-			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H);
+			sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H);

 			// recalculate innovation using the updated state
-			aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);
-		}
+			aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index);

-		if (aid_src.innovation_variance[index] < R_MAG) {
-			ECL_ERR("mag numerical error covariance reset");
+			if (aid_src_mag.innovation_variance[index] < R_MAG) {
+				// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
+				_fault_status.flags.bad_mag_z = true;

-			// we need to re-initialise covariances and abort this fusion step
-			if (update_all_states) {
-				resetQuatCov(_params.mag_heading_noise);
+				// we need to re-initialise covariances and abort this fusion step
+				if (update_all_states) {
+					resetQuatCov(_params.mag_heading_noise);
+				}
+
+				resetMagCov();
+
+				ECL_ERR("magZ %s", numerical_error_covariance_reset_string);
+				return false;
 			}
-
-			resetMagCov();
-
-			return false;
 		}

-		VectorState Kfusion = P * H / aid_src.innovation_variance[index];
+		VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index];

 		if (!update_all_states) {
 			// zero non-mag Kalman gains if not updating all states
@@ -117,13 +192,16 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 		}

 		if (!update_tilt) {
-			Kfusion(State::quat_nominal.idx + 0) = 0.f;
+			Kfusion(State::quat_nominal.idx) = 0.f;
 			Kfusion(State::quat_nominal.idx + 1) = 0.f;
 		}

-		if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) {
+		if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) {
 			fused[index] = true;
 			limitDeclination();
+
+		} else {
+			fused[index] = false;
 		}
 	}

@@ -134,8 +212,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	}

 	if (fused[0] && fused[1] && fused[2]) {
-		aid_src.fused = true;
-		aid_src.time_last_fuse = _time_delayed_us;
+		aid_src_mag.fused = true;
+		aid_src_mag.time_last_fuse = _time_delayed_us;

 		if (update_all_states) {
 			_time_last_heading_fuse = _time_delayed_us;
@@ -144,6 +222,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 		return true;
 	}

+	aid_src_mag.fused = false;
 	return false;
 }

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022-2024 PX4. All rights reserved.
+ *   Copyright (c) 2022 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022-2024 PX4. All rights reserved.
+ *   Copyright (c) 2022 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -36,7 +36,8 @@

 #include <matrix/math.hpp>

-#include "../RingBuffer.h"
+#include "common.h"
+#include "RingBuffer.h"

 #include <lib/geo/geo.h>

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-add_library(output_predictor
-	output_predictor.cpp
-	output_predictor.h
-)
-
-add_dependencies(output_predictor prebuild_targets)
@@ -39,6 +39,7 @@
 #define EKF_POSITION_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
+#include "common.h"

 class PositionBiasEstimator
 {
@@ -37,12 +37,7 @@ import symforce
 symforce.set_epsilon_to_symbol()

 import symforce.symbolic as sf
-
-# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils
-import os, sys
-derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils"
-sys.path.append(derivation_utils_dir)
-import derivation_utils
+from utils.derivation_utils import *

 def predict_opt_flow(
        terrain_vpos: sf.Scalar,
@@ -54,7 +49,7 @@ def predict_opt_flow(
    R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix()
    flow_pred = sf.V2()
    dist = - (pos_z - terrain_vpos)
-    dist = derivation_utils.add_epsilon_sign(dist, dist, epsilon)
+    dist = add_epsilon_sign(dist, dist, epsilon)
    flow_pred[0] = -v[1] / dist * R_to_earth[2, 2]
    flow_pred[1] = v[0] / dist * R_to_earth[2, 2]
    return flow_pred
@@ -95,5 +90,5 @@ def terr_est_compute_flow_y_innov_var_and_h(
    return (innov_var, Hy[0, 0])

 print("Derive terrain estimator equations...")
-derivation_utils.generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
-derivation_utils.generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
+generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
+generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
@@ -236,16 +236,57 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  // Output terms (1)
  matrix::Matrix<Scalar, 23, 23> _res;

-  _res.setZero();
-
  _res(0, 0) = std::pow(_tmp15, Scalar(2)) * gyro_var + _tmp15 * _tmp27 + _tmp18 * _tmp26 +
               std::pow(_tmp23, Scalar(2)) * gyro_var + _tmp23 * _tmp25 + _tmp24 * _tmp6 +
               std::pow(_tmp6, Scalar(2)) * gyro_var;
+  _res(1, 0) = 0;
+  _res(2, 0) = 0;
+  _res(3, 0) = 0;
+  _res(4, 0) = 0;
+  _res(5, 0) = 0;
+  _res(6, 0) = 0;
+  _res(7, 0) = 0;
+  _res(8, 0) = 0;
+  _res(9, 0) = 0;
+  _res(10, 0) = 0;
+  _res(11, 0) = 0;
+  _res(12, 0) = 0;
+  _res(13, 0) = 0;
+  _res(14, 0) = 0;
+  _res(15, 0) = 0;
+  _res(16, 0) = 0;
+  _res(17, 0) = 0;
+  _res(18, 0) = 0;
+  _res(19, 0) = 0;
+  _res(20, 0) = 0;
+  _res(21, 0) = 0;
+  _res(22, 0) = 0;
  _res(0, 1) = _tmp15 * _tmp38 + _tmp18 * _tmp35 + _tmp24 * _tmp34 + _tmp25 * _tmp29 +
               _tmp27 * _tmp36 + _tmp29 * _tmp37 + _tmp34 * _tmp39;
  _res(1, 1) = _tmp18 * _tmp43 + std::pow(_tmp29, Scalar(2)) * gyro_var + _tmp29 * _tmp40 +
               std::pow(_tmp34, Scalar(2)) * gyro_var + _tmp34 * _tmp41 +
               std::pow(_tmp36, Scalar(2)) * gyro_var + _tmp36 * _tmp42;
+  _res(2, 1) = 0;
+  _res(3, 1) = 0;
+  _res(4, 1) = 0;
+  _res(5, 1) = 0;
+  _res(6, 1) = 0;
+  _res(7, 1) = 0;
+  _res(8, 1) = 0;
+  _res(9, 1) = 0;
+  _res(10, 1) = 0;
+  _res(11, 1) = 0;
+  _res(12, 1) = 0;
+  _res(13, 1) = 0;
+  _res(14, 1) = 0;
+  _res(15, 1) = 0;
+  _res(16, 1) = 0;
+  _res(17, 1) = 0;
+  _res(18, 1) = 0;
+  _res(19, 1) = 0;
+  _res(20, 1) = 0;
+  _res(21, 1) = 0;
+  _res(22, 1) = 0;
  _res(0, 2) = _tmp15 * _tmp47 * gyro_var + _tmp18 * _tmp46 + _tmp24 * _tmp44 + _tmp25 * _tmp45 +
               _tmp27 * _tmp47 + _tmp37 * _tmp45 + _tmp39 * _tmp44;
  _res(1, 2) = _tmp18 * _tmp48 + _tmp29 * _tmp45 * gyro_var + _tmp34 * _tmp44 * gyro_var +
@@ -253,6 +294,26 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(2, 2) = _tmp18 * _tmp51 + std::pow(_tmp44, Scalar(2)) * gyro_var + _tmp44 * _tmp50 +
               std::pow(_tmp45, Scalar(2)) * gyro_var + _tmp45 * _tmp49 +
               std::pow(_tmp47, Scalar(2)) * gyro_var + _tmp47 * _tmp52;
+  _res(3, 2) = 0;
+  _res(4, 2) = 0;
+  _res(5, 2) = 0;
+  _res(6, 2) = 0;
+  _res(7, 2) = 0;
+  _res(8, 2) = 0;
+  _res(9, 2) = 0;
+  _res(10, 2) = 0;
+  _res(11, 2) = 0;
+  _res(12, 2) = 0;
+  _res(13, 2) = 0;
+  _res(14, 2) = 0;
+  _res(15, 2) = 0;
+  _res(16, 2) = 0;
+  _res(17, 2) = 0;
+  _res(18, 2) = 0;
+  _res(19, 2) = 0;
+  _res(20, 2) = 0;
+  _res(21, 2) = 0;
+  _res(22, 2) = 0;
  _res(0, 3) = _tmp35 * _tmp75 + _tmp46 * _tmp79 - _tmp53 * _tmp57 - _tmp58 * _tmp61 -
               _tmp62 * _tmp64 + _tmp80;
  _res(1, 3) = _tmp43 * _tmp75 + _tmp48 * _tmp79 - _tmp57 * _tmp81 - _tmp61 * _tmp82 -
@@ -263,6 +324,25 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
               std::pow(_tmp60, Scalar(2)) * _tmp97 - _tmp61 * _tmp93 +
               std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 +
               _tmp79 * _tmp92 + _tmp99;
+  _res(4, 3) = 0;
+  _res(5, 3) = 0;
+  _res(6, 3) = 0;
+  _res(7, 3) = 0;
+  _res(8, 3) = 0;
+  _res(9, 3) = 0;
+  _res(10, 3) = 0;
+  _res(11, 3) = 0;
+  _res(12, 3) = 0;
+  _res(13, 3) = 0;
+  _res(14, 3) = 0;
+  _res(15, 3) = 0;
+  _res(16, 3) = 0;
+  _res(17, 3) = 0;
+  _res(18, 3) = 0;
+  _res(19, 3) = 0;
+  _res(20, 3) = 0;
+  _res(21, 3) = 0;
+  _res(22, 3) = 0;
  _res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 +
               _tmp113 * _tmp114 + _tmp115;
  _res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 +
@@ -278,6 +358,24 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
               std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp128 +
               std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp129 + _tmp117 * _tmp126 +
               _tmp130;
+  _res(5, 4) = 0;
+  _res(6, 4) = 0;
+  _res(7, 4) = 0;
+  _res(8, 4) = 0;
+  _res(9, 4) = 0;
+  _res(10, 4) = 0;
+  _res(11, 4) = 0;
+  _res(12, 4) = 0;
+  _res(13, 4) = 0;
+  _res(14, 4) = 0;
+  _res(15, 4) = 0;
+  _res(16, 4) = 0;
+  _res(17, 4) = 0;
+  _res(18, 4) = 0;
+  _res(19, 4) = 0;
+  _res(20, 4) = 0;
+  _res(21, 4) = 0;
+  _res(22, 4) = 0;
  _res(0, 5) = _tmp114 * _tmp136 - _tmp132 * _tmp62 + _tmp133 * _tmp35 - _tmp134 * _tmp58 -
               _tmp135 * _tmp53 + _tmp137;
  _res(1, 5) = _tmp116 * _tmp138 - _tmp132 * _tmp83 + _tmp133 * _tmp43 - _tmp134 * _tmp82 -
@@ -299,6 +397,23 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                          P(13, 0) * _tmp134 - P(14, 0) * _tmp132 + P(5, 0)) +
               _tmp146 + std::pow(_tmp66, Scalar(2)) * _tmp96 +
               std::pow(_tmp73, Scalar(2)) * _tmp97;
+  _res(6, 5) = 0;
+  _res(7, 5) = 0;
+  _res(8, 5) = 0;
+  _res(9, 5) = 0;
+  _res(10, 5) = 0;
+  _res(11, 5) = 0;
+  _res(12, 5) = 0;
+  _res(13, 5) = 0;
+  _res(14, 5) = 0;
+  _res(15, 5) = 0;
+  _res(16, 5) = 0;
+  _res(17, 5) = 0;
+  _res(18, 5) = 0;
+  _res(19, 5) = 0;
+  _res(20, 5) = 0;
+  _res(21, 5) = 0;
+  _res(22, 5) = 0;
  _res(0, 6) =
      P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt;
  _res(1, 6) =
@@ -316,6 +431,22 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
               dt * (P(0, 3) * _tmp138 + P(1, 3) * _tmp133 - P(12, 3) * _tmp135 -
                     P(13, 3) * _tmp134 - P(14, 3) * _tmp132 + P(5, 3));
  _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3));
+  _res(7, 6) = 0;
+  _res(8, 6) = 0;
+  _res(9, 6) = 0;
+  _res(10, 6) = 0;
+  _res(11, 6) = 0;
+  _res(12, 6) = 0;
+  _res(13, 6) = 0;
+  _res(14, 6) = 0;
+  _res(15, 6) = 0;
+  _res(16, 6) = 0;
+  _res(17, 6) = 0;
+  _res(18, 6) = 0;
+  _res(19, 6) = 0;
+  _res(20, 6) = 0;
+  _res(21, 6) = 0;
+  _res(22, 6) = 0;
  _res(0, 7) =
      P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp115 * dt;
  _res(1, 7) =
@@ -332,6 +463,21 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
                     P(13, 4) * _tmp134 - P(14, 4) * _tmp132 + P(5, 4));
  _res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4));
  _res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4));
+  _res(8, 7) = 0;
+  _res(9, 7) = 0;
+  _res(10, 7) = 0;
+  _res(11, 7) = 0;
+  _res(12, 7) = 0;
+  _res(13, 7) = 0;
+  _res(14, 7) = 0;
+  _res(15, 7) = 0;
+  _res(16, 7) = 0;
+  _res(17, 7) = 0;
+  _res(18, 7) = 0;
+  _res(19, 7) = 0;
+  _res(20, 7) = 0;
+  _res(21, 7) = 0;
+  _res(22, 7) = 0;
  _res(0, 8) =
      P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp137 * dt;
  _res(1, 8) =
@@ -347,6 +493,20 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5));
  _res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5));
  _res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5));
+  _res(9, 8) = 0;
+  _res(10, 8) = 0;
+  _res(11, 8) = 0;
+  _res(12, 8) = 0;
+  _res(13, 8) = 0;
+  _res(14, 8) = 0;
+  _res(15, 8) = 0;
+  _res(16, 8) = 0;
+  _res(17, 8) = 0;
+  _res(18, 8) = 0;
+  _res(19, 8) = 0;
+  _res(20, 8) = 0;
+  _res(21, 8) = 0;
+  _res(22, 8) = 0;
  _res(0, 9) = _tmp27;
  _res(1, 9) = _tmp42;
  _res(2, 9) = _tmp52;
@@ -360,6 +520,19 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(7, 9) = P(4, 9) * dt + P(7, 9);
  _res(8, 9) = P(5, 9) * dt + P(8, 9);
  _res(9, 9) = P(9, 9);
+  _res(10, 9) = 0;
+  _res(11, 9) = 0;
+  _res(12, 9) = 0;
+  _res(13, 9) = 0;
+  _res(14, 9) = 0;
+  _res(15, 9) = 0;
+  _res(16, 9) = 0;
+  _res(17, 9) = 0;
+  _res(18, 9) = 0;
+  _res(19, 9) = 0;
+  _res(20, 9) = 0;
+  _res(21, 9) = 0;
+  _res(22, 9) = 0;
  _res(0, 10) = _tmp25;
  _res(1, 10) = _tmp40;
  _res(2, 10) = _tmp49;
@@ -374,6 +547,18 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(8, 10) = P(5, 10) * dt + P(8, 10);
  _res(9, 10) = P(9, 10);
  _res(10, 10) = P(10, 10);
+  _res(11, 10) = 0;
+  _res(12, 10) = 0;
+  _res(13, 10) = 0;
+  _res(14, 10) = 0;
+  _res(15, 10) = 0;
+  _res(16, 10) = 0;
+  _res(17, 10) = 0;
+  _res(18, 10) = 0;
+  _res(19, 10) = 0;
+  _res(20, 10) = 0;
+  _res(21, 10) = 0;
+  _res(22, 10) = 0;
  _res(0, 11) = _tmp24;
  _res(1, 11) = _tmp41;
  _res(2, 11) = _tmp50;
@@ -389,6 +574,17 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(9, 11) = P(9, 11);
  _res(10, 11) = P(10, 11);
  _res(11, 11) = P(11, 11);
+  _res(12, 11) = 0;
+  _res(13, 11) = 0;
+  _res(14, 11) = 0;
+  _res(15, 11) = 0;
+  _res(16, 11) = 0;
+  _res(17, 11) = 0;
+  _res(18, 11) = 0;
+  _res(19, 11) = 0;
+  _res(20, 11) = 0;
+  _res(21, 11) = 0;
+  _res(22, 11) = 0;
  _res(0, 12) = _tmp53;
  _res(1, 12) = _tmp81;
  _res(2, 12) = _tmp85;
@@ -402,6 +598,16 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(10, 12) = P(10, 12);
  _res(11, 12) = P(11, 12);
  _res(12, 12) = P(12, 12);
+  _res(13, 12) = 0;
+  _res(14, 12) = 0;
+  _res(15, 12) = 0;
+  _res(16, 12) = 0;
+  _res(17, 12) = 0;
+  _res(18, 12) = 0;
+  _res(19, 12) = 0;
+  _res(20, 12) = 0;
+  _res(21, 12) = 0;
+  _res(22, 12) = 0;
  _res(0, 13) = _tmp58;
  _res(1, 13) = _tmp82;
  _res(2, 13) = _tmp87;
@@ -416,6 +622,15 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(11, 13) = P(11, 13);
  _res(12, 13) = P(12, 13);
  _res(13, 13) = P(13, 13);
+  _res(14, 13) = 0;
+  _res(15, 13) = 0;
+  _res(16, 13) = 0;
+  _res(17, 13) = 0;
+  _res(18, 13) = 0;
+  _res(19, 13) = 0;
+  _res(20, 13) = 0;
+  _res(21, 13) = 0;
+  _res(22, 13) = 0;
  _res(0, 14) = _tmp62;
  _res(1, 14) = _tmp83;
  _res(2, 14) = _tmp88;
@@ -431,6 +646,14 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(12, 14) = P(12, 14);
  _res(13, 14) = P(13, 14);
  _res(14, 14) = P(14, 14);
+  _res(15, 14) = 0;
+  _res(16, 14) = 0;
+  _res(17, 14) = 0;
+  _res(18, 14) = 0;
+  _res(19, 14) = 0;
+  _res(20, 14) = 0;
+  _res(21, 14) = 0;
+  _res(22, 14) = 0;
  _res(0, 15) = P(0, 15) * _tmp18 + P(10, 15) * _tmp23 + P(11, 15) * _tmp6 + P(9, 15) * _tmp15;
  _res(1, 15) = P(1, 15) * _tmp18 + P(10, 15) * _tmp29 + P(11, 15) * _tmp34 + P(9, 15) * _tmp36;
  _res(2, 15) = P(10, 15) * _tmp45 + P(11, 15) * _tmp44 + P(2, 15) * _tmp18 + P(9, 15) * _tmp47;
@@ -450,6 +673,13 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(13, 15) = P(13, 15);
  _res(14, 15) = P(14, 15);
  _res(15, 15) = P(15, 15);
+  _res(16, 15) = 0;
+  _res(17, 15) = 0;
+  _res(18, 15) = 0;
+  _res(19, 15) = 0;
+  _res(20, 15) = 0;
+  _res(21, 15) = 0;
+  _res(22, 15) = 0;
  _res(0, 16) = P(0, 16) * _tmp18 + P(10, 16) * _tmp23 + P(11, 16) * _tmp6 + P(9, 16) * _tmp15;
  _res(1, 16) = P(1, 16) * _tmp18 + P(10, 16) * _tmp29 + P(11, 16) * _tmp34 + P(9, 16) * _tmp36;
  _res(2, 16) = P(10, 16) * _tmp45 + P(11, 16) * _tmp44 + P(2, 16) * _tmp18 + P(9, 16) * _tmp47;
@@ -470,6 +700,12 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(14, 16) = P(14, 16);
  _res(15, 16) = P(15, 16);
  _res(16, 16) = P(16, 16);
+  _res(17, 16) = 0;
+  _res(18, 16) = 0;
+  _res(19, 16) = 0;
+  _res(20, 16) = 0;
+  _res(21, 16) = 0;
+  _res(22, 16) = 0;
  _res(0, 17) = P(0, 17) * _tmp18 + P(10, 17) * _tmp23 + P(11, 17) * _tmp6 + P(9, 17) * _tmp15;
  _res(1, 17) = P(1, 17) * _tmp18 + P(10, 17) * _tmp29 + P(11, 17) * _tmp34 + P(9, 17) * _tmp36;
  _res(2, 17) = P(10, 17) * _tmp45 + P(11, 17) * _tmp44 + P(2, 17) * _tmp18 + P(9, 17) * _tmp47;
@@ -491,6 +727,11 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(15, 17) = P(15, 17);
  _res(16, 17) = P(16, 17);
  _res(17, 17) = P(17, 17);
+  _res(18, 17) = 0;
+  _res(19, 17) = 0;
+  _res(20, 17) = 0;
+  _res(21, 17) = 0;
+  _res(22, 17) = 0;
  _res(0, 18) = P(0, 18) * _tmp18 + P(10, 18) * _tmp23 + P(11, 18) * _tmp6 + P(9, 18) * _tmp15;
  _res(1, 18) = P(1, 18) * _tmp18 + P(10, 18) * _tmp29 + P(11, 18) * _tmp34 + P(9, 18) * _tmp36;
  _res(2, 18) = P(10, 18) * _tmp45 + P(11, 18) * _tmp44 + P(2, 18) * _tmp18 + P(9, 18) * _tmp47;
@@ -513,6 +754,10 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(16, 18) = P(16, 18);
  _res(17, 18) = P(17, 18);
  _res(18, 18) = P(18, 18);
+  _res(19, 18) = 0;
+  _res(20, 18) = 0;
+  _res(21, 18) = 0;
+  _res(22, 18) = 0;
  _res(0, 19) = P(0, 19) * _tmp18 + P(10, 19) * _tmp23 + P(11, 19) * _tmp6 + P(9, 19) * _tmp15;
  _res(1, 19) = P(1, 19) * _tmp18 + P(10, 19) * _tmp29 + P(11, 19) * _tmp34 + P(9, 19) * _tmp36;
  _res(2, 19) = P(10, 19) * _tmp45 + P(11, 19) * _tmp44 + P(2, 19) * _tmp18 + P(9, 19) * _tmp47;
@@ -536,6 +781,9 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(17, 19) = P(17, 19);
  _res(18, 19) = P(18, 19);
  _res(19, 19) = P(19, 19);
+  _res(20, 19) = 0;
+  _res(21, 19) = 0;
+  _res(22, 19) = 0;
  _res(0, 20) = P(0, 20) * _tmp18 + P(10, 20) * _tmp23 + P(11, 20) * _tmp6 + P(9, 20) * _tmp15;
  _res(1, 20) = P(1, 20) * _tmp18 + P(10, 20) * _tmp29 + P(11, 20) * _tmp34 + P(9, 20) * _tmp36;
  _res(2, 20) = P(10, 20) * _tmp45 + P(11, 20) * _tmp44 + P(2, 20) * _tmp18 + P(9, 20) * _tmp47;
@@ -560,6 +808,8 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(18, 20) = P(18, 20);
  _res(19, 20) = P(19, 20);
  _res(20, 20) = P(20, 20);
+  _res(21, 20) = 0;
+  _res(22, 20) = 0;
  _res(0, 21) = P(0, 21) * _tmp18 + P(10, 21) * _tmp23 + P(11, 21) * _tmp6 + P(9, 21) * _tmp15;
  _res(1, 21) = P(1, 21) * _tmp18 + P(10, 21) * _tmp29 + P(11, 21) * _tmp34 + P(9, 21) * _tmp36;
  _res(2, 21) = P(10, 21) * _tmp45 + P(11, 21) * _tmp44 + P(2, 21) * _tmp18 + P(9, 21) * _tmp47;
@@ -585,6 +835,7 @@ matrix::Matrix<Scalar, 23, 23> PredictCovariance(const matrix::Matrix<Scalar, 24
  _res(19, 21) = P(19, 21);
  _res(20, 21) = P(20, 21);
  _res(21, 21) = P(21, 21);
+  _res(22, 21) = 0;
  _res(0, 22) = P(0, 22) * _tmp18 + P(10, 22) * _tmp23 + P(11, 22) * _tmp6 + P(9, 22) * _tmp15;
  _res(1, 22) = P(1, 22) * _tmp18 + P(10, 22) * _tmp29 + P(11, 22) * _tmp34 + P(9, 22) * _tmp36;
  _res(2, 22) = P(10, 22) * _tmp45 + P(11, 22) * _tmp44 + P(2, 22) * _tmp18 + P(9, 22) * _tmp47;
@@ -56,7 +56,7 @@ def generate_px4_function(function_name, output_names):
    codegen = Codegen.function(
            function_name,
            output_names=output_names,
-            config=CppConfig(zero_initialization_sparsity_threshold=1))
+            config=CppConfig())
    metadata = codegen.generate_function(
            output_dir="generated",
            skip_directory_nesting=True)
@@ -35,10 +35,9 @@
 * @file range_finder_consistency_check.cpp
 */

-#include <aid_sources/range_finder/range_finder_consistency_check.hpp>
+#include "range_finder_consistency_check.hpp"

-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
-		bool horizontal_motion, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
 {
 	if (horizontal_motion) {
 		_time_last_horizontal_motion = time_us;
@@ -56,8 +55,7 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
 	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down

-	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
-	const float var = 2.f * dist_bottom_var / (dt * dt);
+	const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
 	_innov_var = var + vz_var;

 	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
@@ -86,9 +84,8 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 		}

 	} else {
-		if ((_test_ratio < 1.f)
-		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
-		   ) {
+		if (_test_ratio < 1.f
+		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
 			_is_kinematically_consistent = true;
 		}
 	}
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Matthias Grob	1d5d96e8f9	mavlink: return unsupported frame result when that's the case	2024-05-02 17:32:37 +02:00
Matthias Grob	3b21f3999f	mavlink: spacing, naming of command frame handling	2024-05-02 17:30:59 +02:00
Matthias Grob	6dbf39be7c	Update MAVLink to contain _INT frame deprecation warning	2024-05-02 17:19:31 +02:00
Daniel Agar	3bd14fcb16	mavlink: COMMAND_INT receiver reject unsupported MAV_FRAMEs	2024-05-02 16:49:37 +02:00
Daniel Agar	ed35155b50	vehicle_command: add initial frame support (GLOBAL & GLOBAL_RELATIVE_ALT)	2024-05-02 16:49:37 +02:00