ekf2: stop gps yaw fusion on numerical error

ekf2: stop airspeed fusion on numerical error
ekf2: stop flow fusion on numerical error
2026-05-27 20:20:04 +08:00 · 2024-07-11 12:25:14 -04:00 · 2024-07-11 12:23:15 -04:00 · 2024-07-11 12:22:26 -04:00 · 2024-07-11 12:19:50 -04:00 · 2024-07-11 14:54:22 +02:00
209 changed files with 6234 additions and 4050 deletions
@@ -11,6 +11,8 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    env:
+      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -11,6 +11,8 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    env:
+      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -0,0 +1,47 @@
+#!/bin/sh
+# @name Rover Ackermann
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Rover parameters
+param set-default NAV_ACC_RAD 0.5
+param set-default RA_ACC_RAD_GAIN 2
+param set-default RA_ACC_RAD_MAX 3
+param set-default RA_LOOKAHD_GAIN 1
+param set-default RA_LOOKAHD_MAX 10
+param set-default RA_LOOKAHD_MIN 1
+param set-default RA_MAX_ACCEL 1.5
+param set-default RA_MAX_JERK 15
+param set-default RA_MAX_SPEED 3
+param set-default RA_MAX_STR_ANG 0.5236
+param set-default RA_MAX_STR_RATE 360
+param set-default RA_MISS_VEL_DEF 3
+param set-default RA_MISS_VEL_GAIN 5
+param set-default RA_MISS_VEL_MIN 1
+param set-default RA_SPEED_I 0.01
+param set-default RA_SPEED_P 2
+param set-default RA_WHEEL_BASE 0.321
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+# Wheels
+param set-default SIM_GZ_WH_FUNC1 101
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+# Steering
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_REV 1
@@ -83,6 +83,7 @@ px4_add_romfs_files(
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
+	4012_gz_rover_ackermann

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -15,6 +15,9 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 	if [ -n "${PX4_HOME_LON}" ]; then
 		param set SIH_LOC_LON0 ${PX4_HOME_LON}
 	fi
+	if [ -n "${PX4_HOME_ALT}" ]; then
+		param set SIH_LOC_H0 ${PX4_HOME_ALT}
+	fi

 	if simulator_sih start; then

@@ -157,6 +157,7 @@ param set-default CBRK_SUPPLY_CHK 894281

 # disable check, no CPU load reported on posix yet
 param set-default COM_CPU_MAX -1
+param set-default COM_RAM_MAX -1

 # Don't require RC calibration and configuration
 param set-default COM_RC_IN_MODE 1
@@ -6,6 +6,7 @@
 # @class Copter
 #
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v5x exclude
 # @board bitcraze_crazyflie exclude
 #
 # @maintainer Iain Galloway <iain.galloway@nxp.com>
@@ -7,6 +7,10 @@
 #
 # @maintainer
 # @board px4_fmu-v2 exclude
+# @board cuav_x7pro exclude
+# @board px4_fmu-v4pro exclude
+# @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 #

 . ${R}etc/init.d/rc.mc_defaults
@@ -36,7 +40,6 @@ param set-default COM_RC_LOSS_T 3


 # ekf2
-param set-default EKF2_TERR_MASK 1
 param set-default EKF2_BARO_NOISE 2

 param set-default EKF2_BCOEF_X 31.5
@@ -12,6 +12,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board bitcraze_crazyflie exclude
 # @board cuav_x7pro exclude
 #
@@ -10,6 +10,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board bitcraze_crazyflie exclude
 # @board cuav_x7pro exclude
 #
@@ -8,6 +8,7 @@
 # @maintainer Oleg Kalachev <okalachev@gmail.com>
 #
 # @board px4_fmu-v2 exclude
+# @board px4_fmu-v4pro exclude
 # @board bitcraze_crazyflie exclude
 #

@@ -25,7 +25,6 @@
 param set-default BAT1_CAPACITY 4000
 param set-default BAT1_N_CELLS 6
 param set-default BAT1_V_EMPTY 3.3
-param set-default BAT1_V_LOAD_DROP 0.5
 param set-default BAT_AVRG_CURRENT 13

 # Square quadrotor X PX4 numbering
@@ -13,6 +13,7 @@
 # @board px4_fmu-v4pro exclude
 # @board px4_fmu-v5 exclude
 # @board px4_fmu-v5x exclude
+# @board px4_fmu-v6x exclude
 # @board diatone_mamba-f405-mk2 exclude
 #
 . ${R}etc/init.d/rc.mc_defaults
@@ -1,9 +1,6 @@
 #!/bin/sh
 # Standard apps for a ackermann drive rover.

-# Start the attitude and position estimator.
-ekf2 start &
-
 # Start rover ackermann drive controller.
 rover_ackermann start

@@ -217,25 +217,31 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS

-	#
-	# Set parameters and env variables for selected SYS_AUTOSTART.
-	#
-	set AUTOSTART_PATH etc/init.d/rc.autostart
+	# Load airframe configuration based on SYS_AUTOSTART parameter
 	if ! param compare SYS_AUTOSTART 0
 	then
-		if param greater SYS_AUTOSTART 1000000
+		# rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE
+		# Look for airframe in ROMFS
+		. ${R}etc/init.d/rc.autostart
+
+		if [ ${VEHICLE_TYPE} == none ]
 		then
-			# Use external startup file
+			# Look for airframe on SD card
 			if [ $SDCARD_AVAILABLE = yes ]
 			then
-				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
+				. ${R}etc/init.d/rc.autostart_ext
 			else
-				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
+				echo "ERROR [init] SD card not mounted - can't load external airframe"
 			fi
 		fi
-		. ${R}$AUTOSTART_PATH
+
+		if [ ${VEHICLE_TYPE} == none ]
+		then
+			echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
+			param set SYS_AUTOSTART 0
+			tune_control play error
+		fi
 	fi
-	unset AUTOSTART_PATH

 	# Check parameter version and reset upon airframe configuration version mismatch.
 	# Reboot required because "param reset_all" would reset all "param set" lines from airframe.
@@ -18,7 +18,8 @@ exec find boards msg src platforms test \
    -path src/lib/events/libevents -prune -o \
    -path src/lib/parameters/uthash -prune -o \
    -path src/lib/wind_estimator/python/generated -prune -o \
-    -path src/modules/ekf2/EKF -prune -o \
+    -path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \
+    -path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \
    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
    -path src/modules/mavlink/mavlink -prune -o \
    -path test/mavsdk_tests/catch2 -prune -o \
@@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str
        data_plot.save()
        data_plot.close()

-        # plot innovation_check_flags summary
+        # plot innovation flags summary
        data_plot = CheckFlagsPlot(
            status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos',
                                                                               'reject_hagl'],
@@ -79,12 +79,6 @@ class RCOutput():
        result += "then\n"
        result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n"
        result += "\t. /etc/init.d/airframes/${AIRFRAME}\n"
-        if not post_start:
-            result += "else\n"
-            result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n"
-            # Reset the configuration
-            result += "\tparam set SYS_AUTOSTART 0\n"
-            result += "\ttone_alarm ${TUNE_ERR}\n"
        result += "fi\n"
        result += "unset AIRFRAME"
        self.output = result
@@ -7,7 +7,7 @@ jinja2>=2.8
 jsonschema
 kconfiglib
 lxml
-matplotlib>=3.0.*
+matplotlib>=3.0
 numpy>=1.13
 nunavut>=1.1.0
 packaging
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y
+CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
@@ -0,0 +1 @@
+# Same as default, only defconfig is different
@@ -0,0 +1,257 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1016
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_WATCHDOG=y
@@ -0,0 +1 @@
+# Same as default, only defconfig is different
@@ -0,0 +1,259 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorangeplus/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H747XI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1058
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange+"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_PWR_DIRECT_SMPS_SUPPLY=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_WATCHDOG=y
@@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg)

 		hil_battery_status.timestamp = gyro_accel_time;
 		hil_battery_status.voltage_v = 16.0f;
-		hil_battery_status.voltage_filtered_v = 16.0f;
 		hil_battery_status.current_a = 10.0f;
 		hil_battery_status.discharged_mah = -1.0f;
 		hil_battery_status.connected = true;
@@ -55,7 +55,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -52,6 +52,7 @@
 #define IMXRT_IPG_PODF_DIVIDER 5
 #define BOARD_GPT_FREQUENCY  24000000
 #define BOARD_XTAL_FREQUENCY 24000000
+#define BOARD_FLEXIO_PREQ    108000000

 /* SDIO *********************************************************************/

@@ -154,7 +154,6 @@
 *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE)
 *(.text._ZN9Commander13dataLinkCheckEv)
 *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex)
-*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_)
 *(.text._ZN12PX4Gyroscope9set_scaleEf)
 *(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s)
 *(.text._ZN18MavlinkStreamDebug4sendEv)
@@ -114,11 +114,11 @@ const struct clock_configuration_s g_initial_clkconfig = {
 			.div           = 1,
 			.mux           = ACMP_CLK_ROOT_OSC_RC_48M_DIV2,
 		},
-		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
+		.flexio1_clk_root = /* 432 / 4 = 108Mhz */
 		{
 			.enable        = 1,
-			.div           = 2,
-			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2,
+			.div           = 4,
+			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3,
 		},
 		.flexio2_clk_root =
 		{
@@ -492,9 +492,9 @@ const struct clock_configuration_s g_initial_clkconfig = {
 		.mfd             = 268435455,
 		.ss_enable       = 0,
 		.pfd0            = 27, /* (528 * 18) / 27 = 352 MHz */
-		.pfd1            = 16, /* (528 * 16) / 16 = 594 MHz */
-		.pfd2            = 24, /* (528 * 24) / 27 = 396 MHz */
-		.pfd3            = 32, /* (528 * 32) / 27 = 297 MHz */
+		.pfd1            = 16, /* (528 * 18) / 16 = 594 MHz */
+		.pfd2            = 24, /* (528 * 18) / 24 = 396 MHz */
+		.pfd3            = 22, /* (528 * 18) / 22 = 216 MHz */
 	},
 	.sys_pll3 =
 	{
@@ -53,18 +53,12 @@ static const px4_mft_device_t i2c6 = {             // 24LC64T on BASE  8K 32 X 2

 static const px4_mtd_entry_t fmum_fram = {
 	.device = &qspi_flash,
-	.npart = 2,
+	.npart = 1,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 32
-		},
-		{
-			.type = MTD_WAYPOINTS,
-			.path = "/fs/mtd_waypoints",
-			.nblocks = 32
-
+			.nblocks = 256
 		}
 	},
 };
@@ -10,3 +10,7 @@ float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspe
 bool airspeed_sensor_measurement_valid 		# True if data from at least one airspeed sensor is declared valid.

 int8 selected_airspeed_index 			# 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
+
+float32 airspeed_derivative_filtered		# filtered indicated airspeed derivative [m/s/s]
+float32 throttle_filtered			# filtered fixed-wing throttle [-]
+float32 pitch_filtered				# filtered pitch [rad]
@@ -1,9 +1,7 @@
 uint64 timestamp			# time since system start (microseconds)
 bool connected				# Whether or not a battery is connected, based on a voltage threshold
 float32 voltage_v			# Battery voltage in volts, 0 if unknown
-float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
 float32 current_a			# Battery current in amperes, -1 if unknown
-float32 current_filtered_a	# Battery current in amperes, filtered, 0 if unknown
 float32 current_average_a	# Battery current average in amperes (for FW average in level flight), -1 if unknown
 float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
 float32 remaining			# From 1 to 0, -1 if unknown
@@ -68,11 +66,15 @@ uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're ful

 uint8 MAX_INSTANCES = 4

-float32 average_power               # The average power of the current discharge
-float32 available_energy            # The predicted charge or energy remaining in the battery
 float32 full_charge_capacity_wh     # The compensated battery capacity
 float32 remaining_capacity_wh       # The compensated battery capacity remaining
-float32 design_capacity             # The design capacity of the battery
-uint16 average_time_to_full         # The predicted remaining time until the battery reaches full charge, in minutes
 uint16 over_discharge_count         # Number of battery overdischarge
 float32 nominal_voltage             # Nominal voltage of the battery pack
+
+float32 internal_resistance_estimate            # [Ohm] Internal resistance per cell estimate
+float32 ocv_estimate                            # [V] Open circuit voltage estimate
+float32 ocv_estimate_filtered			# [V] Filtered open circuit voltage estimate
+float32 volt_based_soc_estimate			# [0, 1] Normalized volt based state of charge estimate
+float32 voltage_prediction			# [V] Predicted voltage
+float32 prediction_error                        # [V] Prediction error
+float32 estimation_covariance_norm		# Norm of the covariance matrix
@@ -180,6 +180,7 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
+	RoverAckermannStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
@@ -25,4 +25,3 @@ bool fused                   # true if the sample was successfully fused
 # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip
 # TOPICS estimator_aid_src_fake_hgt
 # TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw
-# TOPICS estimator_aid_src_terrain_range_finder
@@ -22,5 +22,5 @@ bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused

 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
-# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow
 # TOPICS estimator_aid_src_drag
@@ -9,4 +9,4 @@ float32 innov			# innovation of the last measurement fusion (m)
 float32 innov_var		# innovation variance of the last measurement fusion (m^2)
 float32 innov_test_ratio	# normalized innovation squared test ratio

-# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias
@@ -20,18 +20,3 @@ bool reset_hgt_to_baro                  # 13 - true when the vertical position s
 bool reset_hgt_to_gps                   # 14 - true when the vertical position state is reset to the gps measurement
 bool reset_hgt_to_rng                   # 15 - true when the vertical position state is reset to the rng measurement
 bool reset_hgt_to_ev                    # 16 - true when the vertical position state is reset to the ev measurement
-
-# warning events
-uint32 warning_event_changes            # number of warning event changes
-bool gps_quality_poor                   #  0 - true when the gps is failing quality checks
-bool gps_fusion_timout                  #  1 - true when the gps data has not been used to correct the state estimates for a significant time period
-bool gps_data_stopped                   #  2 - true when the gps data has stopped for a significant time period
-bool gps_data_stopped_using_alternate   #  3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation
-bool height_sensor_timeout              #  4 - true when the height sensor has not been used to correct the state estimates for a significant time period
-bool stopping_navigation                #  5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation
-bool invalid_accel_bias_cov_reset       #  6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements
-bool bad_yaw_using_gps_course           #  7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course
-bool stopping_mag_use                   #  8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data
-bool vision_data_stopped                #  9 - true when the vision system data has stopped for a significant time period
-bool emergency_yaw_reset_mag_stopped    # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data
-bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data
@@ -13,6 +13,7 @@ bool check_fail_max_horz_drift   # 6 : maximum allowed horizontal position drift
 bool check_fail_max_vert_drift   # 7 : maximum allowed vertical position drift fail - requires stationary vehicle
 bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle
 bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail
+bool check_fail_spoofed_gps      # 10 : GPS signal is spoofed

 float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s)
 float32 position_drift_rate_vertical_m_s   # Vertical position rate magnitude (m/s)
@@ -22,7 +22,6 @@ float32[2] aux_hvel	# horizontal auxiliary velocity innovation from landing targ

 # Optical flow
 float32[2] flow		# flow innvoation (rad/sec) and innovation variance ((rad/sec)**2)
-float32[2] terr_flow	# flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2)

 # Various
 float32 heading		# heading innovation (rad) and innovation variance (rad**2)
@@ -1,7 +1,7 @@
 uint64 timestamp		# time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)

-float32[24] states		# Internal filter states
+float32[25] states		# Internal filter states
 uint8 n_states		# Number of states effectively used

-float32[23] covariances	# Diagonal Elements of Covariance Matrix
+float32[24] covariances	# Diagonal Elements of Covariance Matrix
@@ -15,6 +15,7 @@ uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6		# 6 : maximum allowed horizontal positi
 uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7		# 7 : maximum allowed vertical position drift fail - requires stationary vehicle
 uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8	# 8 : maximum allowed horizontal speed fail - requires stationary vehicle
 uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9	# 9 : maximum allowed vertical velocity discrepancy fail
+uint8 GPS_CHECK_FAIL_SPOOFED = 10		# 10 : GPS signal is spoofed

 uint64 control_mode_flags	# Bitmask to indicate EKF logic state
 uint8 CS_TILT_ALIGN = 0		# 0 - true if the filter tilt alignment is complete
@@ -69,27 +70,14 @@ uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults

 float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m)
 float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m)
-uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks
-# 0 - true if velocity observations have been rejected
-# 1 - true if horizontal position observations have been rejected
-# 2 - true if true if vertical position observations have been rejected
-# 3 - true if the X magnetometer observation has been rejected
-# 4 - true if the Y magnetometer observation has been rejected
-# 5 - true if the Z magnetometer observation has been rejected
-# 6 - true if the yaw observation has been rejected
-# 7 - true if the airspeed observation has been rejected
-# 8 - true if the synthetic sideslip observation has been rejected
-# 9 - true if the height above ground observation has been rejected
-# 10 - true if the X optical flow observation has been rejected
-# 11 - true if the Y optical flow observation has been rejected

-float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit
-float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit
-float32 pos_test_ratio # ratio of the largest horizontal position innovation component to the innovation test limit
-float32 hgt_test_ratio # ratio of the vertical position innovation to the innovation test limit
-float32 tas_test_ratio # ratio of the true airspeed innovation to the innovation test limit
-float32 hagl_test_ratio # ratio of the height above ground innovation to the innovation test limit
-float32 beta_test_ratio # ratio of the synthetic sideslip innovation to the innovation test limit
+float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit
+float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit
+float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit
+float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit
+float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit
+float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit
+float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit

 uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use.
 # 0 - True if the attitude estimate is good
@@ -43,6 +43,8 @@ bool cs_mag                     # 35 - true if 3-axis magnetometer measurement f
 bool cs_ev_yaw_fault            # 36 - true when the EV heading has been declared faulty and is no longer being used
 bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag data is declared consistent with the filter
 bool cs_aux_gpos                # 38 - true if auxiliary global position measurement fusion is intended
+bool cs_rng_terrain             # 39 - true if we are fusing range finder data for terrain
+bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain

 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -1,10 +1,9 @@
 uint64 timestamp # time since system start (microseconds)

-float32 actual_speed # [m/s] Rover ground speed
-float32 desired_speed # [m/s] Rover desired ground speed
-float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
-float32 heading_error # [deg] Heading error of the pure pursuit controller
-float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
-float32 crosstrack_error # [m] Shortest distance from the vehicle to the path
+float32 desired_speed 		# [m/s] Rover desired ground speed
+float32 lookahead_distance 	# [m] Lookahead distance of pure the pursuit controller
+float32 heading_error 		# [deg] Heading error of the pure pursuit controller
+float32 pid_throttle_integral 	# [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions
+float32 crosstrack_error 	# [m] Shortest distance from the vehicle to the path

 # TOPICS rover_ackermann_guidance_status
@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 throttle_setpoint 	# [-1, 1] Normalized throttle setpoint
+float32 steering_setpoint 	# [-1, 1] Normalized steering setpoint
+float32 actual_speed       	# [m/s] Rover ground speed
+
+# TOPICS rover_ackermann_status
@@ -56,6 +56,7 @@ float32 ref_alt				# Reference altitude AMSL, (metres)

 # Distance to surface
 float32 dist_bottom			# Distance from from bottom surface to ground, (metres)
+float32 dist_bottom_var                 # terrain estimate variance (m^2)
 bool dist_bottom_valid			# true if distance to bottom surface is valid
 uint8 dist_bottom_sensor_bitfield	# bitfield indicating what type of sensor is used to estimate dist_bottom
 uint8 DIST_BOTTOM_SENSOR_NONE = 0
@@ -239,10 +239,21 @@ public:
 					unlock_module();
 					px4_usleep(10000); // 10 ms
 					lock_module();
-					i++;

-					if (i % 500 == 0) {
-						PX4_INFO("Waiting for task to stop...");
+					if (++i > 500 && _task_id != -1) { // wait at most 5 sec
+						PX4_ERR("timeout, forcing stop");
+
+						if (_task_id != task_id_is_work_queue) {
+							px4_task_delete(_task_id);
+						}
+
+						_task_id = -1;
+
+						delete _object.load();
+						_object.store(nullptr);
+
+						ret = -1;
+						break;
 					}
 				} while (_task_id != -1);

@@ -125,8 +125,16 @@ public:
 	{
 		if (_subscription.copy(dst)) {
 			const hrt_abstime now = hrt_absolute_time();
-			// shift last update time forward, but don't let it get further behind than the interval
-			_last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now);
+
+			// make sure we don't set a timestamp before the timer started counting (now - _interval_us would wrap because it's unsigned)
+			if (now > _interval_us) {
+				// shift last update time forward, but don't let it get further behind than the interval
+				_last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now);
+
+			} else {
+				_last_update = now;
+			}
+
 			return true;
 		}

@@ -160,7 +168,7 @@ public:
 protected:

 	Subscription	_subscription;
-	uint64_t	_last_update{0};	// last update in microseconds
+	uint64_t	_last_update{0};	// last subscription update in microseconds
 	uint32_t	_interval_us{0};	// maximum update interval in microseconds

 };
@@ -45,7 +45,6 @@
 #include <nuttx/kthread.h>

 #include <sys/wait.h>
-#include <syslog.h>
 #include <stdbool.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -89,8 +88,7 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_

 int px4_task_delete(int pid)
 {
-	syslog(LOG_ERR, "Ignoring force task delete on NuttX\n");
-	return ERROR;
+	return task_delete(pid);
 }

 const char *px4_get_taskname(void)
@@ -46,7 +46,6 @@
 #include "arm_internal.h"

 #define FLEXIO_BASE			IMXRT_FLEXIO1_BASE
-#define FLEXIO_PREQ			120000000
 #define DSHOT_TIMERS			FLEXIO_SHIFTBUFNIS_COUNT
 #define DSHOT_THROTTLE_POSITION		5u
 #define DSHOT_TELEMETRY_POSITION	4u
@@ -305,8 +304,8 @@ static int flexio_irq_handler(int irq, void *context, void *arg)
 int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot)
 {
 	/* Calculate dshot timings based on dshot_pwm_freq */
-	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
-	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);
+	dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF);

 	/* Clock FlexIO peripheral */
 	imxrt_clockall_flexio1();
@@ -116,13 +116,11 @@ void BATT_SMBUS::RunImpl()

 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult;
-	new_report.current_filtered_a = new_report.current_a;

 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
@@ -37,12 +37,11 @@
 * Client-side implementation of UDRAL specification ESC service
 *
 * Publishes the following Cyphal messages:
- *   reg.drone.service.actuator.common.sp.Value8.0.1
- *   reg.drone.service.common.Readiness.0.1
+ *   reg.udral.service.actuator.common.sp.Value31.0.1
+ *   reg.udral.service.common.Readiness.0.1
 *
 * Subscribes to the following Cyphal messages:
- *   reg.drone.service.actuator.common.Feedback.0.1
- *   reg.drone.service.actuator.common.Status.0.1
+ *   zubax.telega.CompactFeedback.0.1
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
 * @author Jacob Crabill <jacob@flyvoly.com>
@@ -51,11 +50,13 @@
 #pragma once

 #include <lib/perf/perf_counter.h>
-#include <uORB/PublicationMulti.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
+#include "../Subscribers/DynamicPortSubscriber.hpp"
+#include "../Publishers/Publisher.hpp"
 #include <lib/mixer_module/mixer_module.hpp>

 // UDRAL Specification Messages
@@ -63,16 +64,15 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>

-/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status
-class UavcanEscController : public UavcanPublisher
+
+class ReadinessPublisher : public UavcanPublisher
 {
 public:
-	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;

-	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
+	ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "readiness") { };

-	~UavcanEscController() {};
+	~ReadinessPublisher() {};

 	void update() override
 	{
@@ -95,58 +95,18 @@ public:
 		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
 			publish_readiness();
 		}
-	};
+	}

-	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
-	{
-		if (_port_id > 0) {
-			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
-			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;

-			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
-				if (i < num_outputs) {
-					msg_sp.value[i] = static_cast<float>(outputs[i]);
+	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
+	actuator_armed_s _armed {};

-				} else {
-					// "unset" values published as NaN
-					msg_sp.value[i] = NAN;
-				}
-			}
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};

-			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-			const CanardTransferMetadata transfer_metadata = {
-				.priority       = CanardPriorityNominal,
-				.transfer_kind  = CanardTransferKindMessage,
-				.port_id        = _port_id,                // This is the subject-ID.
-				.remote_node_id = CANARD_NODE_ID_UNSET,       // Messages cannot be unicast, so use UNSET.
-				.transfer_id    = _transfer_id,
-			};
-
-			int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer,
-					&payload_size);
-
-			if (result == 0) {
-				// set the data ready in the buffer and chop if needed
-				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-				result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-							       &transfer_metadata,
-							       payload_size,
-							       &esc_sp_payload_buffer);
-			}
-		}
-	};
-
-	/**
-	 * Sets the number of rotors
-	 */
-	void set_rotor_count(uint8_t count) { _rotor_count = count; }
-
-private:
-	/**
-	 * ESC status message reception will be reported via this callback.
-	 */
-	void esc_status_sub_cb(const CanardRxTransfer &msg);
+	CanardTransferID _arming_transfer_id;

 	void publish_readiness()
 	{
@@ -155,8 +115,7 @@ private:

 		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

-		// Only publish if we have a valid publication ID set
-		if (_port_id == 0) {
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
 			return;
 		}

@@ -174,12 +133,12 @@ private:

 		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];

-		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id));
 		const CanardTransferMetadata transfer_metadata = {
 			.priority       = CanardPriorityNominal,
 			.transfer_kind  = CanardTransferKindMessage,
-			.port_id        = arming_pid,                // This is the subject-ID.
-			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.port_id        = arming_pid,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
 			.transfer_id    = _arming_transfer_id,
 		};

@@ -187,25 +146,143 @@ private:
 				&payload_size);

 		if (result == 0) {
-			// set the data ready in the buffer and chop if needed
-			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			++_arming_transfer_id;
 			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 						       &transfer_metadata,
 						       payload_size,
 						       &arming_payload_buffer);
 		}
 	};
-
-	uint8_t _rotor_count {0};
-
-	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
-	hrt_abstime _previous_pub_time = 0;
-
-	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
-	actuator_armed_s _armed {};
-
-	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
-	uint64_t _actuator_test_timestamp{0};
-
-	CanardTransferID _arming_transfer_id;
+};
+
+class UavcanEscController : public UavcanPublisher
+{
+public:
+	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
+
+	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { }
+
+	~UavcanEscController() {}
+
+	void update() override
+	{
+	}
+
+	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
+	{
+		if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) {
+			return;
+		}
+
+		uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS;
+
+		for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) {
+			if (outputs[i] != 0) {
+				_max_number_of_nonzero_outputs = i + 1;
+				break;
+			}
+		}
+
+		uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_];
+
+		for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) {
+			payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0);
+		}
+
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = _port_id,
+			.remote_node_id = CANARD_NODE_ID_UNSET,
+			.transfer_id    = _transfer_id,
+		};
+
+		++_transfer_id;
+		_canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				      &transfer_metadata,
+				      _max_number_of_nonzero_outputs * 2,
+				      &payload_buffer);
+	}
+
+private:
+	uint8_t _max_number_of_nonzero_outputs{1};
+};
+
+class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {}
+
+	void subscribe() override
+	{
+		_canard_handle.RxSubscribe(CanardTransferKindMessage,
+					   _subj_sub._canard_sub.port_id,
+					   zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES,
+					   CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+					   &_subj_sub._canard_sub);
+		_esc_status.esc_armed_flags |= 1 << _instance;
+		_esc_status.esc_count++;
+	};
+
+	void unsubscribe() override
+	{
+		_canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id);
+		_esc_status.esc_armed_flags &= ~(1 << _instance);
+		_esc_status.esc_count--;
+	};
+
+	void callback(const CanardRxTransfer &receive) override
+	{
+		if (_instance >= esc_status_s::CONNECTED_ESC_MAX) {
+			return;
+		}
+
+		auto &ref = _esc_status.esc[_instance];
+		const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload));
+
+		ref.timestamp       = hrt_absolute_time();
+		ref.esc_address     = receive.metadata.remote_node_id;
+		ref.esc_voltage     = 0.2 * feedback->dc_voltage;
+		ref.esc_current     = 0.2 * feedback->dc_current;
+		ref.esc_temperature = NAN;
+		ref.esc_rpm         = feedback->velocity * RAD_PER_SEC_TO_RPM;
+		ref.esc_errorcount  = 0;
+
+		_esc_status.counter++;
+		_esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN;
+		_esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1;
+		_esc_status.timestamp = hrt_absolute_time();
+		_esc_status_pub.publish(_esc_status);
+
+		_esc_status.esc_online_flags = 0;
+		const hrt_abstime now = hrt_absolute_time();
+
+		for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) {
+			if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) {
+				_esc_status.esc_online_flags |= (1 << index);
+			}
+		}
+	};
+
+private:
+	static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968;
+	static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7;
+
+	// https://telega.zubax.com/interfaces/cyphal.html#compact
+#pragma pack(push, 1)
+	struct ZubaxCompactFeedback {
+		uint32_t dc_voltage 		: 11;
+		int32_t dc_current 		: 12;
+		int32_t phase_current_amplitude	: 12;
+		int32_t velocity 		: 13;
+		int8_t demand_factor_pct 	: 8;
+	};
+#pragma pack(pop)
+	static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES);
+
+	static esc_status_s _esc_status;
+
+	uORB::Publication<esc_status_s> _esc_status_pub{ORB_ID(esc_status)};
 };
@@ -62,6 +62,8 @@ using namespace time_literals;

 CyphalNode *CyphalNode::_instance;

+esc_status_s UavcanEscFeedbackSubscriber::_esc_status;
+
 CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
@@ -125,7 +127,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);

 	} else {
-		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC);
 	}

 	if (_instance == nullptr) {
@@ -188,6 +190,8 @@ void CyphalNode::Run()
 		// send uavcan::node::Heartbeat_1_0 @ 1 Hz
 		sendHeartbeat();

+		sendPortList();
+
 		// Check all publishers
 		_pub_manager.update();

@@ -359,10 +363,10 @@ void CyphalNode::sendHeartbeat()
 	if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) {

 		uavcan_node_Heartbeat_1_0 heartbeat{};
-		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
+		const hrt_abstime now = hrt_absolute_time();
+		heartbeat.uptime = now / 1000000;
 		heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL;
 		heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
-		const hrt_abstime now = hrt_absolute_time();
 		size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_;

 		const CanardTransferMetadata transfer_metadata = {
@@ -392,6 +396,45 @@ void CyphalNode::sendHeartbeat()
 	}
 }

+void CyphalNode::sendPortList()
+{
+	static hrt_abstime _uavcan_node_port_List_last{0};
+
+	if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) {
+		return;
+	}
+
+	static uavcan_node_port_List_0_1 msg{};
+	static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_];
+	static CanardTransferID _uavcan_node_port_List_transfer_id{0};
+	size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_;
+	const hrt_abstime now = hrt_absolute_time();
+
+	const CanardTransferMetadata transfer_metadata = {
+		.priority       = CanardPriorityNominal,
+		.transfer_kind  = CanardTransferKindMessage,
+		.port_id        = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
+		.remote_node_id = CANARD_NODE_ID_UNSET,
+		.transfer_id    = _uavcan_node_port_List_transfer_id++
+	};
+
+	// memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_);
+	uavcan_node_port_List_0_1_initialize_(&msg);
+
+	_pub_manager.fillSubjectIdList(msg.publishers);
+	_sub_manager.fillSubjectIdList(msg.subscribers);
+
+	uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size);
+
+	_canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+			      &transfer_metadata,
+			      payload_size,
+			      &uavcan_node_port_List_0_1_buffer
+			     );
+
+	_uavcan_node_port_List_last = now;
+}
+
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
@@ -137,6 +137,9 @@ private:
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();

+	// Sends a port.List at 3s intervals
+	void sendPortList();
+
 	px4::atomic_bool _task_should_exit{false};	///< flag to indicate to tear down the CAN driver

 	bool _initialized{false};		///< number of actuators currently available
@@ -56,6 +56,15 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 		}
 	}

+	for (auto &param : _type_registers) {
+		if (strcmp(param_name, param.name) == 0) {
+			uavcan_register_Value_1_0_select_string_(&value);
+			value._string.value.count = strlen(param.value);
+			memcpy(&value._string, param.value, value._string.value.count);
+			return true;
+		}
+	}
+
 	return false;
 }

@@ -73,19 +82,36 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 		}
 	}

+	for (auto &param : _type_registers) {
+		if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) {
+			uavcan_register_Value_1_0_select_string_(&value);
+			value._string.value.count = strlen(param.value);
+			memcpy(&value._string, param.value, value._string.value.count);
+			return true;
+		}
+	}
+
 	return false;
 }

 bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name)
 {
-	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+	size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder);
+	size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister);
+
+	if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
+		strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
+		name.name.count = strlen(_uavcan_params[id].uavcan_name);
+
+	} else if (id < number_of_integer_registers + number_of_type_registers) {
+		id -= number_of_integer_registers;
+		strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name));
+		name.name.count = strlen(_type_registers[id].name);
+
+	} else {
 		return false;
 	}

-	strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_);
-
-	name.name.count = strlen(_uavcan_params[id].uavcan_name);
-
 	return true;
 }

@@ -103,6 +103,10 @@ typedef struct {
 	bool is_persistent {true};
 } UavcanParamBinder;

+typedef struct {
+	const char *name;
+	const char *value;
+} CyphalTypeRegister;

 class UavcanParamManager
 {
@@ -116,8 +120,9 @@ public:
 private:


-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[22] {
 		{"uavcan.pub.udral.esc.0.id",                	"UCAN1_ESC_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.readiness.0.id",            	"UCAN1_READ_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.servo.0.id",              	"UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -130,7 +135,28 @@ private:
 		{"uavcan.sub.udral.legacy_bms.0.id",         	"UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.0.id",            	"UCAN1_FB0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.1.id",            	"UCAN1_FB1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.2.id",            	"UCAN1_FB2_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.3.id",            	"UCAN1_FB3_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.4.id",            	"UCAN1_FB4_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.5.id",            	"UCAN1_FB5_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.6.id",            	"UCAN1_FB6_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.zubax.feedback.7.id",            	"UCAN1_FB7_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};
+
+	CyphalTypeRegister _type_registers[10] {
+		{"uavcan.pub.udral.esc.0.type", 		"reg.udral.service.actuator.common.sp.Vector31"},
+		{"uavcan.pub.udral.readiness.0.type", 		"reg.udral.service.common.Readiness.0.1"},
+		{"uavcan.sub.zubax.feedback.0.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.1.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.2.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.3.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.4.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.5.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.6.type", 		"zubax.telega.CompactFeedback.0.1"},
+		{"uavcan.sub.zubax.feedback.7.type", 		"zubax.telega.CompactFeedback.0.1"},
+	};
 };
@@ -125,6 +125,15 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
 	return NULL;
 }

+void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list);
+
+	for (auto &dynpub : _dynpublishers) {
+		publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id();
+		publishers_list.sparse_list.count++;
+	}
+}

 void PublicationManager::update()
 {
@@ -67,7 +67,7 @@
 /* Preprocessor calculation of publisher count */

 #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \
-	CONFIG_CYPHAL_ESC_CONTROLLER + \
+	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -79,6 +79,7 @@

 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uavcan/node/port/List_0_1.h>

 #include "Actuators/EscClient.hpp"
 #include "Publishers/udral/Readiness.hpp"
@@ -103,6 +104,7 @@ public:

 	UavcanPublisher *getPublisher(const char *subject_name);

+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list);
 private:
 	void updateDynamicPublications();

@@ -131,6 +133,14 @@ private:
 			"udral.esc",
 			0
 		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
+			{
+				return new ReadinessPublisher(handle, pmgr);
+			},
+			"udral.readiness",
+			0
+		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{
@@ -76,8 +76,6 @@ public:

 		battery_status_s bat_status {0};
 		bat_status.timestamp = hrt_absolute_time();
-		bat_status.voltage_filtered_v = bat_info.voltage;
-		bat_status.current_filtered_a = bat_info.current;
 		bat_status.current_average_a = bat_info.average_power_10sec;
 		bat_status.remaining = bat_info.state_of_charge_pct / 100.0f;
 		bat_status.scale = -1;
@@ -158,3 +158,24 @@ void SubscriptionManager::updateParams()
 	// Check for any newly-enabled subscriptions
 	updateDynamicSubscriptions();
 }
+
+void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list)
+{
+	uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list);
+
+	UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
+
+	auto &sparse_list = subscribers_list.sparse_list;
+
+	while (dynsub != nullptr) {
+		int32_t instance_idx = 0;
+
+		while (dynsub->isValidPortId(dynsub->id(instance_idx))) {
+			sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx);
+			sparse_list.count++;
+			instance_idx++;
+		}
+
+		dynsub = dynsub->next();
+	}
+}
@@ -45,6 +45,10 @@
 #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0
 #endif

+#ifndef CONFIG_CYPHAL_ESC_CONTROLLER
+#define CONFIG_CYPHAL_ESC_CONTROLLER 0
+#endif
+
 #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0
 #endif
@@ -65,12 +69,15 @@

 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
+	8 * CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER

 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
+#include <uavcan/node/port/List_0_1.h>
+#include "Actuators/EscClient.hpp"
 #include "Subscribers/DynamicPortSubscriber.hpp"
 #include "CanardInterface.hpp"

@@ -100,6 +107,7 @@ public:
 	void subscribe();
 	void printInfo();
 	void updateParams();
+	void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list);

 private:
 	void updateDynamicSubscriptions();
@@ -130,6 +138,72 @@ private:
 			0
 		},
 #endif
+#if CONFIG_CYPHAL_ESC_CONTROLLER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 0);
+			},
+			"zubax.feedback",
+			0
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 1);
+			},
+			"zubax.feedback",
+			1
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 2);
+			},
+			"zubax.feedback",
+			2
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 3);
+			},
+			"zubax.feedback",
+			3
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 4);
+			},
+			"zubax.feedback",
+			4
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 5);
+			},
+			"zubax.feedback",
+			5
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 6);
+			},
+			"zubax.feedback",
+			6
+		},
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new UavcanEscFeedbackSubscriber(handle, pmgr, 7);
+			},
+			"zubax.feedback",
+			7
+		},
+#endif
 #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0
 		{
 			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
@@ -162,6 +162,87 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
 */
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1);

+/**
+ * Cyphal ESC readiness port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1);
+
+/**
+ * Cyphal ESC 0 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1);
+
+/**
+ * Cyphal ESC 1 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1);
+
+/**
+ * Cyphal ESC 2 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1);
+
+/**
+ * Cyphal ESC 3 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1);
+
+/**
+ * Cyphal ESC 4 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1);
+
+/**
+ * Cyphal ESC 5 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1);
+
+/**
+ * Cyphal ESC 6 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1);
+
+/**
+ * Cyphal ESC 7 zubax feedback port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1);
+
 /**
 * Cyphal GPS publication port ID.
 *
@@ -56,7 +56,7 @@
 #endif

 #ifdef SEP_LOG_ERROR
-#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_ERR(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif
@@ -71,7 +71,7 @@ parameters:
        type: float
        decimal: 3
        default: 0
-        min: 0
+        min: -360
        max: 360
        unit: deg
        reboot_required: true
@@ -104,7 +104,8 @@ parameters:
        description:
          short: Log GPS communication data
          long: |
-            Dump raw communication data from and to the connected receiver to the log file.
+            Log raw communication between the driver and connected receivers.
+            For example, "To receiver" will log all commands and corrections sent by the driver to the receiver.
        type: enum
        default: 0
        min: 0
@@ -60,8 +60,8 @@ constexpr uint32_t k_dnu_u4_value {4294967295};
 constexpr uint32_t k_dnu_u4_bitfield {0};
 constexpr uint16_t k_dnu_u2_value {65535};
 constexpr uint16_t k_dnu_u2_bitfield {0};
-constexpr float k_dnu_f4_value {-2 * 10000000000};
-constexpr double k_dnu_f8_value {-2 * 10000000000};
+constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f};
+constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};

 /// Maximum size of all expected messages.
 /// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages.
@@ -55,6 +55,7 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/time.h>
+#include <lib/systemlib/mavlink_log.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>

@@ -86,6 +87,11 @@ constexpr int k_max_receiver_read_timeout = 50;
 */
 constexpr size_t k_min_receiver_read_bytes = 32;

+/**
+ * The baud rate of Septentrio receivers with factory default configuration.
+*/
+constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200;
+
 constexpr uint8_t k_max_command_size            = 120;
 constexpr uint16_t k_timeout_5hz                = 500;
 constexpr uint32_t k_read_buffer_size           = 150;
@@ -134,18 +140,19 @@ constexpr const char *k_frequency_25_0hz = "msec40";
 constexpr const char *k_frequency_50_0hz = "msec20";

 px4::atomic<SeptentrioDriver *> SeptentrioDriver::_secondary_instance {nullptr};
+uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000};
+uint32_t SeptentrioDriver::k_default_baud_rate {230400};
+orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr};

 SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) :
 	Device(MODULE_NAME),
 	_instance(instance),
-	_baud_rate(baud_rate)
+	_chosen_baud_rate(baud_rate)
 {
 	strncpy(_port, device_path, sizeof(_port) - 1);
 	// Enforce null termination.
 	_port[sizeof(_port) - 1] = '\0';

-	reset_gps_state_message();
-
 	int32_t enable_sat_info {0};
 	get_parameter("SEP_SAT_INFO", &enable_sat_info);

@@ -171,6 +178,10 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	get_parameter("SEP_STREAM_LOG", &receiver_stream_log);
 	_receiver_stream_log = receiver_stream_log;

+	if (_receiver_stream_log == _receiver_stream_main) {
+		mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream");
+	}
+
 	int32_t automatic_configuration {true};
 	get_parameter("SEP_AUTO_CONFIG", &automatic_configuration);
 	_automatic_configuration = static_cast<bool>(automatic_configuration);
@@ -198,6 +209,8 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	}

 	set_device_type(DRV_GPS_DEVTYPE_SBF);
+
+	reset_gps_state_message();
 }

 SeptentrioDriver::~SeptentrioDriver()
@@ -240,15 +253,13 @@ int SeptentrioDriver::print_status()
 		break;
 	}

-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
 	PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate());
 	PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate());
 	PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled");

-	if (_message_gps_state.timestamp != 0) {
-
+	if (first_gps_uorb_message_created() && _state == State::ReceivingData) {
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
-
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}

@@ -267,7 +278,7 @@ void SeptentrioDriver::run()
 				_uart.setPort(_port);

 				if (_uart.open()) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;

 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -277,14 +288,42 @@ void SeptentrioDriver::run()
 				break;
 			}

+		case State::DetectingBaudRate: {
+				static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
+				expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate;
+
+				if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) {
+					if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) {
+						_state = State::ConfiguringDevice;
+
+					} else {
+						SEP_ERR("Setting local baud rate failed");
+					}
+
+				} else {
+					_current_baud_rate_index++;
+
+					if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) {
+						_current_baud_rate_index = 0;
+					}
+				}
+
+				break;
+			}
+
 		case State::ConfiguringDevice: {
-				if (configure() == PX4_OK) {
-					SEP_INFO("Automatic configuration successful");
+				ConfigureResult result = configure();
+
+				if (!(static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::FailedCompletely))) {
+					if (static_cast<int32_t>(result) & static_cast<int32_t>(ConfigureResult::NoLogging)) {
+						mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging");
+					}
+
+					SEP_INFO("Automatic configuration finished");
 					_state = State::ReceivingData;

 				} else {
-					// Failed to configure device, so wait a bit before trying again.
-					px4_usleep(200000);
+					_state = State::DetectingBaudRate;
 				}

 				break;
@@ -296,7 +335,7 @@ void SeptentrioDriver::run()
 				receive_result = receive(k_timeout_5hz);

 				if (receive_result == -1) {
-					_state = State::ConfiguringDevice;
+					_state = State::DetectingBaudRate;
 				}

 				if (_message_satellite_info && (receive_result & 2)) {
@@ -385,19 +424,51 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[])

 SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance)
 {
-	ModuleArguments arguments{};
-	SeptentrioDriver *gps{nullptr};
+	ModuleArguments arguments {};
+	SeptentrioDriver *gps {nullptr};

 	if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) {
 		return nullptr;
 	}

+	if (arguments.device_path_main && arguments.device_path_secondary
+	    && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different");
+		return nullptr;
+	}
+
+	bool valid_chosen_baud_rate {false};
+
+	for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) {
+		switch (instance) {
+		case Instance::Main:
+			if (arguments.baud_rate_main == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+
+		case Instance::Secondary:
+			if (arguments.baud_rate_secondary == static_cast<int>(k_supported_baud_rates[i])) {
+				valid_chosen_baud_rate = true;
+			}
+
+			break;
+		}
+	}
+
+	if (!valid_chosen_baud_rate) {
+		mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu",
+				     instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate);
+	}
+
 	if (instance == Instance::Main) {
 		if (Serial::validatePort(arguments.device_path_main)) {
-			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);

 		} else {
-			PX4_ERR("invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
+			PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : "");
 		}

 		if (gps && arguments.device_path_secondary) {
@@ -410,7 +481,8 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance

 	} else {
 		if (Serial::validatePort(arguments.device_path_secondary)) {
-			gps = new SeptentrioDriver(arguments.device_path_secondary, instance, arguments.baud_rate_secondary);
+			gps = new SeptentrioDriver(arguments.device_path_secondary, instance,
+						   valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate);

 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -425,6 +497,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance
 int SeptentrioDriver::custom_command(int argc, char *argv[])
 {
 	bool handled = false;
+	const char *failure_reason {"unknown command"};
 	SeptentrioDriver *driver_instance;

 	if (!is_running()) {
@@ -448,7 +521,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;

 			} else {
-				print_usage("incorrect reset type");
+				failure_reason = "unknown reset type";
 			}

 			if (type != ReceiverResetType::None) {
@@ -457,11 +530,11 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 			}

 		} else {
-			print_usage("incorrect usage of reset command");
+			failure_reason = "incorrect usage of reset command";
 		}
 	}

-	return (handled) ? 0 : print_usage("unknown command");
+	return handled ? 0 : print_usage(failure_reason);
 }

 int SeptentrioDriver::print_usage(const char *reason)
@@ -473,25 +546,30 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-GPS driver module that handles the communication with Septentrio devices and publishes the position via uORB.
-
-The module supports a secondary GPS device, specified via `-e` parameter. The position will be published on
-the second uORB topic instance. It can be used for logging and heading computation.
+Driver for Septentrio GNSS receivers.
+It can automatically configure them and make their output available for the rest of the system.
+A secondary receiver is supported for redundancy, logging and dual-receiver heading.
+Septentrio receiver baud rates from 57600 to 1500000 are supported.
+If others are used, the driver will use 230400 and give a warning.

 ### Examples

-Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
+Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400:
+$ septentrio start -d /dev/ttyS0 -b 230400
+
+Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`,
+detect baud rate automatically and preserve them:
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4

-Initiate warm restart of GPS device
+Perform warm reset of the receivers:
 $ gps reset warm
 )DESCR_STR");
 	PRINT_MODULE_USAGE_NAME("septentrio", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary Septentrio receiver", false);
-	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary baud rate", true);
-	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary Septentrio receiver", true);
-	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "<file:dev>", "Primary receiver port", false);
+	PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true);
+	PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "<file:dev>", "Secondary receiver port", true);
+	PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true);

 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -508,15 +586,15 @@ int SeptentrioDriver::reset(ReceiverResetType type)

 	switch (type) {
 	case ReceiverResetType::Hot:
-		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast);
 		break;

 	case ReceiverResetType::Warm:
-		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast);
 		break;

 	case ReceiverResetType::Cold:
-		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout);
+		res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast);
 		break;

 	default:
@@ -553,13 +631,13 @@ int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArgument
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
 		case 'g':
 			if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) {
-				PX4_ERR("baud rate parsing failed");
+				PX4_ERR("Baud rate parsing failed");
 				return PX4_ERROR;
 			}
 			break;
@@ -637,42 +715,31 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }

-uint32_t SeptentrioDriver::detect_receiver_baud_rate(bool forced_input) {
-	// So we can restore the port to its original state.
-	const uint32_t original_baud_rate = _uart.getBaudrate();
-
-	// Baud rates we expect the receiver to be running at.
-	uint32_t expected_baud_rates[] = {115200, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000};
-	if (_baud_rate != 0) {
-		expected_baud_rates[0] = _baud_rate;
+bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
+	if (set_baudrate(baud_rate) != PX4_OK) {
+		return false;
 	}

-	for (uint i = 0; i < sizeof(expected_baud_rates)/sizeof(expected_baud_rates[0]); i++) {
-		if (set_baudrate(expected_baud_rates[i]) != PX4_OK) {
-			set_baudrate(original_baud_rate);
-			return 0;
-		}
-
-		if (forced_input) {
-			force_input();
-		}
-
-		if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("Detected baud rate: %lu", expected_baud_rates[i]);
-			set_baudrate(original_baud_rate);
-			return expected_baud_rates[i];
-		}
+	if (forced_input) {
+		force_input();
 	}

-	set_baudrate(original_baud_rate);
-	return 0;
+	// Make sure that any weird data is "flushed" in the receiver.
+	(void)send_message("\n");
+
+	if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+		SEP_INFO("Detected baud rate: %lu", baud_rate);
+		return true;
+	}
+
+	return false;
 }

 int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	// Read buffer to get the COM port
 	char buf[k_read_buffer_size];
 	size_t buffer_offset = 0;   // The offset into the string where the next data should be read to.
-	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast;
 	bool response_detected = false;

 	// Receiver prints prompt after a message.
@@ -682,7 +749,7 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {

 	do {
 		// Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string.
-		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast);

 		if (read_result < 0) {
 			SEP_WARN("SBF read error");
@@ -734,132 +801,81 @@ int SeptentrioDriver::detect_serial_port(char* const port_name) {
 	}
 }

-int SeptentrioDriver::configure()
+SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
 {
 	char msg[k_max_command_size] {};
-
-	// Passively detect receiver baud rate.
-	uint32_t detected_receiver_baud_rate = detect_receiver_baud_rate(true);
-
-	if (detected_receiver_baud_rate == 0) {
-		SEP_INFO("CONFIG: failed baud detection");
-		return PX4_ERROR;
-	}
-
-	// Set same baud rate on our end.
-	if (set_baudrate(detected_receiver_baud_rate) != PX4_OK) {
-		SEP_INFO("CONFIG: failed local baud rate setting");
-		return PX4_ERROR;
-	}
-
 	char com_port[5] {};
+	ConfigureResult result {ConfigureResult::OK};

 	// Passively detect receiver port.
 	if (detect_serial_port(com_port) != PX4_OK) {
-		SEP_INFO("CONFIG: failed port detection");
-		return PX4_ERROR;
+		SEP_WARN("CONFIG: failed port detection");
+		return ConfigureResult::FailedCompletely;
 	}

 	// We should definitely match baud rates and detect used port, but don't do other configuration if not requested.
 	// This will force input on the receiver. That shouldn't be a problem as it's on our own connection.
 	if (!_automatic_configuration) {
-		return PX4_OK;
+		return ConfigureResult::OK;
 	}

 	// If user requested specific baud rate, set it now. Otherwise keep detected baud rate.
-	if (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
-		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);
+	if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) {
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate);

 		if (!send_message(msg)) {
-			SEP_INFO("CONFIG: baud rate command write error");
-			return PX4_ERROR;
+			SEP_WARN("CONFIG: baud rate command write error");
+			return ConfigureResult::FailedCompletely;
 		}

 		// When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate.
 		// From testing this could take some time.
-		px4_usleep(1000000);
+		px4_usleep(2000000);

-		if (set_baudrate(_baud_rate) != PX4_OK) {
-			SEP_INFO("CONFIG: failed local baud rate setting");
-			return PX4_ERROR;
+		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
+			SEP_WARN("CONFIG: failed local baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}

-		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout)) {
-			SEP_INFO("CONFIG: failed remote baud rate setting");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) {
+			SEP_WARN("CONFIG: failed remote baud rate setting");
+			return ConfigureResult::FailedCompletely;
 		}
-	} else {
-		_baud_rate = detected_receiver_baud_rate;
 	}

 	// Delete all sbf outputs on current COM port to remove clutter data
 	snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port);

-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("CONFIG: failed delete output");
-		return PX4_ERROR; // connection and/or baudrate detection failed
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: failed delete output");
+		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
 	}

-	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
-	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	// Specify the offsets that the receiver applies to the computed attitude angles.
-	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset);
-
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		return PX4_ERROR;
-	}
-
-	const char *sbf_frequency {k_frequency_10_0hz};
-	switch (_sbf_output_frequency) {
-	case SBFOutputFrequency::Hz5_0:
-		sbf_frequency = k_frequency_5_0hz;
-		break;
-	case SBFOutputFrequency::Hz10_0:
-		sbf_frequency = k_frequency_10_0hz;
-		break;
-	case SBFOutputFrequency::Hz20_0:
-		sbf_frequency = k_frequency_20_0hz;
-		break;
-	case SBFOutputFrequency::Hz25_0:
-		sbf_frequency = k_frequency_25_0hz;
-		break;
-	}
-
-	// Output a set of SBF blocks on a given connection at a regular interval.
-	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
-	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-		SEP_INFO("Failed to configure SBF");
-		return PX4_ERROR;
-	}
-
-	if (_receiver_setup == ReceiverSetup::MovingBase) {
-		if (_instance == Instance::Secondary) {
-			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure RTCM output");
-			}
-		} else {
-			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
-			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-				SEP_INFO("Failed to configure attitude source");
-			}
+	// Set up the satellites used in PVT computation.
+	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
+		char requested_satellites[40] {};
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
+			strcat(requested_satellites, "GPS+");
 		}
-	} else {
-		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
-		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
-		if (!send_message(msg)) {
-			SEP_INFO("Failed to configure attitude source");
-			return PX4_ERROR;
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
+			strcat(requested_satellites, "GLONASS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
+			strcat(requested_satellites, "GALILEO+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
+			strcat(requested_satellites, "SBAS+");
+		}
+		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
+			strcat(requested_satellites, "BEIDOU+");
+		}
+		// Make sure to remove the trailing '+' if any.
+		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
+		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
+		// Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers.
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) {
+			SEP_WARN("CONFIG: Failed to configure constellation usage");
+			return ConfigureResult::FailedCompletely;
 		}
 	}

@@ -919,42 +935,77 @@ int SeptentrioDriver::configure()
 		}

 		snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure logging");
-			return PX4_ERROR;
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
+		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
 	}

-	// Set up the satellites used in PVT computation.
-	if (_receiver_constellation_usage != static_cast<int32_t>(SatelliteUsage::Default)) {
-		char requested_satellites[40] {};
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GPS)) {
-			strcat(requested_satellites, "GPS+");
+	// Define/inquire the type of data that the receiver should accept/send on a given connection descriptor
+	snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF");
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	// Specify the offsets that the receiver applies to the computed attitude angles.
+	snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast<double>(_heading_offset), static_cast<double>(_pitch_offset));
+
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	snprintf(msg, sizeof(msg), k_command_set_dynamics, "high");
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		return ConfigureResult::FailedCompletely;
+	}
+
+	const char *sbf_frequency {k_frequency_10_0hz};
+	switch (_sbf_output_frequency) {
+	case SBFOutputFrequency::Hz5_0:
+		sbf_frequency = k_frequency_5_0hz;
+		break;
+	case SBFOutputFrequency::Hz10_0:
+		sbf_frequency = k_frequency_10_0hz;
+		break;
+	case SBFOutputFrequency::Hz20_0:
+		sbf_frequency = k_frequency_20_0hz;
+		break;
+	case SBFOutputFrequency::Hz25_0:
+		sbf_frequency = k_frequency_25_0hz;
+		break;
+	}
+
+	// Output a set of SBF blocks on a given connection at a regular interval.
+	snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency);
+	if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+		SEP_WARN("CONFIG: Failed to configure SBF");
+		return ConfigureResult::FailedCompletely;
+	}
+
+	if (_receiver_setup == ReceiverSetup::MovingBase) {
+		if (_instance == Instance::Secondary) {
+			snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3");
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure RTCM output");
+			}
+		} else {
+			snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base);
+			if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) {
+				SEP_WARN("CONFIG: Failed to configure attitude source");
+			}
 		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::GLONASS)) {
-			strcat(requested_satellites, "GLONASS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::Galileo)) {
-			strcat(requested_satellites, "GALILEO+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::SBAS)) {
-			strcat(requested_satellites, "SBAS+");
-		}
-		if (_receiver_constellation_usage & static_cast<int32_t>(SatelliteUsage::BeiDou)) {
-			strcat(requested_satellites, "BEIDOU+");
-		}
-		// Make sure to remove the trailing '+' if any.
-		requested_satellites[math::max(static_cast<int>(strlen(requested_satellites)) - 1, 0)] = '\0';
-		snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites);
-		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
-			SEP_ERR("Failed to configure constellation usage");
-			return PX4_ERROR;
+	} else {
+		snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna);
+		// This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement.
+		if (!send_message(msg)) {
+			SEP_WARN("CONFIG: Failed to configure attitude source");
+			return ConfigureResult::FailedCompletely;
 		}
 	}

-	return PX4_OK;
+	return result;
 }

 int SeptentrioDriver::parse_char(const uint8_t byte)
@@ -1035,36 +1086,37 @@ int SeptentrioDriver::process_message()
 			PVTGeodetic pvt_geodetic;

 			if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) {
-				if (pvt_geodetic.mode_type < ModeType::StandAlonePVT) {
+				switch (pvt_geodetic.mode_type) {
+				case ModeType::NoPVT:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
-
-				} else if (pvt_geodetic.mode_type == ModeType::PVTWithSBAS) {
+					break;
+				case ModeType::PVTWithSBAS:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFloat || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFloat) {
+					break;
+				case ModeType::RTKFloat:
+				case ModeType::MovingBaseRTKFloat:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT;
-
-				} else if (pvt_geodetic.mode_type == ModeType::RTKFixed || pvt_geodetic.mode_type == ModeType::MovingBaseRTKFixed) {
+					break;
+				case ModeType::RTKFixed:
+				case ModeType::MovingBaseRTKFixed:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED;
-
-				} else {
+					break;
+				default:
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D;
+					break;
 				}

-				// Check fix and error code
-				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
-
 				// Check boundaries and invalidate GPS velocities
 				if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) {
 					_message_gps_state.vel_ned_valid = false;
 				}

-				// Check boundaries and invalidate position
-				// We're not just checking for the do-not-use value but for any value beyond the specified max values
-				if (pvt_geodetic.latitude <= k_dnu_f8_value ||
-				    pvt_geodetic.longitude <= k_dnu_f8_value ||
-				    pvt_geodetic.height <= k_dnu_f8_value ||
-				    pvt_geodetic.undulation <= k_dnu_f4_value) {
+				if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) {
+					_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
+					_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
+					_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
+					_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
+				} else {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}

@@ -1082,23 +1134,22 @@ int SeptentrioDriver::process_message()
 					_message_gps_state.satellites_used = 0;
 				}

-				_message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG;
-				_message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG;
-				_message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast<double>(pvt_geodetic.undulation);
-				_message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height;
-
 				/* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS.
 				 * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */
 				_message_gps_state.eph = static_cast<float>(pvt_geodetic.h_accuracy) / 200.0f;
 				_message_gps_state.epv = static_cast<float>(pvt_geodetic.v_accuracy) / 200.0f;

+				// Check fix and error code
+				_message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None;
 				_message_gps_state.vel_n_m_s = pvt_geodetic.vn;
 				_message_gps_state.vel_e_m_s = pvt_geodetic.ve;
 				_message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu;
 				_message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s +
 							_message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s);

-				_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				if (pvt_geodetic.cog > k_dnu_f4_value) {
+					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
+				}
 				_message_gps_state.c_variance_rad = M_DEG_TO_RAD_F;

 				_message_gps_state.time_utc_usec = 0;
@@ -1178,14 +1229,8 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;

 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_ve_ve;
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vn_vn) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vn_vn;
-				}
-
-				if (_message_gps_state.s_variance_m_s < vel_cov_geodetic.cov_vu_vu) {
-					_message_gps_state.s_variance_m_s = vel_cov_geodetic.cov_vu_vu;
+				if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) {
+					_message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu);
 				}
 			}

@@ -1203,10 +1248,11 @@ int SeptentrioDriver::process_message()

 			AttEuler att_euler;

-			if (_sbf_decoder.parse(&att_euler) &&
+			if (_sbf_decoder.parse(&att_euler) == PX4_OK &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None) {
+			    att_euler.error_aux2 == Error::None &&
+			    att_euler.heading > k_dnu_f4_value) {
 				float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI].

 				// Ensure range is in [-PI, PI].
@@ -1230,7 +1276,8 @@ int SeptentrioDriver::process_message()
 			if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK &&
 			    !att_cov_euler.error_not_requested &&
 			    att_cov_euler.error_aux1 == Error::None &&
-			    att_cov_euler.error_aux2 == Error::None) {
+			    att_cov_euler.error_aux2 == Error::None &&
+			    att_cov_euler.cov_headhead > k_dnu_f4_value) {
 				_message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI]
 			}

@@ -1289,13 +1336,16 @@ bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int
 				return true;
 			} else if (expected_response[response_check_character] == buf[i]) {
 				++response_check_character;
+			} else if (buf[i] == '$') {
+				// Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`)
+				response_check_character = 1;
 			} else {
 				response_check_character = 0;
 			}
 		}
 	} while (timeout_time > hrt_absolute_time());

-	PX4_DEBUG("Response: timeout");
+	SEP_WARN("Response: timeout");
 	return false;
 }

@@ -1492,10 +1542,6 @@ void SeptentrioDriver::publish()

 	_sensor_gps_pub.publish(_message_gps_state);

-	// Heading/yaw data can be updated at a lower rate than the other navigation data.
-	// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
-	_message_gps_state.heading = NAN;
-
 	if (_message_gps_state.spoofing_state != _spoofing_state) {

 		if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) {
@@ -1523,6 +1569,11 @@ void SeptentrioDriver::publish_satellite_info()
 	}
 }

+bool SeptentrioDriver::first_gps_uorb_message_created() const
+{
+	return _message_gps_state.timestamp != 0;
+}
+
 void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len)
 {
 	gps_inject_data_s gps_inject_data{};
@@ -1668,13 +1719,11 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)

 bool SeptentrioDriver::is_healthy() const
 {
-	if (_state == State::ReceivingData) {
-		if (!receiver_configuration_healthy()) {
-			return false;
-		}
+	if (_state == State::ReceivingData && receiver_configuration_healthy()) {
+		return true;
 	}

-	return true;
+	return false;
 }

 void SeptentrioDriver::reset_gps_state_message()
@@ -47,6 +47,7 @@
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/Serial.hpp>
+#include <uORB/uORB.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
@@ -271,9 +272,20 @@ public:
 	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise
 	 */
 	int force_input();
+
+	/**
+	 * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them.
+	 */
+	static uint32_t k_supported_baud_rates[];
+
+	/**
+	 * Default baud rate, used when the user requested an invalid baud rate.
+	 */
+	static uint32_t k_default_baud_rate;
 private:
 	enum class State {
 		OpeningSerialPort,
+		DetectingBaudRate,
 		ConfiguringDevice,
 		ReceivingData,
 	};
@@ -295,9 +307,24 @@ private:
 	};

 	/**
-	 * Maximum timeout to wait for command acknowledgement by the receiver.
+	 * The result of trying to configure the receiver.
+	 */
+	enum class ConfigureResult : int32_t {
+		OK               = 0,
+		FailedCompletely = 1 << 0,
+		NoLogging        = 1 << 1,
+	};
+
+	/**
+	 * Maximum timeout to wait for fast command acknowledgement by the receiver.
 	*/
-	static constexpr uint16_t k_receiver_ack_timeout = 200;
+	static constexpr uint16_t k_receiver_ack_timeout_fast = 200;
+
+	/**
+	 * Maximum timeout to wait for slow command acknowledgement by the receiver.
+	 * Might be the case for commands that send more output back as reply.
+	 */
+	static constexpr uint16_t k_receiver_ack_timeout_slow = 400;

 	/**
 	 * Duration of one update monitoring interval in us.
@@ -306,6 +333,11 @@ private:
 	*/
 	static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s;

+	/**
+	 * uORB type to send messages to ground control stations.
+	 */
+	static orb_advert_t k_mavlink_log_pub;
+
 	/**
 	 * The default stream for output of PVT data.
 	*/
@@ -347,13 +379,15 @@ private:
 	void schedule_reset(ReceiverResetType type);

 	/**
-	 * @brief Detect the current baud rate used by the receiver on the connected port.
+	 * @brief Detect whether the receiver is running at the given baud rate.
+	 * Does not preserve local baud rate!
 	 *
-	 * @param force_input Choose whether the receiver forces input on the port
+	 * @param baud_rate The baud rate to check.
+	 * @param force_input Choose whether the receiver forces input on the port.
 	 *
-	 * @return The detected baud rate on success, or `0` on error
+	 * @return `true` if running at the baud rate, or `false` on error.
 	 */
-	uint32_t detect_receiver_baud_rate(bool forced_input);
+	bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input);

 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -369,9 +403,9 @@ private:
 	 *
 	 * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...).
 	 *
-	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
+	 * @return `ConfigureResult::OK` if configured, or error.
 	 */
-	int configure();
+	ConfigureResult configure();

 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -505,6 +539,13 @@ private:
 	 */
 	void publish_satellite_info();

+	/**
+	 * @brief Check whether the driver has created its first complete `SensorGPS` uORB message.
+	 *
+	 * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting.
+	 */
+	bool first_gps_uorb_message_created() const;
+
 	/**
 	 * @brief Publish RTCM corrections.
 	 *
@@ -579,6 +620,9 @@ private:
 	/**
 	 * @brief Check whether the current receiver configuration is likely healthy.
 	 *
+	 * This is checked by passively monitoring output from the receiver and validating whether it is what is
+	 * expected.
+	 *
 	 * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured.
 	*/
 	bool receiver_configuration_healthy() const;
@@ -611,6 +655,9 @@ private:
 	/**
 	 * @brief Check whether the driver is operating correctly.
 	 *
+	 * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the
+	 * expected data.
+	 *
 	 * @return `true` if the driver is working as expected, `false` otherwise.
 	*/
 	bool is_healthy() const;
@@ -666,7 +713,7 @@ private:
 	uint8_t                                _spoofing_state {0};                                          ///< Receiver spoofing state
 	uint8_t                                _jamming_state {0};                                           ///< Receiver jamming state
 	const Instance                         _instance {Instance::Main};                                   ///< The receiver that this instance of the driver controls
-	uint32_t                               _baud_rate {0};
+	uint32_t                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -683,6 +730,7 @@ private:
 	bool                                   _automatic_configuration {true};                              ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter
 	ReceiverSetup                          _receiver_setup {ReceiverSetup::Default};                     ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter
 	int32_t                                _receiver_constellation_usage {0};                            ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter
+	uint8_t                                _current_baud_rate_index {0};                                 ///< Index of the current baud rate to check

 	// Decoding and parsing
 	DecodingStatus                                 _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into
@@ -39,6 +39,8 @@

 #pragma once

+#include <stdint.h>
+
 namespace septentrio
 {

@@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y)
 	int ret = PX4_OK;

 	// TODO: show battery symbol based on battery fill level
-	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v);
+	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v);
 	buf[sizeof(buf) - 1] = '\0';

 	for (int i = 0; buf[i] != '\0'; i++) {
@@ -330,7 +330,7 @@ OSDatxxxx::update_topics()
 		_battery_sub.copy(&battery);

 		if (battery.connected) {
-			_battery_voltage_filtered_v = battery.voltage_filtered_v;
+			_battery_voltage_v = battery.voltage_v;
 			_battery_discharge_mah = battery.discharged_mah;
 			_battery_valid = true;

@@ -111,7 +111,7 @@ private:
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

 	// battery
-	float _battery_voltage_filtered_v{0.f};
+	float _battery_voltage_v{0.f};
 	float _battery_discharge_mah{0.f};
 	bool _battery_valid{false};

@@ -224,8 +224,8 @@ void CrsfRc::Run()
 				battery_status_s battery_status;

 				if (_battery_status_sub.update(&battery_status)) {
-					uint16_t voltage = battery_status.voltage_filtered_v * 10;
-					uint16_t current = battery_status.current_filtered_a * 10;
+					uint16_t voltage = battery_status.voltage_v * 10;
+					uint16_t current = battery_status.current_a * 10;
 					int fuel = battery_status.discharged_mah;
 					uint8_t remaining = battery_status.remaining * 100;
 					this->SendTelemetryBattery(voltage, current, fuel, remaining);
@@ -241,7 +241,7 @@ void CrsfRc::Run()
 					int32_t longitude = static_cast<int32_t>(round(sensor_gps.longitude_deg * 1e7));
 					uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f;
 					uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f;
-					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m) + 1000;
 					uint8_t num_satellites = sensor_gps.satellites_used;
 					this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites);
 				}
@@ -81,8 +81,8 @@ bool CRSFTelemetry::send_battery()
 		return false;
 	}

-	uint16_t voltage = battery_status.voltage_filtered_v * 10;
-	uint16_t current = battery_status.current_filtered_a * 10;
+	uint16_t voltage = battery_status.voltage_v * 10;
+	uint16_t current = battery_status.current_a * 10;
 	int fuel = battery_status.discharged_mah;
 	uint8_t remaining = battery_status.remaining * 100;
 	return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining);
@@ -90,8 +90,8 @@ bool GHSTTelemetry::send_battery_status()
 	battery_status_s battery_status;

 	if (_battery_status_sub.update(&battery_status)) {
-		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA;
+		voltage_in_10mV = battery_status.voltage_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA;
 		fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH;
 		success = ghst_send_telemetry_battery_status(_uart_fd,
 				static_cast<uint16_t>(voltage_in_10mV),
@@ -146,13 +146,11 @@ void Batmon::RunImpl()

 	// Convert millivolts to volts.
 	new_report.voltage_v = ((float)result) / 1000.0f;
-	new_report.voltage_filtered_v = new_report.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f);
-	new_report.current_filtered_a = new_report.current_a;

 	// Read average current.
 	ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result);
@@ -115,9 +115,7 @@ void TattuCan::Run()
 		battery_status.state_of_health = static_cast<uint16_t>(tattu_message.health_status);

 		battery_status.voltage_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
-		battery_status.voltage_filtered_v = static_cast<float>(tattu_message.voltage) / 1000.0f;
 		battery_status.current_a = static_cast<float>(tattu_message.current) / 1000.0f;
-		battery_status.current_filtered_a = static_cast<float>(tattu_message.current) / 1000.0f;
 		battery_status.remaining = static_cast<float>(tattu_message.remaining_percent) / 100.0f;
 		battery_status.temperature = static_cast<float>(tattu_message.temperature);
 		battery_status.capacity = tattu_message.standard_capacity;
@@ -104,9 +104,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::

 	_battery_status[instance].timestamp = hrt_absolute_time();
 	_battery_status[instance].voltage_v = msg.voltage;
-	_battery_status[instance].voltage_filtered_v = msg.voltage;
 	_battery_status[instance].current_a = msg.current;
-	_battery_status[instance].current_filtered_a = msg.current;
 	_battery_status[instance].current_average_a = msg.current;

 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -436,7 +436,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	}

 	// If we haven't already done so, set the system clock using GPS data
-	if (valid_pos_cov && !_system_clock_set) {
+	if ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
 		timespec ts{};

 		// get the whole microseconds
@@ -46,6 +46,7 @@
 #include <px4_platform_common/defines.h>

 using namespace time_literals;
+using namespace matrix;

 Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) :
 	ModuleParams(parent),
@@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_source(source)
 {
 	const float expected_filter_dt = static_cast<float>(sample_interval_us) / 1_s;
-	_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
-	_current_filter_a.setParameters(expected_filter_dt, .5f);
 	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
-	_throttle_filter.setParameters(expected_filter_dt, 1.f);
+	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
+	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);

 	if (index > 9 || index < 1) {
 		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index);
 	_param_handles.capacity = param_find(param_name);

-	snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index);
-	_param_handles.v_load_drop = param_find(param_name);
-
 	snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index);
 	_param_handles.r_internal = param_find(param_name);

@@ -97,29 +94,36 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");

 	updateParams();
+
+	// Internal resistance estimation initializations
+	_RLS_est(0) = OCV_DEFAULT * _params.n_cells;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(0, 1) = 0.f;
+	_estimation_covariance(1, 0) = 0.f;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
 }

 void Battery::updateVoltage(const float voltage_v)
 {
 	_voltage_v = voltage_v;
-	_voltage_filter_v.update(voltage_v);
 }

 void Battery::updateCurrent(const float current_a)
 {
 	_current_a = current_a;
-	_current_filter_a.update(current_a);
 }

 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
 	if (!_battery_initialized) {
-		_voltage_filter_v.reset(_voltage_v);
-		_current_filter_a.reset(_current_a);
+		resetInternalResistanceEstimation(_voltage_v, _current_a);
 	}

 	// Require minimum voltage otherwise override connected status
-	if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
+	if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) {
 		_connected = false;
 	}

@@ -132,7 +136,7 @@ void Battery::updateBatteryStatus(const hrt_abstime &timestamp)

 	sumDischarged(timestamp, _current_a);
 	_state_of_charge_volt_based =
-		calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState());
+		calculateStateOfChargeVoltageBased(_voltage_v, _current_a);

 	if (!_external_state_of_charge) {
 		estimateStateOfCharge();
@@ -149,9 +153,7 @@ battery_status_s Battery::getBatteryStatus()
 {
 	battery_status_s battery_status{};
 	battery_status.voltage_v = _voltage_v;
-	battery_status.voltage_filtered_v = _voltage_filter_v.getState();
 	battery_status.current_a = _current_a;
-	battery_status.current_filtered_a = _current_filter_a.getState();
 	battery_status.current_average_a = _current_average_filter_a.getState();
 	battery_status.discharged_mah = _discharged_mah;
 	battery_status.remaining = _state_of_charge;
@@ -167,6 +169,14 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.warning = _warning;
 	battery_status.timestamp = hrt_absolute_time();
 	battery_status.faults = determineFaults();
+	battery_status.internal_resistance_estimate = _internal_resistance_estimate;
+	battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a;
+	battery_status.ocv_estimate_filtered = _ocv_filter_v.getState();
+	battery_status.volt_based_soc_estimate = math::interpolate(_ocv_filter_v.getState() / _params.n_cells,
+			_params.v_empty, _params.v_charged, 0.f, 1.f);
+	battery_status.voltage_prediction = _voltage_prediction;
+	battery_status.prediction_error = _prediction_error;
+	battery_status.estimation_covariance_norm = _estimation_covariance_norm;
 	return battery_status;
 }

@@ -213,27 +223,69 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f
 	// remaining battery capacity based on voltage
 	float cell_voltage = voltage_v / _params.n_cells;

-	// correct battery voltage locally for load drop to avoid estimation fluctuations
-	if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) {
-		cell_voltage += _params.r_internal * current_a;
+	// correct battery voltage locally for load drop according to internal resistance and current
+	if (current_a > FLT_EPSILON) {
+		updateInternalResistanceEstimation(voltage_v, current_a);

-	} else {
-		vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-		_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint);
-		const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
-		const float throttle = thrust_setpoint.length();
+		if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+			cell_voltage += _params.r_internal * current_a;

-		_throttle_filter.update(throttle);
-
-		if (!_battery_initialized) {
-			_throttle_filter.reset(throttle);
+		} else { // Use estimated internal resistance value
+			cell_voltage += _internal_resistance_estimate * current_a;
 		}

-		// assume linear relation between throttle and voltage drop
-		cell_voltage += throttle * _params.v_load_drop;
 	}

-	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	_cell_voltage_filter_v.update(cell_voltage);
+	return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f);
+}
+
+void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	Vector2f x{1, -current_a};
+	_voltage_prediction = (x.transpose() * _RLS_est)(0, 0);
+	_prediction_error = voltage_v - _voltage_prediction;
+	const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0));
+	const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error;
+	const Matrix2f estimation_covariance_temp = (_estimation_covariance
+			- Matrix<float, 2, 1>(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA;
+	const float estimation_covariance_temp_norm =
+		sqrtf(powf(estimation_covariance_temp(0, 0), 2.f)
+		      + 2.f * powf(estimation_covariance_temp(1, 0), 2.f)
+		      + powf(estimation_covariance_temp(1, 1), 2.f));
+
+	if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves
+		_RLS_est = RSL_est_temp;
+		_estimation_covariance = estimation_covariance_temp;
+		_estimation_covariance_norm = estimation_covariance_temp_norm;
+		_internal_resistance_estimate =
+			math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values
+
+	} else { // Update OCV estimate with IR estimate
+		_RLS_est(0) = voltage_v + _RLS_est(1) * current_a;
+	}
+
+	_ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+}
+
+void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a)
+{
+	_RLS_est(0) = voltage_v;
+	_RLS_est(1) = R_DEFAULT * _params.n_cells;
+	_estimation_covariance.setZero();
+	_estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells;
+	_estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells;
+	_estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0),
+					    2.f) + powf(_estimation_covariance(1, 1), 2.f));
+	_internal_resistance_estimate = R_DEFAULT;
+	_ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a);
+
+	if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a);
+
+	} else { // Use estimated internal resistance value
+		_cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a);
+	}
 }

 void Battery::estimateStateOfCharge()
@@ -354,7 +406,6 @@ void Battery::updateParams()
 	param_get(_param_handles.v_charged, &_params.v_charged);
 	param_get(_param_handles.n_cells, &_params.n_cells);
 	param_get(_param_handles.capacity, &_params.capacity);
-	param_get(_param_handles.v_load_drop, &_params.v_load_drop);
 	param_get(_param_handles.r_internal, &_params.r_internal);
 	param_get(_param_handles.source, &_params.source);
 	param_get(_param_handles.low_thr, &_params.low_thr);
@@ -58,7 +58,6 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/flight_phase_estimation.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_thrust_setpoint.h>

 /**
 * BatteryBase is a base class for any type of battery.
@@ -118,7 +117,6 @@ protected:
 		param_t v_charged;
 		param_t n_cells;
 		param_t capacity;
-		param_t v_load_drop;
 		param_t r_internal;
 		param_t low_thr;
 		param_t crit_thr;
@@ -132,7 +130,6 @@ protected:
 		float v_charged;
 		int32_t  n_cells;
 		float capacity;
-		float v_load_drop;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -155,7 +152,6 @@ private:
 	void computeScale();
 	float computeRemainingTime(float current_a);

-	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
@@ -167,12 +163,11 @@ private:
 	uint8_t _priority{0};
 	bool _battery_initialized{false};
 	float _voltage_v{0.f};
-	AlphaFilter<float> _voltage_filter_v;
+	AlphaFilter<float> _ocv_filter_v;
+	AlphaFilter<float> _cell_voltage_filter_v;
 	float _current_a{-1};
-	AlphaFilter<float> _current_filter_a;
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
-	AlphaFilter<float> _throttle_filter;
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
@@ -183,4 +178,19 @@ private:
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};
 	hrt_abstime _last_unconnected_timestamp{0};
+
+	// Internal Resistance estimation
+	void updateInternalResistanceEstimation(const float voltage_v, const float current_a);
+	void resetInternalResistanceEstimation(const float voltage_v, const float current_a);
+	matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T
+	matrix::Matrix2f _estimation_covariance;
+	float _estimation_covariance_norm{0.f};
+	float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance
+	float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator
+	float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage
+	static constexpr float LAMBDA = 0.95f; 	// [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm)
+	static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance
+	static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage
+	static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance
+	static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage
 };
@@ -0,0 +1,208 @@
+# Test internal resistance estimator on flight logs
+# run with:
+# python3 int_res_est_replay.py -f <pathToLogFile> -c <#batteryCells>
+#   -u <(optional)fullCellVoltage> -e <(optional)emptyCellVoltage> -l <(optional)forgettingFactor> -d <(optional)filterMeasurements>
+# Note: Can lead to slightly different results than the online estimation due to the fact that
+# the log frequency of the voltage and current are not the same as the online frequency.
+
+from pyulog import ULog
+import matplotlib.pyplot as plt
+import numpy as np
+import argparse
+
+def getData(log, topic_name, variable_name, instance=0):
+    for elem in log.data_list:
+        if elem.name == topic_name and instance == elem.multi_id:
+            return elem.data[variable_name]
+    return np.array([])
+
+def us2s(time_us):
+    return time_us * 1e-6
+
+def getParam(log, param_name):
+    if param_name in log.initial_parameters:
+        return log.initial_parameters[param_name]
+    else:
+        print(f"Parameter {param_name} not found in log.")
+    return None
+
+def rls_update(theta, P, x, V, I, lam):
+    gamma = P @ x / (lam + x.T @ P @ x)
+    error = V - x.T @ theta
+    data_cov = x.T @ P @ x
+    theta_temp = theta + gamma * error
+    P_temp = (P - gamma @ x.T @ P) / lam
+    if (abs(np.linalg.norm(P)) < abs(np.linalg.norm(P_temp))):
+        theta_corr = np.array([V + theta[1] * I, theta[1]]) # Correct OCV estimation
+        P_corr = P
+        return theta_corr, P_corr, error, data_cov, 0, 0
+    return theta_temp, P_temp, error, data_cov, gamma[0], gamma[1]
+
+def main(log_name, n_cells, full_cell, empty_cell, lam):
+    log = ULog(log_name)
+
+    log_n_cells = getParam(log, 'BAT1_N_CELLS')
+    log_full_cell = getParam(log, 'BAT1_V_CHARGED')
+    log_empty_cell = getParam(log, 'BAT1_V_EMPTY')
+
+    # Debug information
+    print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}")
+
+    # Use log parameters unless overridden
+    if n_cells is None:
+        n_cells = log_n_cells
+    else:
+        print(f"Using override for n_cells: {n_cells}")
+    if full_cell is None:
+        full_cell = log_full_cell
+    else:
+        print(f"Using override for full_cell: {full_cell}")
+    if empty_cell is None:
+        empty_cell = log_empty_cell
+    else:
+        print(f"Using override for empty_cell: {empty_cell}")
+
+    # Debug information for final parameter values
+    print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}")
+
+    timestamps = us2s(getData(log, 'battery_status', 'timestamp'))
+    I = getData(log, 'battery_status', 'current_a')
+    V = getData(log, 'battery_status', 'voltage_v')
+    remaining = getData(log, 'battery_status', 'remaining')
+
+    if not timestamps.size or not I.size or not V.size or not remaining.size:
+        print("Error: Incomplete data.")
+        return
+
+    # Initializations
+    theta = np.array([[V[0] + 0.005 * n_cells * I[0]], [0.005 * n_cells]])  # Initial VOC and R
+    P = np.diag([1.2 * n_cells, 0.1 * n_cells]) # Initial covariance
+    error = 0
+
+    # For plotting
+    VOC_est = np.zeros_like(I)
+    R_est = np.zeros_like(I)
+    error_hist = np.zeros_like(I)
+    v_est = np.zeros_like(I)
+    internal_resistance_stable = np.zeros_like(I)
+    internal_resistance_stable[-1] = 0.005
+    cov_norm = np.zeros_like(I)
+    r_cov = np.zeros_like(I)
+    ocv_cov = np.zeros_like(I)
+    mixed_cov = np.zeros_like(I)
+    data_cov_hist = np.zeros_like(I)
+    gamma_voc_hist = np.zeros_like(I)
+    gamma_r_hist = np.zeros_like(I)
+
+    for index in range(len(I)):
+        # RLS algorithm
+        x = np.array([[1.0], [-I[index]]]) # Input vector
+        theta, P, error, data_cov, gamma_voc_hist[index], gamma_r_hist[index] = rls_update(theta, P, x, V[index], I[index], lam) # Run RLS
+
+        # For plotting
+        VOC_est[index] = theta[0][0]
+        R_est[index] = theta[1][0]
+        error_hist[index] = error
+        v_est[index] = x.T @ theta
+        cov_norm[index] = abs(np.linalg.norm(P))
+        ocv_cov[index] = P[0][0]
+        r_cov[index] = P[1][1]
+        mixed_cov[index] = P[0][1]
+        data_cov_hist[index] = data_cov
+        internal_resistance_stable[index] = max(R_est[index]/n_cells, 0.001)
+
+    # Plot data
+    print("Internal Resistance mean (per cell): ", np.mean(R_est) / n_cells)
+
+    # Summary plot
+    sumFig = plt.figure("Battery Estimation with RLS")
+
+    volt = plt.subplot(2, 3, 1)
+    volt.plot(timestamps, V, label='Measured voltage')
+    volt.plot(timestamps, v_est, label='Estimated voltage')
+    volt.plot(timestamps, np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, label='OCV estimate')
+    ocv_smoothed = np.convolve(np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, np.ones(30)/30, mode='full')[0:np.size(timestamps)]
+    ocv_smoothed[0:30] = ocv_smoothed[31]
+    volt.plot(timestamps, ocv_smoothed, label='OCV estimate smoothed')
+    volt.plot(timestamps, np.full_like(I, full_cell * n_cells), label='100% SOC')
+    volt.set_title("Measured Voltage vs Estimated voltage vs Estimated Open circuit voltage [V]")
+    volt.legend()
+
+    intR = plt.subplot(2, 3, 2)
+    intR.plot(timestamps, np.array(R_est) * 1000 / n_cells, label='Internal resistance estimate')
+    intR.set_title("Internal resistance estimate (per cell) [mOhm]")
+    intR.legend()
+
+    soc = plt.subplot(2, 3, 3)
+    soc.plot(timestamps, remaining, label='SoC logged')
+    soc.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    soc.plot(timestamps, np.interp(ocv_smoothed / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator smoothed')
+    soc.set_title("State of charge")
+    soc.legend()
+
+    curr = plt.subplot(2, 3, 4)
+    curr.plot(timestamps, I, label='Measured current')
+    curr.set_title("Measured Current [A]")
+    curr.legend()
+
+    err = plt.subplot(2, 3, 5)
+    err.plot(timestamps, error_hist, label='$Error$')
+    err.set_title("Voltage estimation error [V]")
+    err.legend()
+
+    cov = plt.subplot(2, 3, 6)
+    cov.plot(timestamps, cov_norm, label = 'Covariance norm')
+    cov.set_title("Covariance norm")
+    cov.legend()
+
+    # # SoC estimation plots
+    # socFig = plt.figure("SoC estimation")
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.005 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with default $R_{int}$')
+    # plt.plot(timestamps, remaining, label='SoC logged')
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    # # plt.plot(timestamps, np.convolve(np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), np.ones(500)/500, mode='full')[0:np.size(timestamps)], label='SoC with estimator smoothed')
+    # # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.0009 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with $R_{int}$ measured beforehand')
+    # # plt.plot(timestamps, np.interp(VOC_est/n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with VOC estimate')
+    # plt.legend()
+
+    # # Covariance plots
+    # covFig = plt.figure("Covariance plots")
+    # covR = plt.subplot(2, 2, 1)
+    # covR.plot(timestamps, r_cov, label = 'r_cov')
+    # covR.set_title("Internal resistance covariance")
+    # covR.legend()
+    # covVOC = plt.subplot(2, 2, 2)
+    # covVOC.plot(timestamps, ocv_cov, label = 'ocv_cov')
+    # covVOC.set_title("Open circuit covariance")
+    # covVOC.legend()
+    # covM = plt.subplot(2, 2, 3)
+    # covM.plot(timestamps, mixed_cov, label = 'mixed_cov')
+    # covM.set_title("Mixed covariance")
+    # covM.legend()
+    # covM = plt.subplot(2, 2, 4)
+    # covM.plot(timestamps, cov_norm, label = 'cov_norm')
+    # covM.set_title("Covariance norm")
+    # covM.legend()
+
+    # # Gain plots
+    # gainFig = plt.figure("Gain plots")
+    # gainVoc = plt.subplot(1, 2, 1)
+    # gainVoc.plot(timestamps, gamma_voc_hist, label = 'gain_voc')
+    # gainVoc.set_title("Gain VOC")
+    # gainVoc.legend()
+    # gainR = plt.subplot(1, 2, 2)
+    # gainR.plot(timestamps, gamma_r_hist, label = 'gain_r')
+    # gainR.set_title("Gain R")
+    # gainR.legend()
+
+    plt.show()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.')
+    parser.add_argument('-f', type = str,   required = True,  help = 'Full path to ulog file')
+    parser.add_argument('-c', type = float, required = False, help = 'Number of cells in battery')
+    parser.add_argument('-u', type = float, required = False, default = None, help = 'Full cell voltage')
+    parser.add_argument('-e', type = float, required = False, default = None,  help = 'Empty cell voltage')
+    parser.add_argument('-l', type = float, required = False, default = 0.99, help = 'Forgetting factor')
+    args = parser.parse_args()
+    main(args.f, args.c, args.u, args.e, args.l)
@@ -39,33 +39,11 @@ parameters:
            instance_start: 1
            default: [4.05, 4.05, 4.05]

-        BAT${i}_V_LOAD_DROP:
-            description:
-                short: Voltage drop per cell on full throttle
-                long: |
-                    This implicitly defines the internal resistance
-                    to maximum current ratio for the battery and assumes linearity.
-                    A good value to use is the difference between the
-                    5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is
-                    set.
-
-            type: float
-            unit: V
-            min: 0.07
-            max: 0.5
-            decimal: 2
-            increment: 0.01
-            reboot_required: true
-            num_instances: *max_num_config_instances
-            instance_start: 1
-            default: [0.1, 0.1, 0.1]
-
        BAT${i}_R_INTERNAL:
            description:
                short: Explicitly defines the per cell internal resistance for battery ${i}
                long: |
-                    If non-negative, then this will be used in place of
-                    BAT${i}_V_LOAD_DROP for all calculations.
+                    If non-negative, then this will be used instead of the online estimated internal resistance.

            type: float
            unit: Ohm
@@ -76,7 +54,7 @@ parameters:
            reboot_required: true
            num_instances: *max_num_config_instances
            instance_start: 1
-            default: [0.005, 0.005, 0.005]
+            default: [-1.0, -1.0, -1.0]

        BAT${i}_N_CELLS:
            description:
@@ -261,13 +261,11 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)

 	// Convert millivolts to volts.
 	data.voltage_v = ((float)result) * 0.001f;
-	data.voltage_filtered_v = data.voltage_v;

 	// Read current.
 	ret |= _interface->read_word(BATT_SMBUS_CURRENT, result);

 	data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f);
-	data.current_filtered_a = data.current_a;

 	// Read remaining capacity.
 	ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result);
@@ -131,7 +131,6 @@ private:
 		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
 		(ParamFloat<px4::params::FW_SERVICE_CEIL>) _param_service_ceiling,
 		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
-		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
 		(ParamFloat<px4::params::FW_THR_ASPD_MIN>) _param_fw_thr_aspd_min,
@@ -33,6 +33,7 @@

 px4_add_library(mathlib
 	math/test/test.cpp
+	math/filter/FilteredDerivative.hpp
 	math/filter/LowPassFilter2p.hpp
 	math/filter/MedianFilter.hpp
 	math/filter/NotchFilter.hpp
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FilteredDerivative.hpp
+ *
+ * @brief Derivative function passed through a first order "alpha" IIR digital filter
+ *
+ * @author Silvan Fuhrer <silvan@auterion.com>
+ */
+
+#pragma once
+
+// #include <float.h>
+// #include <mathlib/math/Functions.hpp>
+#include <mathlib/math/filter/AlphaFilter.hpp>
+
+using namespace math;
+
+template <typename T>
+class FilteredDerivative
+{
+public:
+	FilteredDerivative() = default;
+	~FilteredDerivative() = default;
+
+	/**
+	 * Set filter parameters for time abstraction
+	 *
+	 * Both parameters have to be provided in the same units.
+	 *
+	 * @param sample_interval interval between two samples
+	 * @param time_constant filter time constant determining convergence
+	 */
+	void setParameters(float sample_interval, float time_constant)
+	{
+		_alpha_filter.setParameters(sample_interval, time_constant);
+		_sample_interval = sample_interval;
+	}
+
+	/**
+	 * Set filter state to an initial value
+	 *
+	 * @param sample new initial value
+	 */
+	void reset(const T &sample)
+	{
+		_alpha_filter.reset(sample);
+		_initialized = false;
+	}
+
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	const T &update(const T &sample)
+	{
+		if (_initialized) {
+			if (_sample_interval > FLT_EPSILON) {
+				_alpha_filter.update((sample - _previous_sample) / _sample_interval);
+
+			} else {
+				_initialized = false;
+			}
+
+		} else {
+			// don't update in the first iteration
+			_initialized = true;
+		}
+
+		_previous_sample = sample;
+		return _alpha_filter.getState();
+	}
+
+	const T &getState() const { return _alpha_filter.getState(); }
+
+
+private:
+	AlphaFilter<T> _alpha_filter;
+	float _sample_interval{0.f};
+	T _previous_sample{0.f};
+	bool _initialized{false};
+};
@@ -618,6 +618,7 @@ SquareMatrix <Type, M> choleskyInv(const SquareMatrix<Type, M> &A)
 	return L_inv.T() * L_inv;
 }

+using Matrix2f = SquareMatrix<float, 2>;
 using Matrix3f = SquareMatrix<float, 3>;
 using Matrix3d = SquareMatrix<double, 3>;

@@ -60,6 +60,8 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	check_load_factor(input_data.accel_z);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
 				  input_data.ground_velocity, input_data.gnss_valid);
+	check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle,
+			      input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att);
 	update_airspeed_valid_status(input_data.timestamp);
 }

@@ -277,16 +279,76 @@ AirspeedValidator::check_load_factor(float accel_z)
 	}
 }

+void
+AirspeedValidator::check_first_principle(const uint64_t timestamp, const float throttle_fw, const float throttle_trim,
+		const uint64_t tecs_timestamp, const Quatf &att_q)
+{
+	if (! _first_principle_check_enabled) {
+		_first_principle_check_failed = false;
+		_time_last_first_principle_check_passing = timestamp;
+		return;
+	}
+
+	const float pitch = matrix::Eulerf(att_q).theta();
+	const hrt_abstime tecs_dt = timestamp - tecs_timestamp; // return if TECS data is old (TECS not running)
+
+	if (!_in_fixed_wing_flight || tecs_dt > 500_ms || !PX4_ISFINITE(_IAS) || !PX4_ISFINITE(throttle_fw)
+	    || !PX4_ISFINITE(throttle_trim) || !PX4_ISFINITE(pitch)) {
+		// do not do anything in that case
+		return;
+	}
+
+	const float dt = static_cast<float>(timestamp - _time_last_first_principle_check) / 1_s;
+	_time_last_first_principle_check = timestamp;
+
+	// update filters
+	if (dt < FLT_EPSILON || dt > 1.f) {
+		// reset if dt is too large
+		_IAS_derivative.reset(0.f);
+		_throttle_filtered.reset(throttle_fw);
+		_pitch_filtered.reset(pitch);
+		_time_last_first_principle_check_passing = timestamp;
+
+	} else {
+		// update filters, with different time constant
+		_IAS_derivative.setParameters(dt, 5.f);
+		_throttle_filtered.setParameters(dt, 0.5f);
+		_pitch_filtered.setParameters(dt, 1.5f);
+
+		_IAS_derivative.update(_IAS);
+		_throttle_filtered.update(throttle_fw);
+		_pitch_filtered.update(pitch);
+	}
+
+	// declare high throttle if more than 5% above trim
+	const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max);
+	const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold;
+	const bool pitching_down = _pitch_filtered.getState() < _param_psp_off;
+
+	// check if the airspeed derivative is too low given the throttle and pitch
+	const bool check_failing = _IAS_derivative.getState() < kIASDerivateThreshold && high_throttle && pitching_down;
+
+	if (!check_failing) {
+		_time_last_first_principle_check_passing = timestamp;
+		_first_principle_check_failed = false;
+	}
+
+	if (timestamp - _time_last_first_principle_check_passing > _aspd_fp_t_window * 1_s) {
+		// only update the test_failed flag once the timeout since first principle check failing is over
+		_first_principle_check_failed = check_failing;
+	}
+}

 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
+	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed
+	    || _first_principle_check_failed) {
 		// at least one check (data stuck, innovation or load factor) failed, so record timestamp
 		_time_checks_failed = timestamp;

 	} else if (! _data_stuck_test_failed && !_innovations_check_failed
-		   && !_load_factor_check_failed) {
+		   && !_load_factor_check_failed && !_first_principle_check_failed) {
 		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}
@@ -41,6 +41,8 @@
 #include <airspeed/airspeed.h>
 #include <lib/wind_estimator/WindEstimator.hpp>
 #include <uORB/topics/airspeed_wind.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/FilteredDerivative.hpp>


 using matrix::Dcmf;
@@ -66,6 +68,9 @@ struct airspeed_validator_update_data {
 	float vel_test_ratio;
 	float mag_test_ratio;
 	bool in_fixed_wing_flight;
+	float fixed_wing_tecs_throttle;
+	float fixed_wing_tecs_throttle_trim;
+	uint64_t tecs_timestamp;
 };

 class AirspeedValidator
@@ -83,6 +88,9 @@ public:
 	float get_TAS() { return _TAS; }
 	bool get_airspeed_valid() { return _airspeed_valid; }
 	float get_CAS_scale_validated() {return _CAS_scale_validated;}
+	float get_airspeed_derivative() { return _IAS_derivative.getState(); }
+	float get_throttle_filtered() { return _throttle_filtered.getState(); }
+	float get_pitch_filtered() { return _pitch_filtered.getState(); }

 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);

@@ -118,6 +126,10 @@ public:
 	void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; }
 	void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; }
 	void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; }
+	void set_enable_first_principle_check(bool enable) { _first_principle_check_enabled = enable; }
+	void set_psp_off_param(float psp_off_param) { _param_psp_off = psp_off_param; }
+	void set_throttle_max_param(float throttle_max_param) { _param_throttle_max = throttle_max_param; }
+	void set_fp_t_window(float t_window) { _aspd_fp_t_window = t_window; }

 private:

@@ -127,10 +139,17 @@ private:
 	bool _data_stuck_check_enabled{false};
 	bool _innovation_check_enabled{false};
 	bool _load_factor_check_enabled{false};
+	bool _first_principle_check_enabled{false};

 	// airspeed scale validity check
 	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)

+	static constexpr float kHighThrottleDelta =
+		0.05f; ///<  throttle delta above trim throttle required to consider throttle high
+	static constexpr float kIASDerivateThreshold =
+		0.1f; ///< threshold for IAS derivative to detect airspeed failure. Failure is
+	// detected if in a high throttle and low pitch situation and the filtered IAS derivative is below this threshold
+
 	// general states
 	bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks
 	float _IAS{0.0f}; ///< indicated airsped in m/s
@@ -158,6 +177,17 @@ private:
 	float _airspeed_stall{8.0f}; ///< stall speed of aircraft used for load factor check
 	float	_load_factor_ratio{0.5f};	///< ratio of maximum load factor predicted by stall speed to measured load factor

+	// first principle check
+	bool _first_principle_check_failed{false}; ///< first principle check has detected failure
+	float _aspd_fp_t_window{0.f}; ///< time window for first principle check
+	FilteredDerivative<float> _IAS_derivative; ///< indicated airspeed derivative for first principle check
+	AlphaFilter<float> _throttle_filtered; ///< filtered throttle for first principle check
+	AlphaFilter<float> _pitch_filtered; ///< filtered pitch for first principle check
+	hrt_abstime _time_last_first_principle_check{0}; ///< time airspeed first principle was last checked (uSec)
+	hrt_abstime _time_last_first_principle_check_passing{0}; ///< time airspeed first principle was last passing (uSec)
+	float _param_psp_off{0.0f}; ///< parameter pitch in level flight [rad]
+	float _param_throttle_max{0.0f}; ///< parameter maximum throttle value
+
 	// states of airspeed valid declaration
 	bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed)
 	float _checks_fail_delay{2.f}; ///< delay for airspeed invalid declaration after single check failure (Sec)
@@ -185,6 +215,8 @@ private:
 	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
 				       float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool gnss_valid);
 	void check_load_factor(float accel_z);
+	void check_first_principle(const uint64_t timestamp, const float throttle, const float throttle_trim,
+				   const uint64_t tecs_timestamp, const Quatf &att_q);
 	void update_airspeed_valid_status(const uint64_t timestamp);
 	void reset();
 	void reset_CAS_scale_check();
@@ -57,6 +57,7 @@
 #include <uORB/topics/position_setpoint.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -112,6 +113,7 @@ private:

 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};

+	uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)};
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
 	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
@@ -125,6 +127,7 @@ private:
 	uORB::SubscriptionMultiArray<airspeed_s, MAX_NUM_AIRSPEED_SENSORS> _airspeed_subs{ORB_ID::airspeed};


+	tecs_status_s _tecs_status {};
 	estimator_status_s _estimator_status {};
 	vehicle_acceleration_s _accel {};
 	vehicle_air_data_s _vehicle_air_data {};
@@ -162,9 +165,16 @@ private:
 		CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0),
 		CHECK_TYPE_DATA_STUCK_BIT = (1 << 1),
 		CHECK_TYPE_INNOVATION_BIT = (1 << 2),
-		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3)
+		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3),
+		CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4)
 	};

+
+	param_t _param_handle_pitch_sp_offset{PARAM_INVALID};
+	float _param_pitch_sp_offset{0.0f};
+	param_t _param_handle_fw_thr_max{PARAM_INVALID};
+	float _param_fw_thr_max{0.0f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
 		(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
@@ -185,8 +195,12 @@ private:
 		(ParamFloat<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
 		(ParamFloat<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */

+		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas,
+		(ParamFloat<px4::params::ASPD_FP_T_WINDOW>) _aspd_fp_t_window,
+
+		// external parameters
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
-		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
+		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
 	)

 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -203,6 +217,8 @@ AirspeedModule::AirspeedModule():
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_param_handle_pitch_sp_offset = param_find("FW_PSP_OFF");
+	_param_handle_fw_thr_max = param_find("FW_THR_MAX");
 	// initialise parameters
 	update_params();

@@ -355,6 +371,9 @@ AirspeedModule::Run()
 		input_data.accel_z = _accel.xyz[2];
 		input_data.vel_test_ratio = _estimator_status.vel_test_ratio;
 		input_data.mag_test_ratio = _estimator_status.mag_test_ratio;
+		input_data.tecs_timestamp = _tecs_status.timestamp;
+		input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp;
+		input_data.fixed_wing_tecs_throttle_trim = _tecs_status.throttle_trim;

 		// iterate through all airspeed sensors, poll new data from them and update their validators
 		for (int i = 0; i < _number_of_airspeed_sensors; i++) {
@@ -442,6 +461,14 @@ void AirspeedModule::update_params()
 {
 	updateParams();

+	if (_param_handle_pitch_sp_offset != PARAM_INVALID) {
+		param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset);
+	}
+
+	if (_param_handle_fw_thr_max != PARAM_INVALID) {
+		param_get(_param_handle_fw_thr_max, &_param_fw_thr_max);
+	}
+
 	_param_airspeed_scale[0] = _param_airspeed_scale_1.get();
 	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
 	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
@@ -476,6 +503,11 @@ void AirspeedModule::update_params()
 				CheckTypeBits::CHECK_TYPE_INNOVATION_BIT);
 		_airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() &
 				CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT);
+		_airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() &
+				CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT);
+		_airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset));
+		_airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max);
+		_airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get());
 	}
 }

@@ -501,6 +533,8 @@ void AirspeedModule::poll_topics()
 	_vehicle_local_position_sub.update(&_vehicle_local_position);
 	_position_setpoint_sub.update(&_position_setpoint);

+	_tecs_status_sub.update(&_tecs_status);
+
 	if (_vehicle_attitude_sub.updated()) {
 		vehicle_attitude_s vehicle_attitude;
 		_vehicle_attitude_sub.update(&vehicle_attitude);
@@ -661,6 +695,11 @@ void AirspeedModule::select_airspeed_and_publish()
 	airspeed_validated.airspeed_sensor_measurement_valid = false;
 	airspeed_validated.selected_airspeed_index = _valid_airspeed_index;

+	airspeed_validated.airspeed_derivative_filtered = _airspeed_validator[_valid_airspeed_index -
+					      1].get_airspeed_derivative();
+	airspeed_validated.throttle_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_throttle_filtered();
+	airspeed_validated.pitch_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_pitch_filtered();
+
 	switch (_valid_airspeed_index) {
 	case airspeed_index::DISABLED_INDEX:
 		break;
@@ -149,11 +149,12 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
 * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0.
 *
 * @min 0
- * @max 15
+ * @max 31
 * @bit 0 Only data missing check (triggers if more than 1s no data)
 * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode)
 * @bit 2 Innovation check (see ASPD_FS_INNOV)
 * @bit 3 Load factor check (triggers if measurement is below stall speed)
+ * @bit 4 First principle check (airspeed change vs. throttle and pitch)
 * @group Airspeed Validator
 */
 PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7);
@@ -239,3 +240,19 @@ PARAM_DEFINE_FLOAT(ASPD_FS_T_START, -1.f);
 * @group Airspeed Validator
 */
 PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f);
+
+/**
+ * First principle airspeed check time window
+ *
+ * Window for comparing airspeed change to throttle and pitch change.
+ * Triggers when the airspeed change within this window is negative while throttle increases
+ * and the vehicle pitches down.
+ * Is meant to catch degrading airspeed blockages as can happen when flying through icing conditions.
+ * Relies on  FW_THR_TRIM being set accurately.
+ *
+ * @unit s
+ * @min 0
+ * @decimal 1
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_FP_T_WINDOW, 2.0f);
@@ -43,7 +43,10 @@ CpuResourceChecks::CpuResourceChecks()

 void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_cpu_max.get() < FLT_EPSILON) {
+	const bool cpu_load_check_enabled = _param_com_cpu_max.get() > FLT_EPSILON;
+	const bool ram_usage_check_enabled = _param_com_ram_max.get() > FLT_EPSILON;
+
+	if (!cpu_load_check_enabled && !ram_usage_check_enabled) {
 		return;
 	}

@@ -54,15 +57,15 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		/* EVENT
 		 * @description
 		 * <profile name="dev">
-		 * If the system does not provide any CPU load information, use the parameter <param>COM_CPU_MAX</param>
-		 * to disable the check.
+		 * If the system does not provide any CPU and RAM load information, use the parameters <param>COM_CPU_MAX</param>
+		 * and <param>COM_RAM_MAX</param> to disable the checks.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"),
-				       events::Log::Error, "No CPU load information");
+				       events::Log::Error, "No CPU and RAM load information");

 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU and RAM load information");
 		}

 	} else {
@@ -71,7 +74,7 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 		_high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time());

 		// fail check if CPU load is above the threshold for 2 seconds
-		if (_high_cpu_load_hysteresis.get_state()) {
+		if (cpu_load_check_enabled && _high_cpu_load_hysteresis.get_state()) {
 			/* EVENT
 			 * @description
 			 * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update
@@ -88,5 +91,26 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter)
 				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent);
 			}
 		}
+
+		const float ram_usage_percent = cpuload.ram_usage * 100.f;
+		const bool high_ram_usage = ram_usage_percent > _param_com_ram_max.get();
+
+		if (ram_usage_check_enabled && high_ram_usage) {
+			/* EVENT
+			 * @description
+			 * The RAM usage can be reduced for example by disabling unused modules (e.g. mavlink instances).
+			 *
+			 * <profile name="dev">
+			 * The threshold can be adjusted via <param>COM_RAM_MAX</param> parameter.
+			 * </profile>
+			 */
+			reporter.healthFailure<float>(NavModes::All, health_component_t::system, events::ID("check_ram_usage_too_high"),
+						      events::Log::Error, "RAM usage too high: {1:.1}%", ram_usage_percent);
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RAM usage too high: %3.1f%%",
+						     (double)ram_usage_percent);
+			}
+		}
 	}
 }
@@ -54,6 +54,7 @@ private:
 	systemlib::Hysteresis _high_cpu_load_hysteresis{false};

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max
+					(ParamFloat<px4::params::COM_CPU_MAX>) _param_com_cpu_max,
+					(ParamFloat<px4::params::COM_RAM_MAX>) _param_com_ram_max
 				       )
 };
@@ -318,6 +318,22 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor

 		_gps_was_fused = ekf_gps_fusion;

+		if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) {
+			if (!_gnss_spoofed) {
+				_gnss_spoofed = true;
+
+				if (reporter.mavlink_log_pub()) {
+					mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t");
+				}
+
+				events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info},
+					     "GNSS signal spoofed");
+			}
+
+		} else {
+			_gnss_spoofed = false;
+		}
+
 		if (!context.isArmed() && ekf_gps_check_fail) {
 			NavModes required_groups_gps = required_groups;
 			events::Log log_level = events::Log::Error;
@@ -450,6 +466,18 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 							    events::ID("check_estimator_gps_vert_speed_drift_too_high"),
 							    log_level, "GPS Vertical Speed Drift too high");

+			} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) {
+				message = "Preflight%s: GPS signal spoofed";
+				/* EVENT
+				 * @description
+				 * <profile name="dev">
+				 * This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
+				 * </profile>
+				 */
+				reporter.armingCheckFailure(required_groups_gps, health_component_t::gps,
+							    events::ID("check_estimator_gps_spoofed"),
+							    log_level, "GPS signal spoofed");
+
 			} else {
 				if (!ekf_gps_fusion) {
 					// Likely cause unknown
@@ -64,7 +64,6 @@ private:
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
 	void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				       const vehicle_local_position_s &lpos);
-
 	void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
 	void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
 	void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
@@ -102,6 +101,7 @@ private:
 	bool _position_reliant_on_optical_flow{false};

 	bool _gps_was_fused{false};
+	bool _gnss_spoofed{false};

 	bool _nav_failure_imminent_warned{false};

@@ -802,6 +802,21 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f);
 */
 PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f);

+/**
+ * Maximum allowed RAM usage to pass checks
+ *
+ * The check fails if the RAM usage is above this threshold.
+ *
+ * A negative value disables the check.
+ *
+ * @group Commander
+ * @unit %
+ * @min -1
+ * @max 100
+ * @increment 1
+ */
+PARAM_DEFINE_FLOAT(COM_RAM_MAX, 95.0f);
+
 /**
 * Required number of redundant power modules
 *
@@ -203,6 +203,7 @@ 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/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -212,7 +213,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

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

 add_subdirectory(EKF)
@@ -125,6 +125,7 @@ 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/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()
@@ -134,7 +135,7 @@ if(CONFIG_EKF2_SIDESLIP)
 endif()

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

 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate()

 void ZeroVelocityUpdate::reset()
 {
-	_time_last_zero_velocity_fuse = 0;
+	_time_last_fuse = 0;
 }

 bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed)
 {
 	// Fuse zero velocity at a limited rate (every 200 milliseconds)
-	const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us);
+	const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us);

 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest
@@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye
 				ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i);
 			}

-			_time_last_zero_velocity_fuse = imu_delayed.time_us;
+			_time_last_fuse = imu_delayed.time_us;

 			return true;
 		}
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	d5876ab0d2	ekf2: stop gps yaw fusion on numerical error	2024-07-11 12:25:14 -04:00
Daniel Agar	467f96f2a4	ekf2: stop airspeed fusion on numerical error	2024-07-11 12:23:15 -04:00
Daniel Agar	4263fa9ee4	ekf2: stop flow fusion on numerical error	2024-07-11 12:22:26 -04:00
Daniel Agar	b425253a54	ekf2: stop mag fusion on numerical error	2024-07-11 12:19:50 -04:00
Claudio Chies	33be5d8356	Survey - fix of survey tracking problem on steep slopes (#23371 ) * initial working * incoperated review	2024-07-11 14:54:22 +02:00
Daniel Agar	9124a7b471	ekf2: add IMU delta_ang_dt/delta_vel_dt safety constrain before pushing into buffer	2024-07-10 21:20:47 -04:00
Daniel Agar	ac77049c47	ekf2: directly use IMU sample to find corresponding aid source sample - I think this helps make it clear we're using a sensor sample corresponding with a particular IMU sample	2024-07-10 21:20:47 -04:00
Daniel Agar	f93dc61770	ekf2: use bias corrected angular velocity - avoid unnecessarily storing _ang_rate_delayed_raw	2024-07-10 21:20:47 -04:00
Julian Oes	20137bea40	boards: add console build for Cube Orange(+) This adds a build variant which enables the serial console, and therefore disables the ADSB receiver.	2024-07-10 21:14:08 -04:00
Claudio Chies	57e303b11b	bugfix for failing actions	2024-07-10 21:12:55 -04:00
PX4 BuildBot	e0ea91fc27	Update submodule gz to latest Thu Jul 11 00:39:09 UTC 2024 - gz in PX4/Firmware (`2c3401dc83`): https://github.com/PX4/PX4-gazebo-models/commit/881558c8c274d0d9f21970de24333122e050b561 - gz current upstream: https://github.com/PX4/PX4-gazebo-models/commit/312cd002ff9602644efef58eef93e447c10dcbe8 - Changes: https://github.com/PX4/PX4-gazebo-models/compare/881558c8c274d0d9f21970de24333122e050b561...312cd002ff9602644efef58eef93e447c10dcbe8 312cd00 2024-07-08 chfriedrich98 - Add rover ackermann model (#46)	2024-07-10 21:05:57 -04:00
chfriedrich98	c391509c23	ackermann: add SITL airframe	2024-07-10 21:04:59 -04:00
Matthias Grob	2c3401dc83	uORB: SubscriptionInterval fix timestamp wrapping when initializing less than the interval time after boot (#23384 ) * SubscriptionInterval: ensure _last_update is never before timer start	2024-07-10 12:43:31 -04:00
Daniel Agar	75bb339d94	ekf2: remove warning events logging - some of these warning flags aren't even being used, and most of the rest we can figure out from other sources	2024-07-10 10:43:55 -04:00
Daniel Agar	c29b4ff87e	ekf2: apply astyle formatting and enforce	2024-07-10 10:43:55 -04:00
chfriedrich98	3fe609f769	exclude 4017 from v5x to save flash	2024-07-10 12:06:48 +02:00
chfriedrich98	03ff837c50	ackermann: new features and improvements added return mode support, slew rates for actuators, new ackermann specific message, improved cornering slow down effect and fixed logging issue.	2024-07-10 12:06:48 +02:00
Daniel Agar	223397c20e	ekf2: always add accel/gyro bias process noise - continue adding accel/gyro bias process noise even if inhibited	2024-07-10 11:49:01 +02:00
Marco Hauswirth	419652b9fe	EKF2: Spoofing GPS check (#23366 ) * estimator gps check fail flag for spoofing * warn whenever spoofing state changes to true, use default hysteresis to completely stop fusion * dont introduce more GPS namings, GNSS instead * flash: exclude mantis for cuav_x7pro	2024-07-09 16:31:11 +02:00
Daniel Agar	62ff39a669	ekf2: EV vel (body) update last fuse timestamps - these are set by the NED fuseVelocity() helper so also need to be set in the body frame velocity case	2024-07-09 10:16:12 -04:00
Daniel Agar	5d08b97fd7	ekf2: add vehicle_local_position dist_bottom_var	2024-07-09 10:10:01 -04:00
Daniel Agar	3e3b886b5d	ekf2: add terrain estimator_status_flags	2024-07-09 10:10:01 -04:00
Daniel Agar	64a6971bdb	ekf2: only limit opt flow HAGL if range only terrain - increase HALG limit from 75%->90% of sensor max	2024-07-09 10:10:01 -04:00
Daniel Agar	c56f84fe8a	ekf2: range, check if terrain valid for reset on fusion timeout	2024-07-09 10:10:01 -04:00
Daniel Agar	e52025cc20	ekf2: optical flow fusion timeout only reset if quality is good	2024-07-09 10:10:01 -04:00
Daniel Agar	6be06ecbb3	ekf2: optical flow failing also reset terrain if needed	2024-07-09 10:10:01 -04:00
Daniel Agar	ea8f14b883	ekf2: optical flow logic, timeout if bad_tilt, etc - previously we could get stuck with optical flow still technically active (_control_status.flags.opt_flow=true), but nothing being updated due to excessive tilt, etc	2024-07-09 10:10:01 -04:00
Daniel Agar	8bf15b01c4	ekf2: optical flow don't compute innovation variance twice - collapse updateOptFlow() and startFlowFusion() to avoid recomputing H - this is a relatively expensive call we can easily avoid with the right structure	2024-07-09 10:10:01 -04:00
Daniel Agar	f709ed409d	ekf2: optical flow stop reset all flags	2024-07-09 10:10:01 -04:00
Daniel Agar	9dfd82ab06	ekf2: optical flow remove _flow_data_ready flag	2024-07-09 10:10:01 -04:00
Daniel Agar	7047f9441c	ekf2: fix calcOptFlowBodyRateComp() gyro bias - adjust flow sample gyro_rate immediately after popping from ring buffer - always update flow gyro bias (calcOptFlowBodyRateComp()) regardless of flow quality or magnitude	2024-07-09 10:10:01 -04:00
Daniel Agar	4d324da9f8	ekf2: update flow aid src status every sample	2024-07-09 10:10:01 -04:00
Daniel Agar	bcd666b3f8	ekf2: fix optical flow start logic - remove fallthrough that enables flow regardless of success - add appropriate start messages for each case	2024-07-09 10:10:01 -04:00
Daniel Agar	bf4e564b23	ekf2: resetTerrainToFlow() reset aid src status appropriately	2024-07-09 10:10:01 -04:00
Daniel Agar	ced609daa8	ekf2: flow fusion start require valid fusion	2024-07-09 10:10:01 -04:00
Daniel Agar	1df8f3f9d2	ekf2: resetFlowFusion() reset aid src status appropriately	2024-07-09 10:10:01 -04:00
Roman Bapst	8221940b60	Added pitot tube icing detection (#23206 ) * lib: add FilteredDerivative class * AirspeedValidator: add first principle check - filters throttle, pitch and airspeed rate, and triggers if the airspeed rate is negative even though the vehicle is pitching down and giving high throttle. Check has to fail for duration defined by ASPD_FP_T_WINDOW to trigger an airspeed failure. * AirspeedValidator: define constants for first principle check * FilteredDerivative: set initialised to false if sample interval is invalid * airspeed_selector: improved comment * increase IAS derivative filter time constant from 4 to 5 * use legacy parameter handling for FW_PSP_OFF * handle FW_THR_MAX as well * ROMFS/airframes: exclude some airframes for v6x and v4pro to save flash on them --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-09 11:16:40 +02:00
Julian Oes	a35cecece4	gnss: add missing include Breaks CLion otherwise.	2024-07-08 20:38:40 -04:00
Peter van der Perk	6bd81f38a6	imxrt dshot timing fix (#23365 ) * imxrt: Change PLL settings for more accurate dshot timing * Update NuttX submodule	2024-07-08 12:57:15 -04:00
Silvan Fuhrer	77709c2948	FW Position control: clean up param descriptions Mostly to save flash, but also to improve generally. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-08 16:44:09 +02:00
Claudio Chies	aed0fd41cf	SIH - change how LAT and LON is used for Takeoff location (#23363 ) change how lat long is used for SIH	2024-07-08 14:51:08 +02:00
Marco Hauswirth	4bc0286eb8	fix error from refactring commit, fix reset on ground (#23370 )	2024-07-08 13:55:05 +02:00
Marco Hauswirth	e04c53241a	EKF2: reset position by fusion (#23279 ) * reset position by fusion * handle local_pos_valid for fixed wing in gnss denied * [WIP] ekf2: setEkfGlobalOrigin respect current height reference and vertical position aiding * global origin, also reset vertical pos without gps as ref * fix wo gnss, that bitcraze ci passes * revert some changes as requested * remove duplicate reset messages * undo unrelated whitespace changes, I'll fix it everywhere in a followup * [SQUASH] ekf2: add vehicle_command_ack * resetGlobalPosToExternalObservation consolidate logic * remove gnss check from local_pos validation check * reset when 0<accuracy<1, otherwise fuse * replace gps param with flag * ekf2: dead reckon time exceeded, respect ZUPT preflight if air data or optical flow expected * subtract timeout from last inertial dead-reck, change fake pos conditions, save flash --------- Co-authored-by: Daniel Agar <daniel@agar.ca>	2024-07-07 22:43:55 +02:00
Peter van der Perk	ac1effa32a	fmu-v6xrt: MTD use full FRAM (32KB)	2024-07-05 10:25:08 -04:00
Ryan Johnston	fd8df2e84d	Update int_res_est_replay.py (#23351 ) Pulls cell count, min voltage and max voltage from log file but still allows for over-rides. Also added debug info to tell user what what it found in the log and what it is using Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>	2024-07-05 11:04:45 +02:00
Marco Hauswirth	a1f43636f3	ekf2: EV fusion in body frame (#23191 )	2024-07-04 21:17:19 -04:00
Silvan Fuhrer	1f33abb4e9	battery_status.msg: remove unused fields (#22938 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-04 11:57:26 +02:00
PonomarevDA	4c5ce7af6b	Cyphal: add feedback for 8 ESC	2024-07-03 13:02:18 -04:00
PonomarevDA	8569eeb90c	Cyphal: add *type registers for ESC	2024-07-03 13:02:18 -04:00
PonomarevDA	f81e36a3a0	Cyphal: optimize ESC setpoint	2024-07-03 13:02:18 -04:00
PonomarevDA	41bd6c92e2	Cyphal: add zubax.telega.CompactFeedback	2024-07-03 13:02:18 -04:00
PonomarevDA	515543b1c5	Cyphal: divide EscClient into 2 publishers, so setpoint and readiness are 2 different ports now	2024-07-03 13:02:18 -04:00
Dmitry Ponomarev	52476633a8	Cyphal: use actual time instead of transfer id in uptime field of heartbeat	2024-07-03 13:02:18 -04:00
Dmitry Ponomarev	b063202b45	Cyphal: remove setpoint scaling to 8192	2024-07-03 13:02:18 -04:00
Dmitry Ponomarev	d3480d1302	Cyphal: add port.List	2024-07-03 13:02:18 -04:00
Matthias Grob	c8c46788ed	Autostart: load airframes with priority ROMFS -> SD card	2024-07-03 18:32:16 +02:00
Thomas Frans	c0663ee85c	gnss(septentrio): fix line lenghth of module documentation	2024-07-03 11:21:34 -04:00
Thomas Frans	e27b252433	gnss(septentrio): fix incorrect heading offset configuration Heading offset was configured as radians but should be configured as degrees on Septentrio receivers. The parameter was already in degrees but the configuration logic was changing it into radians. Also allow the entire allowed range of heading offset values for Septentrio receivers.	2024-07-03 11:21:34 -04:00
Thomas Frans	49dc896d20	gnss(septentrio): fix broken heading Heading wasn't working because of an incorrect check during parsing.	2024-07-03 11:21:34 -04:00
Thomas Frans	bfbbf2ff6f	gnss(septentrio): improve `SEP_DUMP_COMM` parameter documentation The documentation for `SEP_DUMP_COMM` was quite unclear and users had to use the user guide to find out what exactly it did. The new documentation tries to make the purpose clearer.	2024-07-03 11:21:34 -04:00
Thomas Frans	7bb239637e	gnss(septentrio): fix error on driver start with same device paths This fixes an incorrect check of the device paths during instantiation of the Septentrio driver that caused the driver to start and not print an error message.	2024-07-03 11:21:34 -04:00
Thomas Frans	522a25a410	gnss(septentrio): first batch of bugfixes after internal testing Internal testing revealed usability issues. Those and some other problems are fixed.	2024-07-03 11:21:34 -04:00
Silvan Fuhrer	33701aa3d5	BatteryStatus: remove voltage_filtered_a Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-03 16:41:49 +02:00
Silvan Fuhrer	c2ae6a7e24	BatteryStatus: remove current_filtered_a Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-03 16:41:49 +02:00
zhangteng0526	e03e0261a1	Fix buffer overflow in mavlink_receive.cpp	2024-07-03 08:11:32 +02:00
chfriedrich98	f65653a391	battery: add internal resistance estimation	2024-07-02 19:05:13 +02:00
chfriedrich98	71029689e7	battery: add replay file for internal resistance estimation	2024-07-02 19:05:13 +02:00
Silvan Fuhrer	6d549811bc	fmu v3: disable GYRO_FFT to save flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-07-02 11:44:54 -04:00
Marco Hauswirth	3880073716	ekf2: fix timeout after gps failure (#23346 )	2024-07-02 10:38:49 -04:00
Daniel Agar	0742d356f5	ekf2: more conservative clipping checks for bad_acc_clipping fault status (#23337 ) - track accel clipping count per axis - only set bad_acc_clipping fault_status if at least one axis is clipping continuously or if all have been clipping at warning level - Note: this doesn't impact the clipping projections that boost the accel process noise, pause bias estimation, and skip gravity fusion on a per sample basis	2024-06-28 16:45:08 -04:00
bluedisk	408d8abe95	Tools/setup: fix the wrong - deprecated - expression in the requirements.txt - Fixes matplotlib version expression from ">=3.0.*" ro ">=3.0" which is the right syntax Fixes issue #23329 Co-authored-by: lee wonwoo <leewonwoo@leeui-MacBookPro.local>	2024-06-28 10:20:26 -04:00
Alex Klimaj	053b4a4423	drivers/uavcan: GNSS set system time based on fix_type instead of valid_pos_cov	2024-06-27 21:35:45 -04:00
Peter van der Perk	58f7c3e9c9	NuttX with imxrt1170 soc vdd backport (#23333 )	2024-06-27 16:21:45 -04:00
PX4 BuildBot	8b26e5e252	Update submodule libevents to latest Thu Jun 27 12:39:19 UTC 2024 - libevents in PX4/Firmware (4e3561cad8d24fefe66d266e969652d7ab20162b): https://github.com/mavlink/libevents/commit/8d5c44661bf79106361eb0b5170025b86e85a525 - libevents current upstream: https://github.com/mavlink/libevents/commit/9474657606d13301d426e044450c4f84de2221be - Changes: https://github.com/mavlink/libevents/compare/8d5c44661bf79106361eb0b5170025b86e85a525...9474657606d13301d426e044450c4f84de2221be 9474657 2024-06-13 Beat Küng - cmake: add namespaced target & installation include dir 9f2e68d 2024-06-12 Beat Küng - CMakeLists: set CMAKE_CXX_STANDARD if not set 3204e8f 2024-06-12 Beat Küng - parser.h: use std::vector<EventArgumentDefinition>::size_type eab8144 2024-04-29 Beat Küng - fix parser: avoid signed to unsigned conversion 159f83e 2024-04-29 Beat Küng - cpp: only enable Wall and others for GCC	2024-06-27 16:21:20 -04:00
Matthias Grob	e4446adba1	Add check for high RAM usage We had a case where someone took off with an experimental system with 100% RAM usage on the embedded system without noticing. This lead to problems during flight. Since we already have a CPU load check it seems natural to also check the reported RAM usage.	2024-06-27 11:20:22 +02:00
Daniel Agar	30b854da35	ekf2: verbose logging control (new EKF2_LOG_VERBOSE) - new parameter EKF2_LOG_VERBOSE to enable (currently enabled by default) - force advertise topics immediately (based on EKF2_LOG_VERBOSE and per aid source configuration) - logger optionally log all estimator topics at minimal rate	2024-06-27 01:10:57 -04:00
Patrik Dominik Pordi	8070c70f2c	uxrce_dds_client: dds_topics.yaml add vehicle_land_detected - px4_msgs::msg::VehicleLandDetected has been added to dds_topics.yaml	2024-06-27 01:10:04 -04:00
Daniel Agar	78fd9a15f8	flight_mode_manager: delete unused avoidance waypoint	2024-06-27 01:08:16 -04:00
Daniel Agar	338bcc6ca3	ekf2: disable EKF2_EV_CTRL and EKF2_AGP_CTRL by default	2024-06-26 17:10:28 -04:00
alexklimaj	9cc4e2ac01	boards ark pi6x add vl53l0x	2024-06-26 17:09:37 -04:00
Silvan Fuhrer	1ae96d6509	EKF2: fix builds without CONFIG_EKF2_RANGE_FINDER Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-26 11:05:38 +02:00
bresch	a50ef2eb5e	ekf2-terrain: make terrain validity based on uncertainty When using optical flow for terrain aiding, we cannot assume that the terrain estimate is valid if flow is fused as is can only be observed during motion. When no direct observation is available, the terrain is assumed to be valid if its 1sigma uncertainty is < 10% of the hagl.	2024-06-26 11:05:38 +02:00
bresch	a665764b0e	ekf2: remove unused EKF2_TERR_MASK	2024-06-26 11:05:38 +02:00
bresch	7903ddf5df	ekf2-terrain: terrain is not a separate estimator	2024-06-26 11:05:38 +02:00
bresch	9001c23926	ekf2: clean up hagl vs terrain naming Terrain is the state: terrain vertical position Hagl (height above ground level) is the vertical distance between the vertical position and the terrain vertical position	2024-06-26 11:05:38 +02:00
bresch	68980b59e2	ekf2: add terrain state	2024-06-26 11:05:38 +02:00
KonradRudin	09f066a73a	mission: skip a vtol takoff mission item if already in air (#23319 ) * mission: skip a vtol takoff mission item if already in air and a fixed wing * MissionBase: also skip FW takeoff when already in-air Signed-off-by: Silvan Fuhrer <silvan@auterion.com> * mission: use setNextMissionItem to skip vtol takeoff when already in air * mission: Only skip the VTOL takeoff in air for mission and rtl mission If flying RTL mission reverse it must still include the takeoff point. --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2024-06-25 16:33:45 +02:00
Nate	6fd0e98a69	Correct units of CRSF GPS altitude Bug fix to correct returning mm units of altitude to m.	2024-06-24 12:27:21 +02:00
David Sidrane	e8e8a60ca8	NuttX with backport of stm32h7 No phy	2024-06-24 06:12:12 -04:00
Matthias Grob	8cc7c99b59	mavlink: report generator error (#23313 ) Without this flag the command silently succeeds even though the logs contains an error. It's much more developer friendly to fail early in case of an error. The log path is then also shown in the console output.	2024-06-24 10:00:03 +02:00
Daniel Agar	30ce560e3a	ekf2: mag control reset filtered test ratio on start (if aligning yaw)	2024-06-20 13:41:54 -04:00
Daniel Agar	dcb1103299	ekf2: move estimator_status test ratios to filtered values	2024-06-20 13:41:54 -04:00
				`@@ -0,0 +1 @@`
				`# Same as default, only defconfig is different`