vehicle_command_ack: rename result_param1 to progress and comment usage

To avoid confusion: - naming consistent with MAVLink definition - comments about the few different ways the field is used
2026-06-12 21:30:06 +08:00 · 2024-06-18 16:44:11 +02:00
226 changed files with 4090 additions and 6374 deletions
@@ -11,8 +11,6 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    env:
-      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -11,8 +11,6 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    env:
-      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -1,47 +0,0 @@
-#!/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,7 +83,6 @@ 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,9 +15,6 @@ 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,7 +157,6 @@ 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,7 +6,6 @@
 # @class Copter
 #
 # @board px4_fmu-v2 exclude
-# @board px4_fmu-v5x exclude
 # @board bitcraze_crazyflie exclude
 #
 # @maintainer Iain Galloway <iain.galloway@nxp.com>
@@ -7,10 +7,6 @@
 #
 # @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
@@ -40,6 +36,7 @@ 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,7 +12,6 @@
 # @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,7 +10,6 @@
 # @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,7 +8,6 @@
 # @maintainer Oleg Kalachev <okalachev@gmail.com>
 #
 # @board px4_fmu-v2 exclude
-# @board px4_fmu-v4pro exclude
 # @board bitcraze_crazyflie exclude
 #

@@ -25,6 +25,7 @@
 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,7 +13,6 @@
 # @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,6 +1,9 @@
 #!/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,31 +217,25 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS

-	# Load airframe configuration based on SYS_AUTOSTART parameter
+	#
+	# Set parameters and env variables for selected SYS_AUTOSTART.
+	#
+	set AUTOSTART_PATH etc/init.d/rc.autostart
 	if ! param compare SYS_AUTOSTART 0
 	then
-		# 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 ]
+		if param greater SYS_AUTOSTART 1000000
 		then
-			# Look for airframe on SD card
+			# Use external startup file
 			if [ $SDCARD_AVAILABLE = yes ]
 			then
-				. ${R}etc/init.d/rc.autostart_ext
+				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
 			else
-				echo "ERROR [init] SD card not mounted - can't load external airframe"
+				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
 			fi
 		fi
-
-		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
+		. ${R}$AUTOSTART_PATH
 	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,8 +18,7 @@ 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/python/ekf_derivation/generated -prune -o \
-    -path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \
+    -path src/modules/ekf2/EKF -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 flags summary
+        # plot innovation_check_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,6 +79,12 @@ 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,7 +10,6 @@ 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
@@ -39,6 +39,7 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
+CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -57,6 +58,7 @@ CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
+CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y
 CONFIG_MODULES_MANUAL_CONTROL=y
@@ -68,6 +70,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
+CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -95,3 +98,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -1 +0,0 @@
-# Same as default, only defconfig is different
@@ -1,257 +0,0 @@
-#
-# 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
@@ -1 +0,0 @@
-# Same as default, only defconfig is different
@@ -1,259 +0,0 @@
-#
-# 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,6 +1148,7 @@ 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=n
+CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -6,7 +6,6 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=n
 CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
-CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_PAYLOAD_DELIVERER=n
@@ -52,7 +52,6 @@
 #define IMXRT_IPG_PODF_DIVIDER 5
 #define BOARD_GPT_FREQUENCY  24000000
 #define BOARD_XTAL_FREQUENCY 24000000
-#define BOARD_FLEXIO_PREQ    108000000

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

@@ -154,6 +154,7 @@
 *(.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 = /* 432 / 4 = 108Mhz */
+		.flexio1_clk_root = /* 240 / 2 = 120Mhz */
 		{
 			.enable        = 1,
-			.div           = 4,
-			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3,
+			.div           = 2,
+			.mux           = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2,
 		},
 		.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 * 18) / 16 = 594 MHz */
-		.pfd2            = 24, /* (528 * 18) / 24 = 396 MHz */
-		.pfd3            = 22, /* (528 * 18) / 22 = 216 MHz */
+		.pfd1            = 16, /* (528 * 16) / 16 = 594 MHz */
+		.pfd2            = 24, /* (528 * 24) / 27 = 396 MHz */
+		.pfd3            = 32, /* (528 * 32) / 27 = 297 MHz */
 	},
 	.sys_pll3 =
 	{
@@ -53,12 +53,18 @@ 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 = 1,
+	.npart = 2,
 	.partd = {
 		{
 			.type = MTD_PARAMETERS,
 			.path = "/fs/mtd_params",
-			.nblocks = 256
+			.nblocks = 32
+		},
+		{
+			.type = MTD_WAYPOINTS,
+			.path = "/fs/mtd_waypoints",
+			.nblocks = 32
+
 		}
 	},
 };
@@ -10,7 +10,3 @@ 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,7 +1,9 @@
 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
@@ -66,15 +68,11 @@ 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,7 +180,6 @@ set(msg_files
 	RegisterExtComponentReply.msg
 	RegisterExtComponentRequest.msg
 	RoverAckermannGuidanceStatus.msg
-	RoverAckermannStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
@@ -25,3 +25,4 @@ 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
+# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_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
+# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias
@@ -20,3 +20,18 @@ 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,7 +13,6 @@ 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,6 +22,7 @@ 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[25] states		# Internal filter states
+float32[24] states		# Internal filter states
 uint8 n_states		# Number of states effectively used

-float32[24] covariances	# Diagonal Elements of Covariance Matrix
+float32[23] covariances	# Diagonal Elements of Covariance Matrix
@@ -15,7 +15,6 @@ 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
@@ -70,14 +69,27 @@ 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 # 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
+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

 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,8 +43,6 @@ 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,9 +1,10 @@
 uint64 timestamp # time since system start (microseconds)

-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 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

 # TOPICS rover_ackermann_guidance_status
@@ -1,7 +0,0 @@
-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
@@ -25,7 +25,7 @@ uint8 ORB_QUEUE_LENGTH = 4

 uint32 command						# Command that is being acknowledged
 uint8 result						# Command result
-uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
+uint8 progress					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
 int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
 uint16 target_component 				# Target component / mode executor
@@ -56,7 +56,6 @@ 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
@@ -74,9 +74,9 @@ ssize_t Serial::read(uint8_t *buffer, size_t buffer_size)
 	return _impl.read(buffer, buffer_size);
 }

-ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms)
+ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
 {
-	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_ms);
+	return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_us);
 }

 ssize_t Serial::write(const void *buffer, size_t buffer_size)
@@ -62,7 +62,7 @@ public:
 	bool close();

 	ssize_t read(uint8_t *buffer, size_t buffer_size);
-	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_ms = 0);
+	ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0);

 	ssize_t write(const void *buffer, size_t buffer_size);

@@ -125,16 +125,8 @@ public:
 	{
 		if (_subscription.copy(dst)) {
 			const hrt_abstime now = hrt_absolute_time();
-
-			// 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;
-			}
-
+			// 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);
 			return true;
 		}

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

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

 };
@@ -251,7 +251,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 	return ret;
 }

-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms)
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
 {
 	if (!_open) {
 		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
@@ -264,7 +264,6 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char
 	}

 	const hrt_abstime start_time_us = hrt_absolute_time();
-	hrt_abstime timeout_us = timeout_ms * 1000;
 	int total_bytes_read = 0;

 	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
@@ -46,6 +46,7 @@
 #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
@@ -304,8 +305,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 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF);
-	bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF);
+	dshot_tcmp = 0x2F00 | (((FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2)) & 0xFF);
+	bdshot_tcmp = 0x2900 | (((FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 1) & 0xFF);

 	/* Clock FlexIO peripheral */
 	imxrt_clockall_flexio1();
@@ -244,7 +244,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 	return ret;
 }

-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms)
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
 {
 	if (!_open) {
 		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
@@ -257,7 +257,6 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char
 	}

 	const hrt_abstime start_time_us = hrt_absolute_time();
-	hrt_abstime timeout_us = timeout_ms * 1000;
 	int total_bytes_read = 0;

 	while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) {
@@ -173,7 +173,7 @@ ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size)
 	return ret_read;
 }

-ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms)
+ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_us)
 {
 	if (!_open) {
 		PX4_ERR("Cannot readAtLeast from serial device until it has been opened");
@@ -186,7 +186,6 @@ ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t char
 	}

 	const hrt_abstime start_time_us = hrt_absolute_time();
-	hrt_abstime timeout_us = timeout_ms * 1000;
 	int total_bytes_read = 0;

 	while (total_bytes_read < (int) character_count) {
@@ -116,11 +116,13 @@ 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,11 +37,12 @@
 * Client-side implementation of UDRAL specification ESC service
 *
 * Publishes the following Cyphal messages:
- *   reg.udral.service.actuator.common.sp.Value31.0.1
- *   reg.udral.service.common.Readiness.0.1
+ *   reg.drone.service.actuator.common.sp.Value8.0.1
+ *   reg.drone.service.common.Readiness.0.1
 *
 * Subscribes to the following Cyphal messages:
- *   zubax.telega.CompactFeedback.0.1
+ *   reg.drone.service.actuator.common.Feedback.0.1
+ *   reg.drone.service.actuator.common.Status.0.1
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
 * @author Jacob Crabill <jacob@flyvoly.com>
@@ -50,13 +51,11 @@
 #pragma once

 #include <lib/perf/perf_counter.h>
-#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.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
@@ -64,15 +63,16 @@ using std::isfinite;
 #include <reg/udral/service/actuator/common/sp/Vector31_0_1.h>
 #include <reg/udral/service/common/Readiness_0_1.h>

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

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

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

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

-	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
-	hrt_abstime _previous_pub_time = 0;
+	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_;

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

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

-	CanardTransferID _arming_transfer_id;
+			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);

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

 		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;

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

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

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

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

 		if (result == 0) {
-			++_arming_transfer_id;
+			// 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.
 			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
 						       &transfer_metadata,
 						       payload_size,
 						       &arming_payload_buffer);
 		}
 	};
-};
-
-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)};
+
+	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;
 };
@@ -62,8 +62,6 @@ 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),
@@ -127,7 +125,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, 512, CANARD_MTU_CAN_CLASSIC);
+		_instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC);
 	}

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

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

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

 		uavcan_node_Heartbeat_1_0 heartbeat{};
-		const hrt_abstime now = hrt_absolute_time();
-		heartbeat.uptime = now / 1000000;
+		heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime
 		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 = {
@@ -396,45 +392,6 @@ 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,9 +137,6 @@ 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,15 +56,6 @@ 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;
 }

@@ -82,36 +73,19 @@ 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)
 {
-	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 {
+	if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) {
 		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,10 +103,6 @@ typedef struct {
 	bool is_persistent {true};
 } UavcanParamBinder;

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

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


-	const UavcanParamBinder _uavcan_params[22] {
+	const UavcanParamBinder _uavcan_params[13] {
 		{"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},
@@ -135,28 +130,7 @@ 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,15 +125,6 @@ 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 + \
-	2 * CONFIG_CYPHAL_ESC_CONTROLLER + \
+	CONFIG_CYPHAL_ESC_CONTROLLER + \
 	CONFIG_CYPHAL_READINESS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \
 	CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER
@@ -79,7 +79,6 @@

 #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"
@@ -104,7 +103,6 @@ public:

 	UavcanPublisher *getPublisher(const char *subject_name);

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

@@ -133,14 +131,6 @@ private:
 			"udral.esc",
 			0
 		},
-		{
-			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher *
-			{
-				return new ReadinessPublisher(handle, pmgr);
-			},
-			"udral.readiness",
-			0
-		},
 #endif
 #if CONFIG_CYPHAL_READINESS_PUBLISHER
 		{
@@ -76,6 +76,8 @@ 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,24 +158,3 @@ 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,10 +45,6 @@
 #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
@@ -69,15 +65,12 @@

 #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"

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

 private:
 	void updateDynamicSubscriptions();
@@ -138,72 +130,6 @@ 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,87 +162,6 @@ 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_ERR(__VA_ARGS__);}
+#define SEP_ERR(...)            {PX4_WARN(__VA_ARGS__);}
 #else
 #define SEP_ERR(...)            {}
 #endif
@@ -71,7 +71,7 @@ parameters:
        type: float
        decimal: 3
        default: 0
-        min: -360
+        min: 0
        max: 360
        unit: deg
        reboot_required: true
@@ -104,8 +104,7 @@ parameters:
        description:
          short: Log GPS communication data
          long: |
-            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.
+            Dump raw communication data from and to the connected receiver to the log file.
        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.0f * 10000000000.0f};
-constexpr double k_dnu_f8_value {-2.0 * 10000000000.0};
+constexpr float k_dnu_f4_value {-2 * 10000000000};
+constexpr double k_dnu_f8_value {-2 * 10000000000};

 /// 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,7 +55,6 @@
 #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>

@@ -87,11 +86,6 @@ 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;
@@ -140,19 +134,18 @@ 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),
-	_chosen_baud_rate(baud_rate)
+	_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);

@@ -178,10 +171,6 @@ 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);
@@ -209,8 +198,6 @@ SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, u
 	}

 	set_device_type(DRV_GPS_DEVTYPE_SBF);
-
-	reset_gps_state_message();
 }

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

-	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate());
+	PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _baud_rate);
 	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 (first_gps_uorb_message_created() && _state == State::ReceivingData) {
+	if (_message_gps_state.timestamp != 0) {
+
 		PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast<double>(rtcm_injection_frequency()));
+
 		print_message(ORB_ID(sensor_gps), _message_gps_state);
 	}

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

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

 				} else {
 					// Failed to open port, so wait a bit before trying again.
@@ -288,42 +277,14 @@ 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: {
-				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");
+				if (configure() == PX4_OK) {
+					SEP_INFO("Automatic configuration successful");
 					_state = State::ReceivingData;

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

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

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

 				if (_message_satellite_info && (receive_result & 2)) {
@@ -424,51 +385,19 @@ 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,
-						   valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate);
+			gps = new SeptentrioDriver(arguments.device_path_main, instance, arguments.baud_rate_main);

 		} 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) {
@@ -481,8 +410,7 @@ SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance

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

 		} else {
 			PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : "");
@@ -497,7 +425,6 @@ 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()) {
@@ -521,7 +448,7 @@ int SeptentrioDriver::custom_command(int argc, char *argv[])
 				type = ReceiverResetType::Cold;

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

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

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

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

 int SeptentrioDriver::print_usage(const char *reason)
@@ -546,30 +473,25 @@ int SeptentrioDriver::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-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.
+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.

 ### Examples

-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:
+Starting 2 GPS devices (main one on /dev/ttyS3, secondary on /dev/ttyS4)
 $ septentrio start -d /dev/ttyS3 -e /dev/ttyS4

-Perform warm reset of the receivers:
+Initiate warm restart of GPS device
 $ 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 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_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_DEFAULT_COMMANDS();
 	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver");
@@ -586,15 +508,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_fast);
+		res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout);
 		break;

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

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

 	default:
@@ -631,13 +553,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;
@@ -715,31 +637,42 @@ void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type)
 	}
 }

-bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) {
-	if (set_baudrate(baud_rate) != PX4_OK) {
-		return false;
+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;
 	}

-	if (forced_input) {
-		force_input();
+	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];
+		}
 	}

-	// 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;
+	set_baudrate(original_baud_rate);
+	return 0;
 }

 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_fast;
+	hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout;
 	bool response_detected = false;

 	// Receiver prints prompt after a message.
@@ -749,7 +682,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_fast);
+		int read_result = read(reinterpret_cast<uint8_t *>(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout);

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

-SeptentrioDriver::ConfigureResult SeptentrioDriver::configure()
+int 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_WARN("CONFIG: failed port detection");
-		return ConfigureResult::FailedCompletely;
+		SEP_INFO("CONFIG: failed port detection");
+		return PX4_ERROR;
 	}

 	// 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 ConfigureResult::OK;
+		return PX4_OK;
 	}

 	// If user requested specific baud rate, set it now. Otherwise keep detected 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 (strstr(com_port, "COM") != nullptr && _baud_rate != 0) {
+		snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _baud_rate);

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

 		// 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(2000000);
+		px4_usleep(1000000);

-		if (set_baudrate(_chosen_baud_rate) != PX4_OK) {
-			SEP_WARN("CONFIG: failed local baud rate setting");
-			return ConfigureResult::FailedCompletely;
+		if (set_baudrate(_baud_rate) != PX4_OK) {
+			SEP_INFO("CONFIG: failed local 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;
+		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;
 		}
+	} 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_fast)) {
-		SEP_WARN("CONFIG: failed delete output");
-		return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed
+	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
 	}

-	// 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)) {
+		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");
+			}
 		}
-		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;
+	} 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;
 		}
 	}

@@ -935,77 +919,42 @@ SeptentrioDriver::ConfigureResult 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_fast)) {
-			result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
+		if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout)) {
+			SEP_ERR("Failed to configure logging");
+			return PX4_ERROR;
 		}
 	} else if (_receiver_stream_log == _receiver_stream_main) {
-		result = static_cast<ConfigureResult>(static_cast<int32_t>(result) | static_cast<int32_t>(ConfigureResult::NoLogging));
+		SEP_WARN("Skipping logging setup: logging stream can't equal main stream");
 	}

-	// 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");
-			}
+	// 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_WARN("CONFIG: Failed to configure attitude source");
-			return ConfigureResult::FailedCompletely;
+		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;
 		}
 	}

-	return result;
+	return PX4_OK;
 }

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

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

-				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 {
+				// 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) {
 					_message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE;
 				}

@@ -1134,22 +1082,23 @@ 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);

-				if (pvt_geodetic.cog > k_dnu_f4_value) {
-					_message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F;
-				}
+				_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;
@@ -1229,8 +1178,14 @@ int SeptentrioDriver::process_message()
 			VelCovGeodetic vel_cov_geodetic;

 			if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) {
-				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);
+				_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;
 				}
 			}

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

 			AttEuler att_euler;

-			if (_sbf_decoder.parse(&att_euler) == PX4_OK &&
+			if (_sbf_decoder.parse(&att_euler) &&
 			    !att_euler.error_not_requested &&
 			    att_euler.error_aux1 == Error::None &&
-			    att_euler.error_aux2 == Error::None &&
-			    att_euler.heading > k_dnu_f4_value) {
+			    att_euler.error_aux2 == Error::None) {
 				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].
@@ -1276,8 +1230,7 @@ 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.cov_headhead > k_dnu_f4_value) {
+			    att_cov_euler.error_aux2 == Error::None) {
 				_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]
 			}

@@ -1336,16 +1289,13 @@ 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());

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

@@ -1542,6 +1492,10 @@ 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) {
@@ -1569,11 +1523,6 @@ 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{};
@@ -1719,11 +1668,13 @@ void SeptentrioDriver::set_clock(timespec rtc_gps_time)

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

-	return false;
+	return true;
 }

 void SeptentrioDriver::reset_gps_state_message()
@@ -47,7 +47,6 @@
 #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>
@@ -272,20 +271,9 @@ 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,
 	};
@@ -307,24 +295,9 @@ private:
 	};

 	/**
-	 * 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.
+	 * Maximum timeout to wait for command acknowledgement by the receiver.
 	*/
-	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;
+	static constexpr uint16_t k_receiver_ack_timeout = 200;

 	/**
 	 * Duration of one update monitoring interval in us.
@@ -333,11 +306,6 @@ 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.
 	*/
@@ -379,15 +347,13 @@ private:
 	void schedule_reset(ReceiverResetType type);

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

 	/**
 	 * @brief Try to detect the serial port used on the receiver side.
@@ -403,9 +369,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 `ConfigureResult::OK` if configured, or error.
+	 * @return `PX4_OK` on success, `PX4_ERROR` otherwise.
 	 */
-	ConfigureResult configure();
+	int configure();

 	/**
 	 * @brief Parse the next byte of a received message from the receiver.
@@ -539,13 +505,6 @@ 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.
 	 *
@@ -620,9 +579,6 @@ 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;
@@ -655,9 +611,6 @@ 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;
@@ -713,7 +666,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                               _chosen_baud_rate {0};                                        ///< The baud rate requested by the user
+	uint32_t                               _baud_rate {0};
 	static px4::atomic<SeptentrioDriver *> _secondary_instance;
 	hrt_abstime                            _sleep_end {0};                                               ///< End time for sleeping
 	State                                  _resume_state {State::OpeningSerialPort};                     ///< Resume state after sleep
@@ -730,7 +683,6 @@ 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,8 +39,6 @@

 #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_v);
+	snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_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_v = battery.voltage_v;
+			_battery_voltage_filtered_v = battery.voltage_filtered_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_v{0.f};
+	float _battery_voltage_filtered_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_v * 10;
-					uint16_t current = battery_status.current_a * 10;
+					uint16_t voltage = battery_status.voltage_filtered_v * 10;
+					uint16_t current = battery_status.current_filtered_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) + 1000;
+					uint16_t altitude = static_cast<int16_t>(sensor_gps.altitude_msl_m * 1e3) + 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_v * 10;
-	uint16_t current = battery_status.current_a * 10;
+	uint16_t voltage = battery_status.voltage_filtered_v * 10;
+	uint16_t current = battery_status.current_filtered_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_v * FACTOR_VOLTS_TO_10MV;
-		current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA;
+		voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV;
+		current_in_10mA = battery_status.current_filtered_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,11 +146,13 @@ 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,7 +115,9 @@ 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,7 +104,9 @@ 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 ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) {
+	if (valid_pos_cov && !_system_clock_set) {
 		timespec ts{};

 		// get the whole microseconds
@@ -46,7 +46,6 @@
 #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),
@@ -54,9 +53,10 @@ 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);
-	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
-	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
+	_throttle_filter.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,6 +81,9 @@ 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);

@@ -94,36 +97,29 @@ 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) {
-		resetInternalResistanceEstimation(_voltage_v, _current_a);
+		_voltage_filter_v.reset(_voltage_v);
+		_current_filter_a.reset(_current_a);
 	}

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

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

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

 	if (!_external_state_of_charge) {
 		estimateStateOfCharge();
@@ -153,7 +149,9 @@ 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;
@@ -169,14 +167,6 @@ 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;
 }

@@ -223,69 +213,27 @@ 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 according to internal resistance and current
-	if (current_a > FLT_EPSILON) {
-		updateInternalResistanceEstimation(voltage_v, current_a);
+	// 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;

-		if (_params.r_internal >= 0.f) { // Use user specified internal resistance value
-			cell_voltage += _params.r_internal * 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();

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

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

-	_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);
-	}
+	return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
 }

 void Battery::estimateStateOfCharge()
@@ -406,6 +354,7 @@ 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,6 +58,7 @@
 #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.
@@ -117,6 +118,7 @@ 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;
@@ -130,6 +132,7 @@ protected:
 		float v_charged;
 		int32_t  n_cells;
 		float capacity;
+		float v_load_drop;
 		float r_internal;
 		float low_thr;
 		float crit_thr;
@@ -152,6 +155,7 @@ 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)};
@@ -163,11 +167,12 @@ private:
 	uint8_t _priority{0};
 	bool _battery_initialized{false};
 	float _voltage_v{0.f};
-	AlphaFilter<float> _ocv_filter_v;
-	AlphaFilter<float> _cell_voltage_filter_v;
+	AlphaFilter<float> _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]
@@ -178,19 +183,4 @@ 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
 };
@@ -1,208 +0,0 @@
-# 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,11 +39,33 @@ 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 instead of the online estimated internal resistance.
+                    If non-negative, then this will be used in place of
+                    BAT${i}_V_LOAD_DROP for all calculations.

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

        BAT${i}_N_CELLS:
            description:
@@ -261,11 +261,13 @@ 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,6 +131,7 @@ 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,7 +33,6 @@

 px4_add_library(mathlib
 	math/test/test.cpp
-	math/filter/FilteredDerivative.hpp
 	math/filter/LowPassFilter2p.hpp
 	math/filter/MedianFilter.hpp
 	math/filter/NotchFilter.hpp
@@ -290,11 +290,6 @@ inline bool isFinite(const float &value)
 	return PX4_ISFINITE(value);
 }

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

@@ -279,76 +277,16 @@ 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
-	    || _first_principle_check_failed) {
+	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_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 && !_first_principle_check_failed) {
+		   && !_load_factor_check_failed) {
 		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}
@@ -41,8 +41,6 @@
 #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;
@@ -68,9 +66,6 @@ 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
@@ -88,9 +83,6 @@ 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);

@@ -126,10 +118,6 @@ 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:

@@ -139,17 +127,10 @@ 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
@@ -177,17 +158,6 @@ 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)
@@ -215,8 +185,6 @@ 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,7 +57,6 @@
 #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>
@@ -113,7 +112,6 @@ 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)};
@@ -127,7 +125,6 @@ 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 {};
@@ -165,16 +162,9 @@ 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_FIRST_PRINCIPLE_BIT = (1 << 4)
+		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3)
 	};

-
-	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,
@@ -195,12 +185,8 @@ 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::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
+		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas
 	)

 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -217,8 +203,6 @@ 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();

@@ -371,9 +355,6 @@ 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++) {
@@ -461,14 +442,6 @@ 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();
@@ -503,11 +476,6 @@ 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());
 	}
 }

@@ -533,8 +501,6 @@ 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);
@@ -695,11 +661,6 @@ 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,12 +149,11 @@ 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 31
+ * @max 15
 * @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);
@@ -240,19 +239,3 @@ 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);
--- a/Show More
+++ b/Show More
				`@@ -1 +0,0 @@`
				`# Same as default, only defconfig is different`