Merge remote-tracking branch 'px4/main' into pr-ekf2_astyle_follow_up

2026-11-08 22:21:35 +08:00 · 2024-07-15 14:32:48 -04:00
parent 82b6797547 1cd7d54170
commit 9712c602b9
66 changed files with 2504 additions and 386 deletions
@@ -11,6 +11,8 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    env:
+      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -11,6 +11,8 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
+    env:
+      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
@@ -0,0 +1,47 @@
+#!/bin/sh
+# @name Rover Ackermann
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_ackermann_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Rover parameters
+param set-default NAV_ACC_RAD 0.5
+param set-default RA_ACC_RAD_GAIN 2
+param set-default RA_ACC_RAD_MAX 3
+param set-default RA_LOOKAHD_GAIN 1
+param set-default RA_LOOKAHD_MAX 10
+param set-default RA_LOOKAHD_MIN 1
+param set-default RA_MAX_ACCEL 1.5
+param set-default RA_MAX_JERK 15
+param set-default RA_MAX_SPEED 3
+param set-default RA_MAX_STR_ANG 0.5236
+param set-default RA_MAX_STR_RATE 360
+param set-default RA_MISS_VEL_DEF 3
+param set-default RA_MISS_VEL_GAIN 5
+param set-default RA_MISS_VEL_MIN 1
+param set-default RA_SPEED_I 0.01
+param set-default RA_SPEED_P 2
+param set-default RA_WHEEL_BASE 0.321
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+# Wheels
+param set-default SIM_GZ_WH_FUNC1 101
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+# Steering
+param set-default SIM_GZ_SV_FUNC1 201
+param set-default SIM_GZ_SV_REV 1
@@ -83,6 +83,7 @@ px4_add_romfs_files(
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
+	4012_gz_rover_ackermann

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -0,0 +1 @@
+# Same as default, only defconfig is different
@@ -0,0 +1,257 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H743ZI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1016
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_WATCHDOG=y
@@ -0,0 +1 @@
+# Same as default, only defconfig is different
@@ -0,0 +1,259 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_DISABLE_PTHREAD is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_CAT is not set
+# CONFIG_NSH_DISABLE_CD is not set
+# CONFIG_NSH_DISABLE_CP is not set
+# CONFIG_NSH_DISABLE_DATE is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_ECHO is not set
+# CONFIG_NSH_DISABLE_ENV is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_EXPORT is not set
+# CONFIG_NSH_DISABLE_FREE is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_HELP is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_KILL is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_LS is not set
+# CONFIG_NSH_DISABLE_MKDIR is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_MOUNT is not set
+# CONFIG_NSH_DISABLE_MV is not set
+# CONFIG_NSH_DISABLE_PS is not set
+# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set
+# CONFIG_NSH_DISABLE_PWD is not set
+# CONFIG_NSH_DISABLE_RM is not set
+# CONFIG_NSH_DISABLE_RMDIR is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_SET is not set
+# CONFIG_NSH_DISABLE_SLEEP is not set
+# CONFIG_NSH_DISABLE_SOURCE is not set
+# CONFIG_NSH_DISABLE_TEST is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+# CONFIG_NSH_DISABLE_UMOUNT is not set
+# CONFIG_NSH_DISABLE_UNSET is not set
+# CONFIG_NSH_DISABLE_USLEEP is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorangeplus/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32h7"
+CONFIG_ARCH_CHIP_STM32H747XI=y
+CONFIG_ARCH_CHIP_STM32H7=y
+CONFIG_ARCH_INTERRUPTSTACK=768
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_ASSERT_RESET_VALUE=0
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=79954
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x1058
+CONFIG_CDCACM_PRODUCTSTR="CubeOrange+"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x2DAE
+CONFIG_CDCACM_VENDORSTR="CubePilot"
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_TCBINFO=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_EXPERIMENTAL=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_INIT_ENTRYPOINT="nsh_main"
+CONFIG_INIT_STACKSIZE=3194
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_MAX_EXITFUNS=1
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=4
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_PROGMEM=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NAME_MAX=40
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5
+CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDMMC1_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32H7_ADC1=y
+CONFIG_STM32H7_ADC3=y
+CONFIG_STM32H7_BBSRAM=y
+CONFIG_STM32H7_BBSRAM_FILES=5
+CONFIG_STM32H7_BKPSRAM=y
+CONFIG_STM32H7_DMA1=y
+CONFIG_STM32H7_DMA2=y
+CONFIG_STM32H7_DMACAPABLE=y
+CONFIG_STM32H7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32H7_I2C1=y
+CONFIG_STM32H7_I2C2=y
+CONFIG_STM32H7_I2C4=y
+CONFIG_STM32H7_I2C_DYNTIMEO=y
+CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32H7_OTGFS=y
+CONFIG_STM32H7_PROGMEM=y
+CONFIG_STM32H7_PWR_DIRECT_SMPS_SUPPLY=y
+CONFIG_STM32H7_RTC=y
+CONFIG_STM32H7_RTC_HSECLOCK=y
+CONFIG_STM32H7_RTC_MAGIC_REG=1
+CONFIG_STM32H7_SAVE_CRASHDUMP=y
+CONFIG_STM32H7_SDMMC1=y
+CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32H7_SPI1=y
+CONFIG_STM32H7_SPI1_DMA=y
+CONFIG_STM32H7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32H7_SPI2=y
+CONFIG_STM32H7_SPI4=y
+CONFIG_STM32H7_SPI4_DMA=y
+CONFIG_STM32H7_SPI4_DMA_BUFFER=1024
+CONFIG_STM32H7_TIM1=y
+CONFIG_STM32H7_TIM3=y
+CONFIG_STM32H7_TIM4=y
+CONFIG_STM32H7_UART4=y
+CONFIG_STM32H7_UART7=y
+CONFIG_STM32H7_UART8=y
+CONFIG_STM32H7_USART2=y
+CONFIG_STM32H7_USART3=y
+CONFIG_STM32H7_USART6=y
+CONFIG_STM32H7_USART_BREAKS=y
+CONFIG_STM32H7_USART_INVERT=y
+CONFIG_STM32H7_USART_SINGLEWIRE=y
+CONFIG_STM32H7_USART_SWAP=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_TTY_SIGINT=y
+CONFIG_TTY_SIGINT_CHAR=0x03
+CONFIG_TTY_SIGTSTP=y
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_SERIAL_CONSOLE=y
+CONFIG_UART7_TXBUFSIZE=1500
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=1500
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_IFLOWCONTROL=y
+CONFIG_USART3_OFLOWCONTROL=y
+CONFIG_USART3_RXBUFSIZE=600
+CONFIG_USART3_TXBUFSIZE=3000
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_WATCHDOG=y
@@ -26,12 +26,12 @@ bool cs_mag_fault             # 18 - true when the magnetometer has been declare
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
 bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
-bool cs_gps_yaw               # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
+bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
-bool cs_gps_yaw_fault         # 27 - true when the GNSS heading has been declared faulty and is no longer being used
+bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
 bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
@@ -14,8 +14,10 @@ float32 alt			# Altitude AMSL, (meters)
 float32 alt_ellipsoid		# Altitude above ellipsoid, (meters)

 float32 delta_alt 	# Reset delta for altitude
+float32 delta_terrain   # Reset delta for terrain
 uint8 lat_lon_reset_counter	# Counter for reset events on horizontal position coordinates
 uint8 alt_reset_counter 	# Counter for reset events on altitude
+uint8 terrain_reset_counter     # Counter for reset events on terrain

 float32 eph			# Standard deviation of horizontal position error, (metres)
 float32 epv			# Standard deviation of vertical position error, (metres)
@@ -55,9 +55,13 @@ float64 ref_lon				# Reference point longitude, (degrees)
 float32 ref_alt				# Reference altitude AMSL, (metres)

 # Distance to surface
+bool dist_bottom_valid			# true if distance to bottom surface is valid
 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
+
+float32 delta_dist_bottom               # Amount of vertical shift of dist bottom estimate in latest reset [m]
+uint8 dist_bottom_reset_counter         # Index of latest dist bottom estimate reset
+
 uint8 dist_bottom_sensor_bitfield	# bitfield indicating what type of sensor is used to estimate dist_bottom
 uint8 DIST_BOTTOM_SENSOR_NONE = 0
 uint8 DIST_BOTTOM_SENSOR_RANGE = 1	# (1 << 0) a range sensor is used to estimate dist_bottom field
@@ -47,6 +47,7 @@ set(SRCS_COMMON
 	Subscription.cpp
 	Subscription.hpp
 	SubscriptionCallback.hpp
+	SubscriptionInterval.cpp
 	SubscriptionInterval.hpp
 	SubscriptionMultiArray.hpp
 	uORB.cpp
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "SubscriptionInterval.hpp"
+
+namespace uORB
+{
+
+bool SubscriptionInterval::updated()
+{
+	if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
+		return _subscription.updated();
+	}
+
+	return false;
+}
+
+bool SubscriptionInterval::update(void *dst)
+{
+	if (updated()) {
+		return copy(dst);
+	}
+
+	return false;
+}
+
+bool SubscriptionInterval::copy(void *dst)
+{
+	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;
+		}
+
+		return true;
+	}
+
+	return false;
+}
+
+} // namespace uORB
@@ -93,53 +93,21 @@ public:
 	/**
 	 * Check if there is a new update.
 	 * */
-	bool updated()
-	{
-		if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) {
-			return _subscription.updated();
-		}
-
-		return false;
-	}
+	bool updated();

 	/**
 	 * Copy the struct if updated.
 	 * @param dst The destination pointer where the struct will be copied.
 	 * @return true only if topic was updated and copied successfully.
 	 */
-	bool update(void *dst)
-	{
-		if (updated()) {
-			return copy(dst);
-		}
-
-		return false;
-	}
+	bool update(void *dst);

 	/**
 	 * Copy the struct
 	 * @param dst The destination pointer where the struct will be copied.
 	 * @return true only if topic was copied successfully.
 	 */
-	bool copy(void *dst)
-	{
-		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;
-			}
-
-			return true;
-		}
-
-		return false;
-	}
+	bool copy(void *dst);

 	bool		valid() const { return _subscription.valid(); }

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2017-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
@@ -33,6 +33,7 @@

 add_subdirectory(bmp280)
 add_subdirectory(bmp388)
+add_subdirectory(bmp581)
 add_subdirectory(dps310)
 add_subdirectory(lps22hb)
 #add_subdirectory(lps25h) # not ready for general inclusion
@@ -4,6 +4,7 @@ menu "barometer"
        default n
        select DRIVERS_BAROMETER_BMP280
        select DRIVERS_BAROMETER_BMP388
+        select DRIVERS_BAROMETER_BMP581
        select DRIVERS_BAROMETER_DPS310
        select DRIVERS_BAROMETER_LPS22HB
        select DRIVERS_BAROMETER_LPS33HW
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__barometer__bmp581
+	MAIN bmp581
+	SRCS
+		bmp581_spi.cpp
+		bmp581_i2c.cpp
+		bmp581.cpp
+		bmp581_main.cpp
+	DEPENDS
+		px4_work_queue
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_BMP581
+	bool "bmp581"
+	default n
+	---help---
+		Enable support for bmp581
@@ -0,0 +1,727 @@
+/****************************************************************************
+ *
+ *   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 FITNESSf
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "bmp581.h"
+#include <cstdint>
+
+BMP581::BMP581(const I2CSPIDriverConfig &config, IBMP581 *interface) :
+	I2CSPIDriver(config),
+	_interface(interface),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
+	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors"))
+{
+}
+
+BMP581::~BMP581()
+{
+	/* free perf counters */
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+
+	delete _interface;
+}
+
+int BMP581::init()
+{
+	int ret;
+
+	ret = soft_reset();
+
+	if (ret != PX4_OK) {
+		PX4_DEBUG("failed to reset baro during init");
+		return -EIO;
+	}
+
+	_chip_id = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR);
+
+	if ((_chip_id != BMP581_CHIP_ID_PRIM) && (_chip_id != BMP581_CHIP_ID_SEC)) {
+		PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP581_CHIP_ID_PRIM, BMP581_CHIP_ID_SEC);
+		return -EIO;
+	}
+
+	_chip_rev_id = _interface->get_reg(BMP5_REG_REV_ID_ADDR);
+
+	ret = set_config();
+
+	if (ret != PX4_OK) {
+		PX4_WARN("failed to set_config");
+		return -EIO;
+	}
+
+	start();
+
+	return PX4_OK;
+}
+
+void BMP581::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	printf("chip id: 0x%02x rev id: 0x%02x\n", _chip_id, _chip_rev_id);
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_perf);
+	perf_print_counter(_comms_errors);
+	printf("measurement interval: %u us \n", _measure_interval);
+}
+
+void BMP581::start()
+{
+	_collect_phase = false;
+
+	ScheduleOnInterval(_measure_interval, _measure_interval * 3);
+}
+
+void BMP581::RunImpl()
+{
+
+	if (_collect_phase) {
+		collect();
+	}
+
+	measure();
+}
+
+int BMP581::measure()
+{
+	int ret;
+
+	_collect_phase = true;
+
+	perf_begin(_measure_perf);
+
+	/* start measurement */
+	ret = set_power_mode(BMP5_POWERMODE_FORCED);
+
+	if (ret != PX4_OK) {
+		PX4_DEBUG("failed to set power mode");
+		perf_count(_comms_errors);
+		perf_cancel(_measure_perf);
+		return -EIO;
+	}
+
+	perf_end(_measure_perf);
+
+	return PX4_OK;
+}
+
+int BMP581::collect()
+{
+	_collect_phase = false;
+
+	bmp5_sensor_data data{};
+
+	uint8_t int_status;
+	int ret;
+
+	perf_begin(_sample_perf);
+
+	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	int_status = get_interrupt_status();
+
+	if (int_status & BMP5_INT_ASSERTED_DRDY) {
+		ret = get_sensor_data(&data);
+
+		if (ret != PX4_OK) {
+			perf_count(_comms_errors);
+			perf_cancel(_sample_perf);
+			return -EIO;
+		}
+	}
+
+	//publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = data.pressure;
+	sensor_baro.temperature = data.temperature;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
+	perf_end(_sample_perf);
+
+	return PX4_OK;
+}
+
+/*!
+ * @brief This API performs the soft reset of the sensor
+ *
+ * https://github.com/boschsensortec/BMP5-Sensor-API/blob/master/bmp5.c
+ */
+int BMP581::soft_reset()
+{
+	uint8_t status;
+	int ret;
+
+	if (intf == BMP5_SPI_INTF) {
+		/* Performing a single read via SPI of registers,
+		* e.g. registers CHIP_ID, before the actual
+		* SPI communication with the device.
+		*/
+		status = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR);
+	}
+
+	ret = _interface->set_reg(BMP5_SOFT_RESET_CMD, BMP5_REG_CMD_ADDR);
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	usleep(BMP5_DELAY_US_SOFT_RESET);
+
+	if (intf == BMP5_SPI_INTF) {
+		/* Performing a single read via SPI of registers,
+		* e.g. registers CHIP_ID, before the actual
+		* SPI communication with the device.
+		*/
+		status = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR);
+	}
+
+	status = _interface->get_reg(BMP5_REG_STATUS_ADDR);
+
+	/* Check if nvm_rdy status = 1 and nvm_err status = 0 to proceed */
+	if (!((status & BMP5_INT_NVM_RDY) && (!(status & BMP5_INT_NVM_ERR)))) {
+		return PX4_ERROR;
+	}
+
+	status = _interface->get_reg(BMP5_REG_INT_STATUS_ADDR);
+
+	if (status & BMP5_INT_ASSERTED_POR_SOFTRESET_COMPLETE) {
+		return PX4_OK;
+
+	} else {
+		return PX4_ERROR;
+	}
+}
+
+int BMP581::set_config()
+{
+	int ret;
+
+	ret = set_power_mode(BMP5_POWERMODE_STANDBY);
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	ret = set_osr_odr_press_config();
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	ret = set_iir_config();
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	ret = configure_interrupt();
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	ret = int_source_select();
+
+	if (ret != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+/*!
+ *  @brief This API is used to get interrupt status.
+ */
+uint8_t BMP581::get_interrupt_status()
+{
+	uint8_t status = 0xFF;
+
+	status = _interface->get_reg(BMP5_REG_INT_STATUS_ADDR);
+
+	return status;
+}
+
+uint8_t BMP581::check_deepstandby_mode()
+{
+	int rslt;
+	uint8_t fifo_frame_sel;
+	uint8_t reg_data[2];
+	uint8_t powermode = 0xFF;
+
+	fifo_frame_sel = _interface->get_reg(BMP5_REG_FIFO_SEL_ADDR);
+	fifo_frame_sel = BMP5_GET_BITS_POS_0(fifo_frame_sel, BMP5_FIFO_FRAME_SEL);
+
+	rslt = _interface->get_reg_buf(BMP5_REG_OSR_CONFIG_ADDR, reg_data, 2);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	uint8_t odr_reg = 0xFF;
+	odr_reg = BMP5_GET_BITSLICE(reg_data[1], BMP5_ODR);
+
+	rslt = _interface->get_reg_buf(BMP5_REG_DSP_CONFIG_ADDR, reg_data, 2);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	uint8_t set_iir_t_reg = 0xFF;
+	uint8_t set_iir_p_reg = 0xFF;
+
+	set_iir_t_reg = BMP5_GET_BITS_POS_0(reg_data[1], BMP5_SET_IIR_TEMP);
+	set_iir_p_reg = BMP5_GET_BITSLICE(reg_data[1], BMP5_SET_IIR_PRESS);
+
+	/* As per datasheet odr should be less than 5Hz. But register value for 5Hz is less than 4Hz and so,
+	* thus in this below condition odr is checked whether greater than 5Hz macro.
+	*/
+	if ((odr_reg > BMP5_ODR_05_HZ) && (fifo_frame_sel == BMP5_DISABLE) &&
+	    (set_iir_t_reg == BMP5_IIR_FILTER_BYPASS) && (set_iir_p_reg == BMP5_IIR_FILTER_BYPASS)) {
+		powermode = BMP5_POWERMODE_DEEP_STANDBY;
+	}
+
+	return powermode;
+}
+
+/*!
+ *  @brief This API is used to get powermode of the sensor.
+ */
+uint8_t BMP581::get_power_mode()
+{
+	uint8_t deep_dis;
+	uint8_t reg_data;
+	uint8_t powermode;
+
+	reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR);
+	powermode = BMP5_GET_BITS_POS_0(reg_data, BMP5_POWERMODE);
+
+	if (powermode == BMP5_POWERMODE_STANDBY) {
+		deep_dis = BMP5_GET_BITSLICE(reg_data, BMP5_DEEP_DISABLE);
+
+		if (deep_dis == BMP5_DEEP_ENABLED) {
+			powermode = check_deepstandby_mode();
+		}
+	}
+
+	return powermode;
+}
+
+/*!
+ *  @brief This API is used to set powermode of the sensor.
+ */
+int BMP581::set_power_mode(uint8_t power_mode)
+{
+	uint8_t lst_pwrmode;
+	uint8_t reg_data;
+	int rslt;
+
+	lst_pwrmode = get_power_mode();
+
+	if (lst_pwrmode != BMP5_POWERMODE_STANDBY) {
+		reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR);
+		reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_DEEP_DISABLE, BMP5_DEEP_DISABLED);
+		reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_POWERMODE, BMP5_POWERMODE_STANDBY);
+		rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR);
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+
+		ScheduleDelayed(BMP5_DELAY_US_STANDBY);
+	}
+
+	switch (power_mode) {
+	case BMP5_POWERMODE_DEEP_STANDBY:
+		rslt = set_deep_standby_mode();
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+
+		break;
+
+	case BMP5_POWERMODE_STANDBY:
+		break;
+
+	case BMP5_POWERMODE_NORMAL:
+	case BMP5_POWERMODE_FORCED:
+	case BMP5_POWERMODE_CONTINOUS:
+		reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR);
+		reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_DEEP_DISABLE, BMP5_DEEP_DISABLED);
+		reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_POWERMODE, power_mode);
+		rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR);
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+
+		break;
+	}
+
+	return PX4_OK;
+}
+
+/*!
+ * @brief This internal API is used to set sensor in deep standby mode.
+ */
+int BMP581::set_deep_standby_mode()
+{
+	uint8_t reg_data;
+	int rslt;
+
+	reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR);
+	reg_data = BMP5_SET_BIT_VAL_0(reg_data, BMP5_DEEP_DISABLE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_ODR, BMP5_ODR_01_HZ);
+	rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	reg_data = _interface->get_reg(BMP5_REG_DSP_IIR_ADDR);
+	reg_data = reg_data & BMP5_IIR_BYPASS;
+	rslt = _interface->set_reg(reg_data, BMP5_REG_DSP_IIR_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	reg_data = _interface->get_reg(BMP5_REG_FIFO_SEL_ADDR);
+	reg_data = BMP5_SET_BIT_VAL_0(reg_data, BMP5_FIFO_FRAME_SEL);
+	rslt = _interface->set_reg(reg_data, BMP5_REG_FIFO_SEL_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+/*!
+ * @brief This internal API is used to set sensor in standby powermode when powermode is deepstandby mode.
+ */
+int BMP581::set_standby_mode()
+{
+	uint8_t powermode;
+	int ret;
+
+	powermode = get_power_mode();
+
+	if (powermode == BMP5_POWERMODE_DEEP_STANDBY) {
+		ret = set_power_mode(BMP5_POWERMODE_STANDBY);
+
+		if (ret != PX4_OK) {
+			return PX4_ERROR;
+		}
+	}
+
+	return PX4_OK;
+}
+
+uint32_t BMP581::get_measurement_time()
+{
+	/*
+	  From BST-BMP3581-DS004-04.pdf, page 19, table 9
+
+	  Pressure      Temperature    Measurement
+	  Oversample    Oversample     Time
+	  1x            1x             2.7 ms
+	  2x            1x             3.3 ms
+	  4x            1x             4.6 ms
+	  8x            1x             7.2 ms
+	  16x           1x             12.5 ms
+	  32x           2x             23.3 ms
+	  64x           4x	       44.2 ms
+	  128x          8x             88.0 ms
+	*/
+
+	uint32_t meas_time_us = 0; // unsupported value by default
+
+	if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_1X) {
+		switch (OVERSAMPLING_PRESSURE) {
+		case BMP5_OVERSAMPLING_1X:
+			meas_time_us = 2700;
+			break;
+
+		case BMP5_OVERSAMPLING_2X:
+			meas_time_us = 3300;
+			break;
+
+		case BMP5_OVERSAMPLING_4X:
+			meas_time_us = 4600;
+			break;
+
+		case BMP5_OVERSAMPLING_8X:
+			meas_time_us = 7200;
+			break;
+
+		case BMP5_OVERSAMPLING_16X:
+			meas_time_us = 12500;
+			break;
+		}
+
+	} else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_2X) {
+		switch (OVERSAMPLING_PRESSURE) {
+		case BMP5_OVERSAMPLING_32X:
+			meas_time_us = 23300;
+			break;
+
+		}
+
+	} else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_4X) {
+		switch (OVERSAMPLING_PRESSURE) {
+		case BMP5_OVERSAMPLING_64X:
+			meas_time_us = 44200;
+			break;
+
+		}
+
+	} else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_8X) {
+		switch (OVERSAMPLING_PRESSURE) {
+		case BMP5_OVERSAMPLING_128X:
+			meas_time_us = 88000;
+			break;
+
+		}
+	}
+
+	return meas_time_us;
+}
+
+/*!
+ *  @brief This API sets the configuration for oversampling temperature, oversampling of
+ *  pressure and ODR configuration along with pressure enable.
+ *
+ *  @note If ODR is set to a value higher than 5Hz then powermode is set as standby mode, as ODR value greater than 5HZ
+ *  without disabling deep-standby mode makes powermode invalid.
+ */
+int BMP581::set_osr_odr_press_config()
+{
+	uint8_t reg_data[2] = {0};
+	int rslt;
+
+	_measure_interval = get_measurement_time();
+
+	if (_measure_interval == 0) {
+		PX4_WARN("unsupported oversampling selected");
+		return false;
+	}
+
+	if (OUTPUT_DATA_RATE < BMP5_ODR_05_HZ) {
+		rslt = set_standby_mode();
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+	}
+
+	rslt = _interface->get_reg_buf(BMP5_REG_OSR_CONFIG_ADDR, reg_data, 2);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	reg_data[0] = BMP5_SET_BITS_POS_0(reg_data[0], BMP5_TEMP_OS, OVERSAMPLING_TEMPERATURE);
+	reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_PRESS_OS, OVERSAMPLING_PRESSURE);
+	reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_PRESS_EN, PRESSURE_ENABLE);
+	reg_data[1] = BMP5_SET_BITSLICE(reg_data[1], BMP5_ODR, OUTPUT_DATA_RATE);
+
+	rslt = _interface->set_reg(reg_data[0], BMP5_REG_OSR_CONFIG_ADDR);
+	rslt = _interface->set_reg(reg_data[1], BMP5_REG_ODR_CONFIG_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+/*!
+ *  @brief This API sets the configuration for IIR of temperature and pressure.
+ *
+ *  @note If IIR value for both temperature and pressure is set a value other than bypass then powermode is set
+ *  as standby mode, as IIR with value other than bypass without disabling deep-standby mode makes powermode invalid.
+ */
+int BMP581::set_iir_config()
+{
+	uint8_t curr_powermdoe;
+	uint8_t reg_data[2];
+	int rslt;
+
+	if ((IIR_FILTER_COEFF_TEMPERATURE != BMP5_IIR_FILTER_BYPASS) || (IIR_FILTER_COEFF_PRESSURE != BMP5_IIR_FILTER_BYPASS)) {
+		rslt = set_standby_mode();
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+	}
+
+	curr_powermdoe = get_power_mode();
+
+	if (curr_powermdoe != BMP5_POWERMODE_STANDBY) {
+		rslt = set_power_mode(BMP5_POWERMODE_STANDBY);
+
+		if (rslt != PX4_OK) {
+			return PX4_ERROR;
+		}
+	}
+
+	rslt = _interface->get_reg_buf(BMP5_REG_DSP_CONFIG_ADDR, reg_data, 2);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_SHDW_SET_IIR_TEMP, BMP5_ENABLE);
+	reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_SHDW_SET_IIR_PRESS, BMP5_ENABLE);
+	reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_IIR_FLUSH_FORCED_EN, BMP5_ENABLE);
+
+	reg_data[1] = IIR_FILTER_COEFF_TEMPERATURE;
+	reg_data[1] = BMP5_SET_BITSLICE(reg_data[1], BMP5_SET_IIR_PRESS, IIR_FILTER_COEFF_PRESSURE);
+
+	rslt = _interface->set_reg(reg_data[0], BMP5_REG_DSP_CONFIG_ADDR);
+	rslt = _interface->set_reg(reg_data[1], BMP5_REG_DSP_IIR_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* Since IIR works only in standby mode we are not re-writing to deepstandby mode
+	* as deep standby mode resets the IIR settings to default
+	*/
+	if ((curr_powermdoe != BMP5_POWERMODE_STANDBY) && (curr_powermdoe != BMP5_POWERMODE_DEEP_STANDBY)) {
+		if (!set_power_mode(curr_powermdoe)) {
+			return PX4_ERROR;
+		}
+
+	}
+
+	return PX4_OK;
+}
+
+/*!
+* @brief This API is used to configure the interrupt settings.
+*/
+int BMP581::configure_interrupt()
+{
+	uint8_t reg_data = 0;
+	uint8_t int_source = 0;
+	int rslt;
+
+	reg_data = _interface->get_reg(BMP5_REG_INT_CONFIG_ADDR);
+
+	/* Any change between latched/pulsed mode has to be applied while interrupt is disabled */
+	/* Step 1 : Turn off all INT sources (INT_SOURCE -> 0x00) */
+	rslt = _interface->set_reg(int_source, BMP5_REG_INT_SOURCE_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	/* Step 2 : Read the INT_STATUS register to clear the status */
+	_interface->get_reg(BMP5_REG_INT_STATUS_ADDR);
+
+	/* Step 3 : Set the desired mode in INT_CONFIG.int_mode */
+	reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_INT_MODE, INTERRUPT_MODE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_POL, INTERRUPT_POLARITY);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_OD, INTERRUPT_DRIVE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_EN, BMP5_ENABLE);
+
+	rslt = _interface->set_reg(reg_data, BMP5_REG_INT_CONFIG_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+int BMP581::int_source_select()
+{
+	uint8_t reg_data;
+	int rslt;
+
+	reg_data = _interface->get_reg(BMP5_REG_INT_SOURCE_ADDR);
+
+	reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_INT_DRDY_EN, BMP5_ENABLE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_FIFO_FULL_EN, BMP5_DISABLE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_FIFO_THRES_EN, BMP5_DISABLE);
+	reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_OOR_PRESS_EN, BMP5_DISABLE);
+
+	rslt = _interface->set_reg(reg_data, BMP5_REG_INT_SOURCE_ADDR);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	return PX4_OK;
+}
+
+/*!
+ *  @brief This API reads the temperature(deg C) or both pressure(Pa) and temperature(deg C) data from the
+ * sensor and store it in the bmp5_sensor_data structure instance passed by the user.
+ */
+int BMP581::get_sensor_data(bmp5_sensor_data *sensor_data)
+{
+	uint8_t reg_data[6] = {0};
+	int32_t raw_data_t;
+	uint32_t raw_data_p;
+	int8_t rslt;
+
+	rslt = _interface->get_reg_buf(BMP5_REG_TEMP_DATA_XLSB_ADDR, reg_data, 6);
+
+	if (rslt != PX4_OK) {
+		return PX4_ERROR;
+	}
+
+	raw_data_t = (int32_t)((int32_t)((uint32_t)(((uint32_t)reg_data[2] << 16) | ((uint16_t)reg_data[1] << 8) | reg_data[0])
+					 << 8) >> 8);
+	/* Division by 2^16(whose equivalent value is 65536) is performed to get temperature data in deg C */
+	sensor_data->temperature = (float)(raw_data_t / 65536.0);
+
+	if (PRESSURE_ENABLE == BMP5_ENABLE) {
+		raw_data_p = (uint32_t)((uint32_t)(reg_data[5] << 16) | (uint16_t)(reg_data[4] << 8) | reg_data[3]);
+		/* Division by 2^6(whose equivalent value is 64) is performed to get pressure data in Pa */
+		sensor_data->pressure = (float)(raw_data_p / 64.0);
+
+	} else {
+		sensor_data->pressure = 0.0;
+	}
+
+	return PX4_OK;
+}
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *   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 bmp581.h
+ *
+ * Shared defines for the bmp581 driver.
+ */
+#pragma once
+
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <lib/cdev/CDev.hpp>
+#include <perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
+
+#include "board_config.h"
+
+// From https://github.com/boschsensortec/BMP5-Sensor-API/blob/master/bmp5_defs.h
+
+/* BIT SLICE GET AND SET FUNCTIONS */
+#define BMP5_GET_BITSLICE(regvar, bitname) \
+	((regvar & bitname##_MSK) >> bitname##_POS)
+
+#define BMP5_SET_BITSLICE(regvar, bitname, val) \
+	((regvar & ~bitname##_MSK) | \
+	 ((val << bitname##_POS) & bitname##_MSK))
+
+#define BMP5_SET_BIT_VAL_0(reg_data, bitname)	(reg_data & ~(bitname##_MSK))
+
+#define BMP5_SET_BITS_POS_0(reg_data, bitname, data) \
+	((reg_data & ~(bitname##_MSK)) | \
+	 (data & bitname##_MSK))
+
+#define BMP5_GET_BITS_POS_0(reg_data, bitname)	(reg_data & (bitname##_MSK))
+
+/* Chip id of BMP5 */
+#define BMP581_CHIP_ID_PRIM		(0x50)
+#define BMP581_CHIP_ID_SEC		(0x51)
+
+#define BMP5_ENABLE                     (0x01)
+#define BMP5_DISABLE                    (0x00)
+
+/* Register addresses */
+#define BMP5_REG_CHIP_ID_ADDR           (0x01)
+#define BMP5_REG_REV_ID_ADDR            (0x02)
+#define BMP5_REG_INT_CONFIG_ADDR        (0x14)
+#define BMP5_REG_INT_SOURCE_ADDR        (0x15)
+#define BMP5_REG_FIFO_SEL_ADDR          (0x18)
+#define BMP5_REG_TEMP_DATA_XLSB_ADDR    (0x1D)
+#define BMP5_REG_INT_STATUS_ADDR        (0x27)
+#define BMP5_REG_STATUS_ADDR            (0x28)
+#define BMP5_REG_NVM_ADDR               (0x2B)
+#define BMP5_REG_DSP_CONFIG_ADDR        (0x30)
+#define BMP5_REG_DSP_IIR_ADDR           (0x31)
+#define BMP5_REG_OSR_CONFIG_ADDR        (0x36)
+#define BMP5_REG_ODR_CONFIG_ADDR        (0x37)
+#define BMP5_REG_CMD_ADDR               (0x7E)
+
+/* Delay definition */
+#define BMP5_DELAY_US_SOFT_RESET	(2000)
+#define BMP5_DELAY_US_STANDBY           (2500)
+
+/* Soft reset command */
+#define BMP5_SOFT_RESET_CMD		(0xB6)
+
+/* Deepstandby enable/disable */
+#define BMP5_DEEP_ENABLED		(0)
+#define BMP5_DEEP_DISABLED              (1)
+
+/* ODR settings */
+#define BMP5_ODR_50_HZ                  (0x0F)
+#define BMP5_ODR_05_HZ                  (0x18)
+#define BMP5_ODR_01_HZ                  (0x1C)
+
+
+/* Oversampling for temperature and pressure */
+#define BMP5_OVERSAMPLING_1X		(0x00)
+#define BMP5_OVERSAMPLING_2X		(0x01)
+#define BMP5_OVERSAMPLING_4X		(0x02)
+#define BMP5_OVERSAMPLING_8X		(0x03)
+#define BMP5_OVERSAMPLING_16X		(0x04)
+#define BMP5_OVERSAMPLING_32X		(0x05)
+#define BMP5_OVERSAMPLING_64X		(0x06)
+#define BMP5_OVERSAMPLING_128X		(0x07)
+
+/* IIR filter for temperature and pressure */
+#define BMP5_IIR_FILTER_BYPASS		(0x00)
+#define BMP5_IIR_FILTER_COEFF_1		(0x01)
+
+/* Macro is used to bypass both iir_t and iir_p together */
+#define BMP5_IIR_BYPASS                 (0xC0)
+
+/* Interrupt configurations */
+#define BMP5_INT_MODE_PULSED            (0)
+#define BMP5_INT_POL_ACTIVE_HIGH        (1)
+#define BMP5_INT_OD_PUSHPULL            (0)
+
+/* NVM and Interrupt status asserted macros */
+#define BMP5_INT_ASSERTED_POR_SOFTRESET_COMPLETE  (0x10)
+#define BMP5_INT_NVM_RDY                          (0x02)
+#define BMP5_INT_NVM_ERR                          (0x04)
+
+/* Interrupt configurations */
+#define BMP5_INT_MODE_MSK               (0x01)
+
+#define BMP5_INT_POL_MSK                (0x02)
+#define BMP5_INT_POL_POS                (1)
+
+#define BMP5_INT_OD_MSK                 (0x04)
+#define BMP5_INT_OD_POS                 (2)
+
+#define BMP5_INT_EN_MSK                 (0x08)
+#define BMP5_INT_EN_POS                 (3)
+
+#define BMP5_INT_DRDY_EN_MSK            (0x01)
+
+#define BMP5_INT_FIFO_FULL_EN_MSK       (0x02)
+#define BMP5_INT_FIFO_FULL_EN_POS       (1)
+
+#define BMP5_INT_FIFO_THRES_EN_MSK      (0x04)
+#define BMP5_INT_FIFO_THRES_EN_POS      (2)
+
+#define BMP5_INT_OOR_PRESS_EN_MSK       (0x08)
+#define BMP5_INT_OOR_PRESS_EN_POS       (3)
+
+/* ODR configuration */
+#define BMP5_ODR_MSK                    (0x7C)
+#define BMP5_ODR_POS                    (2)
+
+/* OSR configurations */
+#define BMP5_TEMP_OS_MSK                (0x07)
+
+#define BMP5_PRESS_OS_MSK               (0x38)
+#define BMP5_PRESS_OS_POS               (3)
+
+/* Pressure enable */
+#define BMP5_PRESS_EN_MSK               (0x40)
+#define BMP5_PRESS_EN_POS               (6)
+
+/* IIR configurations */
+#define BMP5_SET_IIR_TEMP_MSK           (0x07)
+
+#define BMP5_SET_IIR_PRESS_MSK          (0x38)
+#define BMP5_SET_IIR_PRESS_POS          (3)
+
+#define BMP5_SHDW_SET_IIR_TEMP_MSK      (0x08)
+#define BMP5_SHDW_SET_IIR_TEMP_POS      (3)
+
+#define BMP5_SHDW_SET_IIR_PRESS_MSK     (0x20)
+#define BMP5_SHDW_SET_IIR_PRESS_POS     (5)
+
+#define BMP5_IIR_FLUSH_FORCED_EN_MSK    (0x04)
+#define BMP5_IIR_FLUSH_FORCED_EN_POS    (2)
+
+/* Powermode */
+#define BMP5_POWERMODE_MSK		(0x03)
+
+#define BMP5_DEEP_DISABLE_MSK		(0x80)
+#define BMP5_DEEP_DISABLE_POS		(7)
+
+/* Fifo configurations */
+#define BMP5_FIFO_FRAME_SEL_MSK        (0x03)
+
+/* NVM and Interrupt status asserted macros */
+#define BMP5_INT_ASSERTED_DRDY         (0x01)
+
+/*!
+ * @brief Enumerator for powermode selection
+ */
+enum bmp5_powermode {
+	BMP5_POWERMODE_STANDBY,
+	BMP5_POWERMODE_NORMAL,
+	BMP5_POWERMODE_FORCED,
+	BMP5_POWERMODE_CONTINOUS,
+	BMP5_POWERMODE_DEEP_STANDBY
+};
+
+/*!
+ * @brief Enumerator for interface selection
+ */
+enum bmp5_intf {
+	BMP5_SPI_INTF,
+	BMP5_I2C_INTF,
+};
+
+/*!
+ * @brief BMP5 sensor data structure which comprises of temperature and pressure in floating point with data type as
+ *  float for pressure and temperature.
+ */
+struct bmp5_sensor_data {
+	float pressure;
+	float temperature;
+};
+
+/*
+ * BMP581 internal constants and data structures.
+ */
+class IBMP581
+{
+public:
+	virtual ~IBMP581() = default;
+
+	virtual int init() = 0;
+
+	// read reg value
+	virtual uint8_t get_reg(uint8_t addr) = 0;
+
+	// bulk read reg value
+	virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
+
+	// write reg value
+	virtual int set_reg(uint8_t value, uint8_t addr) = 0;
+
+	virtual uint32_t get_device_id() const = 0;
+
+	virtual uint8_t get_device_address() const = 0;
+};
+
+class BMP581 : public I2CSPIDriver<BMP581>
+{
+public:
+	BMP581(const I2CSPIDriverConfig &config, IBMP581 *interface);
+	virtual ~BMP581();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	virtual int init();
+
+	void print_status();
+
+	void RunImpl();
+
+
+private:
+	static constexpr uint8_t	OVERSAMPLING_TEMPERATURE{BMP5_OVERSAMPLING_2X};
+	static constexpr uint8_t	OVERSAMPLING_PRESSURE{BMP5_OVERSAMPLING_32X};
+	static constexpr uint8_t	OUTPUT_DATA_RATE{BMP5_ODR_50_HZ};
+	static constexpr uint8_t	PRESSURE_ENABLE{BMP5_ENABLE};
+	static constexpr uint8_t 	IIR_FILTER_COEFF_TEMPERATURE{BMP5_IIR_FILTER_COEFF_1};
+	static constexpr uint8_t 	IIR_FILTER_COEFF_PRESSURE{BMP5_IIR_FILTER_COEFF_1};
+
+	static constexpr uint8_t 	INTERRUPT_MODE{BMP5_INT_MODE_PULSED};
+	static constexpr uint8_t	INTERRUPT_POLARITY{BMP5_INT_POL_ACTIVE_HIGH};
+	static constexpr uint8_t 	INTERRUPT_DRIVE{BMP5_INT_OD_PUSHPULL};
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+	IBMP581 		*_interface{nullptr};
+
+	unsigned		_measure_interval{0};			// interval in microseconds needed to measure
+
+	perf_counter_t 		_sample_perf;
+	perf_counter_t 		_measure_perf;
+	perf_counter_t 		_comms_errors;
+
+	bool			_collect_phase{false};
+
+	uint8_t			_chip_id{0};
+	uint8_t			_chip_rev_id{0};
+
+	void			start();
+	int			measure();
+	int		        collect(); // get results and publish
+
+	uint32_t		get_measurement_time();
+
+	int			soft_reset();
+	int			set_config();
+	uint8_t			get_interrupt_status();
+	int			configure_interrupt();
+	int			int_source_select();
+	uint8_t 		get_power_mode();
+	int			set_power_mode(uint8_t power_mode);
+	uint8_t			check_deepstandby_mode();
+	int			set_standby_mode();
+	int			set_deep_standby_mode();
+	int			set_osr_odr_press_config();
+	int			set_iir_config();
+	int			get_sensor_data(bmp5_sensor_data *sensor_data);
+};
+
+
+/* interface factories */
+extern IBMP581 *bmp581_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+extern IBMP581 *bmp581_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency);
+extern enum bmp5_intf intf;
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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 bmp581_i2c.cpp
+ *
+ * I2C interface for BMP581
+ */
+
+#include <drivers/device/i2c.h>
+
+#include "bmp581.h"
+
+class BMP581_I2C: public device::I2C, public IBMP581
+{
+public:
+	BMP581_I2C(uint8_t bus, uint32_t device, int bus_frequency);
+	virtual ~BMP581_I2C() = default;
+
+	int init();
+
+	uint8_t get_reg(uint8_t addr);
+	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
+	int set_reg(uint8_t value, uint8_t addr);
+
+	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
+};
+
+IBMP581 *bmp581_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency)
+{
+	return new BMP581_I2C(busnum, device, bus_frequency);
+}
+
+BMP581_I2C::BMP581_I2C(uint8_t bus, uint32_t device, int bus_frequency) :
+	I2C(DRV_BARO_DEVTYPE_BMP581, MODULE_NAME, bus, device, bus_frequency)
+{
+}
+
+int BMP581_I2C::init()
+{
+	return I2C::init();
+}
+
+uint8_t BMP581_I2C::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), 0};
+	transfer(&cmd[0], 1, &cmd[1], 1);
+
+	return cmd[1];
+}
+
+int BMP581_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	const uint8_t cmd = (uint8_t)(addr);
+	return transfer(&cmd, sizeof(cmd), buf, len);
+}
+
+int BMP581_I2C::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), value};
+	return transfer(cmd, sizeof(cmd), nullptr, 0);
+}
+
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "bmp581.h"
+
+enum bmp5_intf intf;
+
+void BMP581::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("bmp581", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x46);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+I2CSPIDriverBase *BMP581::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	IBMP581 *interface = nullptr;
+
+	if (config.bus_type == BOARD_I2C_BUS) {
+		interface = bmp581_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
+		intf = BMP5_I2C_INTF;
+
+	} else if (config.bus_type == BOARD_SPI_BUS) {
+		interface = bmp581_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		intf = BMP5_SPI_INTF;
+	}
+
+	if (interface == nullptr) {
+		PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
+		return nullptr;
+	}
+
+	if (interface->init() != OK) {
+		delete interface;
+		PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid);
+		return nullptr;
+	}
+
+	BMP581 *dev = new BMP581(config, interface);
+
+	if (dev == nullptr) {
+		delete interface;
+		return nullptr;
+	}
+
+	if (dev->init() != OK) {
+		delete dev;
+		return nullptr;
+	}
+
+	return dev;
+}
+
+
+extern "C" int bmp581_main(int argc, char *argv[])
+{
+	using ThisDriver = BMP581;
+	BusCLIArguments cli{true, true};
+	cli.i2c_address = 0x46;
+	cli.default_i2c_frequency = 100 * 1000;
+	cli.default_spi_frequency = 10 * 1000 * 1000;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_BMP581);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+
+
+
+
+}
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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 bmp581_spi.cpp
+ *
+ * SPI interface for BMP 581 (NOTE: untested!)
+ */
+
+#include <drivers/device/spi.h>
+
+#include "bmp581.h"
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)  //for set
+#define DIR_WRITE			~(1<<7) //for clear
+
+class BMP581_SPI: public device::SPI, public IBMP581
+{
+public:
+	BMP581_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	virtual ~BMP581_SPI() = default;
+
+	int init();
+
+	uint8_t get_reg(uint8_t addr);
+	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
+	int set_reg(uint8_t value, uint8_t addr);
+
+	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
+};
+
+IBMP581 *bmp581_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
+{
+	return new BMP581_SPI(busnum, device, bus_frequency, spi_mode);
+}
+
+BMP581_SPI::BMP581_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_BARO_DEVTYPE_BMP581, MODULE_NAME, bus, device, spi_mode, bus_frequency)
+{
+}
+
+int BMP581_SPI::init()
+{
+	return SPI::init();
+}
+
+uint8_t BMP581_SPI::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit
+	transfer(&cmd[0], &cmd[0], 2);
+	return cmd[1];
+}
+
+int BMP581_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	uint8_t cmd[len + 1] = {(uint8_t)(addr | DIR_READ)};
+	int ret;
+
+	ret = transfer(&cmd[0], &cmd[0], (len + 1));
+	memcpy(buf, &cmd[1], len);
+
+	return ret;
+}
+
+int BMP581_SPI::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit
+	return transfer(&cmd[0], nullptr, 2);
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2023 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -151,6 +151,7 @@
 #define DRV_ACC_DEVTYPE_BMI085          0x6C
 #define DRV_GYR_DEVTYPE_BMI085          0x6D
 #define DRV_BARO_DEVTYPE_BMP390         0x6E
+#define DRV_BARO_DEVTYPE_BMP581		0x6F

 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71
@@ -524,8 +524,13 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
 	float throttle_setpoint{param.throttle_min};

 	if (1.f - param.fast_descend < FLT_EPSILON) {
-		// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
-		throttle_setpoint = param.throttle_min;
+		// During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending
+		if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing
+			throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet
+
+		} else {
+			throttle_setpoint = param.throttle_min;
+		}

 	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
@@ -623,7 +623,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &
 			}
 		}

-		if (estimator_status_flags.cs_gps_yaw_fault) {
+		if (estimator_status_flags.cs_gnss_yaw_fault) {
 			/* EVENT
 			 * @description
 			 * Land now
@@ -175,7 +175,7 @@ if(CONFIG_EKF2_GNSS)
 	)

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

 	list(APPEND EKF_LIBS yaw_estimator)
@@ -96,7 +96,7 @@ if(CONFIG_EKF2_GNSS)
 	)

 	if(CONFIG_EKF2_GNSS_YAW)
-		list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp)
+		list(APPEND EKF_SRCS aid_sources/gnss/gnss_yaw_control.cpp)
 	endif()

 	add_subdirectory(yaw_estimator)
@@ -33,12 +33,12 @@

 #include "ekf.h"

-void Ekf::controlAuxVelFusion()
+void Ekf::controlAuxVelFusion(const imuSample &imu_sample)
 {
 	if (_auxvel_buffer) {
 		auxVelSample sample;

-		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
+		if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) {

 			updateAidSourceStatus(_aid_src_aux_vel,
 					      sample.time_us,                                           // sample timestamp
@@ -41,7 +41,7 @@
 // Maximum allowable time interval between pressure altitude measurements (uSec)
 static constexpr uint64_t BARO_MAX_INTERVAL{200'000};

-void Ekf::controlBaroHeightFusion()
+void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "baro";

@@ -52,10 +52,10 @@ void Ekf::controlBaroHeightFusion()

 	baroSample baro_sample;

-	if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) {
+	if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) {

 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-		const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt);
+		const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt);
 #else
 		const float measurement = baro_sample.hgt;
 #endif
@@ -137,10 +137,12 @@ void Ekf::controlBaroHeightFusion()
 					resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());

-					// reset vertical velocity
-					resetVerticalVelocityToZero();
+					// reset vertical velocity if no valid sources available
+					if (!isVerticalVelocityAidingActive()) {
+						resetVerticalVelocityToZero();
+					}

-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;

 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -169,7 +171,7 @@ void Ekf::controlBaroHeightFusion()
 					bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
 				}

-				aid_src.time_last_fuse = _time_delayed_us;
+				aid_src.time_last_fuse = imu_sample.time_us;
 				bias_est.setFusionActive();
 				_control_status.flags.baro_hgt = true;
 			}
@@ -198,7 +200,7 @@ void Ekf::stopBaroHgtFusion()
 }

 #if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const
 {
 	if (_control_status.flags.wind && local_position_is_valid()) {
 		// calculate static pressure error = Pmeas - Ptruth
@@ -206,7 +208,8 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated)
 		// negative X and Y directions. Used to correct baro data for positional errors

 		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias;
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (angular_velocity % _params.imu_pos_body);
 		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;

 		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
@@ -41,7 +41,7 @@
 // Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t EV_MAX_INTERVAL{200'000};

-void Ekf::controlExternalVisionFusion()
+void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 {
 	_ev_pos_b_est.predict(_dt_ekf_avg);
 	_ev_hgt_b_est.predict(_dt_ekf_avg);
@@ -49,7 +49,7 @@ void Ekf::controlExternalVisionFusion()
 	// Check for new external vision data
 	extVisionSample ev_sample;

-	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) {
+	if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) {

 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);

@@ -63,10 +63,11 @@ void Ekf::controlExternalVisionFusion()
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);

 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
-		controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
-		controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
-		controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
-		controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt);
+		controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
+		controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
+		controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
+		controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient,
+				      _aid_src_ev_hgt);

 		if (quality_sufficient) {
 			_ev_sample_prev = ev_sample;
@@ -38,8 +38,9 @@

 #include "ekf.h"

-void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV height";

@@ -148,33 +149,6 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 				resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
 				bias_est.setBias(-_state.pos(2) + measurement);

-				// reset vertical velocity
-				if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
-
-					// correct velocity for offset relative to IMU
-					const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
-					const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
-
-					switch (ev_sample.vel_frame) {
-					case VelocityFrame::LOCAL_FRAME_NED:
-					case VelocityFrame::LOCAL_FRAME_FRD: {
-							const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
-							resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
-						}
-						break;
-
-					case VelocityFrame::BODY_FRAME_FRD: {
-							const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
-							const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
-							resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
-						}
-						break;
-					}
-
-				} else {
-					resetVerticalVelocityToZero();
-				}
-
 				aid_src.time_last_fuse = _time_delayed_us;

 			} else if (is_fusion_failing) {
@@ -41,8 +41,9 @@
 static constexpr const char *EV_AID_SRC_NAME = "EV position";


-void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src)
+void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source2d_s &aid_src)
 {
 	const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
@@ -41,8 +41,9 @@
 #include <ekf_derivation/generated/compute_ev_body_vel_hy.h>
 #include <ekf_derivation/generated/compute_ev_body_vel_hz.h>

-void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src)
+void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source3d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV velocity";

@@ -55,8 +56,9 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					     && ev_sample.vel.isAllFinite();

 	// correct velocity for offset relative to IMU
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
 	const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;

 	// rotate measurement into correct earth frame if required
@@ -210,6 +212,17 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common

 					stopEvVelFusion();
 				}
+
+			} else if (isHeightResetRequired()) {
+				// reset vertical velocity if height is failing
+				if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
+					const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
+					const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
+					resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2));
+
+				} else {
+					resetVerticalVelocityTo(measurement(2), measurement_var(2));
+				}
 			}

 		} else {
@@ -38,8 +38,9 @@

 #include "ekf.h"

-void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-			     const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src)
+void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+			     const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+			     estimator_aid_source1d_s &aid_src)
 {
 	static constexpr const char *AID_SRC_NAME = "EV yaw";

@@ -111,15 +111,6 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 					resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + measurement);

-					// reset vertical velocity
-					if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::VEL))) {
-						// use 1.5 as a typical ratio of vacc/hacc
-						resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise)));
-
-					} else {
-						resetVerticalVelocityToZero();
-					}
-
 					aid_src.time_last_fuse = _time_delayed_us;

 				} else if (is_fusion_failing) {
@@ -32,37 +32,122 @@
 ****************************************************************************/

 /**
- * @file gps_yaw_fusion.cpp
- * Definition of functions required to use yaw obtained from GPS dual antenna measurements.
- * Equations generated using EKF/python/ekf_derivation/main.py
- *
- * @author Paul Riseborough <p_riseborough@live.com.au>
+ * @file gnss_yaw_control.cpp
+ * Definition of functions required to use yaw obtained from GNSS dual antenna measurements.
+ * Equations generated using src/modules/ekf2/EKF/python/ekf_derivation/derivation.py
 *
 */

 #include "ekf.h"
-#include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>

 #include <mathlib/mathlib.h>
 #include <cstdlib>

-void Ekf::updateGpsYaw(const gnssSample &gps_sample)
+#include <ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h>
+
+void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample)
+{
+	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
+	    || _control_status.flags.gnss_yaw_fault) {
+
+		stopGnssYawFusion();
+		return;
+	}
+
+	const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw);
+
+	if (is_new_data_available) {
+
+		updateGnssYaw(gnss_sample);
+
+		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
+
+		// Maximum allowable time interval between GNSS yaw measurements (uSec)
+		static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000};
+		const bool data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
+
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& _gps_checks_passed
+				&& !data_intermittent;
+
+		if (_control_status.flags.gnss_yaw) {
+			if (continuing_conditions_passing) {
+
+				fuseGnssYaw(gnss_sample.yaw_offset);
+
+				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
+
+				if (is_fusion_failing) {
+					stopGnssYawFusion();
+
+					// Before takeoff, we do not want to continue to rely on the current heading
+					// if we had to stop the fusion
+					if (!_control_status.flags.in_air) {
+						ECL_INFO("clearing yaw alignment");
+						_control_status.flags.yaw_align = false;
+					}
+				}
+
+			} else {
+				// Stop GNSS yaw fusion but do not declare it faulty
+				stopGnssYawFusion();
+			}
+
+		} else {
+			if (starting_conditions_passing) {
+				// Try to activate GNSS yaw fusion
+				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
+
+				if (!_control_status.flags.in_air
+				    || !_control_status.flags.yaw_align
+				    || not_using_ne_aiding) {
+
+					// Reset before starting the fusion
+					if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) {
+
+						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
+
+						_control_status.flags.gnss_yaw = true;
+						_control_status.flags.yaw_align = true;
+					}
+
+				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
+					// Do not force a reset but wait for the consistency check to pass
+					_control_status.flags.gnss_yaw = true;
+					fuseGnssYaw(gnss_sample.yaw_offset);
+				}
+
+				if (_control_status.flags.gnss_yaw) {
+					ECL_INFO("starting GNSS yaw fusion");
+				}
+			}
+		}
+
+	} else if (_control_status.flags.gnss_yaw
+		   && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) {
+
+		// No yaw data in the message anymore. Stop until it comes back.
+		stopGnssYawFusion();
+	}
+}
+
+void Ekf::updateGnssYaw(const gnssSample &gnss_sample)
 {
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
-	const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset);
+	const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset);

-	const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f;
-	const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise));
+	const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f;
+	const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise));

 	float heading_pred;
 	float heading_innov_var;

 	VectorState H;
-	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON,
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON,
 					    &heading_pred, &heading_innov_var, &H);

 	updateAidSourceStatus(_aid_src_gnss_yaw,
-			      gps_sample.time_us,                          // sample timestamp
+			      gnss_sample.time_us,                          // sample timestamp
 			      measured_hdg,                                // observation
 			      R_YAW,                                       // observation variance
 			      wrap_pi(heading_pred - measured_hdg),        // innovation
@@ -70,7 +155,7 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
 }

-void Ekf::fuseGpsYaw(float antenna_yaw_offset)
+void Ekf::fuseGnssYaw(float antenna_yaw_offset)
 {
 	auto &aid_src = _aid_src_gnss_yaw;

@@ -99,7 +184,8 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)

 		// we reinitialise the covariance matrix and abort this fusion step
 		initialiseCovariance();
-		ECL_ERR("GPS yaw numerical error - covariance reset");
+		ECL_ERR("GNSS yaw numerical error - covariance reset");
+		stopGnssYawFusion();
 		return;
 	}

@@ -129,7 +215,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 	}
 }

-bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
+bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset)
 {
 	// define the predicted antenna array vector and rotate into earth frame
 	const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f};
@@ -140,11 +226,21 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
 		return false;
 	}

-	// GPS yaw measurement is alreday compensated for antenna offset in the driver
+	// GNSS yaw measurement is already compensated for antenna offset in the driver
 	const float measured_yaw = gnss_yaw;

-	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
+	const float yaw_variance = sq(fmaxf(_params.gnss_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance);

 	return true;
 }
+
+void Ekf::stopGnssYawFusion()
+{
+	if (_control_status.flags.gnss_yaw) {
+
+		_control_status.flags.gnss_yaw = false;
+
+		ECL_INFO("stopping GNSS yaw fusion");
+	}
+}
@@ -84,7 +84,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 			updateGnssPos(gnss_sample, _aid_src_gnss_pos);
 		}

-		updateGnssVel(gnss_sample, _aid_src_gnss_vel);
+		updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel);

 	} else if (_control_status.flags.gps) {
 		if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) {
@@ -96,7 +96,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	if (_gps_data_ready) {
 #if defined(CONFIG_EKF2_GNSS_YAW)
 		const gnssSample &gnss_sample = _gps_sample_delayed;
-		controlGpsYawFusion(gnss_sample);
+		controlGnssYawFusion(gnss_sample);
 #endif // CONFIG_EKF2_GNSS_YAW

 		controlGnssYawEstimator(_aid_src_gnss_vel);
@@ -130,17 +130,23 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 				}

 				if (do_vel_pos_reset) {
-					ECL_WARN("GPS fusion timeout, resetting velocity / position");
+					ECL_WARN("GPS fusion timeout, resetting");
+				}

-					if (gnss_vel_enabled) {
+				if (gnss_vel_enabled) {
+					if (do_vel_pos_reset) {
 						resetVelocityToGnss(_aid_src_gnss_vel);
-					}

-					if (gnss_pos_enabled) {
-						resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+					} else if (isHeightResetRequired()) {
+						// reset vertical velocity if height is failing
+						resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]);
 					}
 				}

+				if (gnss_pos_enabled && do_vel_pos_reset) {
+					resetHorizontalPositionToGnss(_aid_src_gnss_pos);
+				}
+
 			} else {
 				stopGpsFusion();
 			}
@@ -171,12 +177,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed)
 	}
 }

-void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
+void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src)
 {
 	// correct velocity for offset relative to IMU
 	const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;

-	const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body;
+	const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
+	const Vector3f vel_offset_body = angular_velocity % pos_offset_body;
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;

@@ -276,8 +283,8 @@ bool Ekf::tryYawEmergencyReset()

 #if defined(CONFIG_EKF2_GNSS_YAW)

-		if (_control_status.flags.gps_yaw) {
-			_control_status.flags.gps_yaw_fault = true;
+		if (_control_status.flags.gnss_yaw) {
+			_control_status.flags.gnss_yaw_fault = true;
 		}

 #endif // CONFIG_EKF2_GNSS_YAW
@@ -345,104 +352,6 @@ bool Ekf::shouldResetGpsFusion() const
 	return (is_reset_required || is_inflight_nav_failure);
 }

-#if defined(CONFIG_EKF2_GNSS_YAW)
-void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
-{
-	if (!(_params.gnss_ctrl & static_cast<int32_t>(GnssCtrl::YAW))
-	    || _control_status.flags.gps_yaw_fault) {
-
-		stopGpsYawFusion();
-		return;
-	}
-
-	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
-
-	if (is_new_data_available) {
-
-		updateGpsYaw(gps_sample);
-
-		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
-
-		// Maximum allowable time interval between GNSS yaw measurements (uSec)
-		static constexpr uint64_t GNSS_YAW_MAX_INTERVAL{1'500'000};
-		const bool data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
-
-		const bool starting_conditions_passing = continuing_conditions_passing
-				&& _gps_checks_passed
-				&& !data_intermittent;
-
-		if (_control_status.flags.gps_yaw) {
-			if (continuing_conditions_passing) {
-
-				fuseGpsYaw(gps_sample.yaw_offset);
-
-				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
-
-				if (is_fusion_failing) {
-					stopGpsYawFusion();
-
-					// Before takeoff, we do not want to continue to rely on the current heading
-					// if we had to stop the fusion
-					if (!_control_status.flags.in_air) {
-						ECL_INFO("clearing yaw alignment");
-						_control_status.flags.yaw_align = false;
-					}
-				}
-
-			} else {
-				// Stop GPS yaw fusion but do not declare it faulty
-				stopGpsYawFusion();
-			}
-
-		} else {
-			if (starting_conditions_passing) {
-				// Try to activate GPS yaw fusion
-				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
-
-				if (!_control_status.flags.in_air
-				    || !_control_status.flags.yaw_align
-				    || not_using_ne_aiding) {
-
-					// Reset before starting the fusion
-					if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-
-						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
-
-						_control_status.flags.gps_yaw = true;
-						_control_status.flags.yaw_align = true;
-					}
-
-				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
-					// Do not force a reset but wait for the consistency check to pass
-					_control_status.flags.gps_yaw = true;
-					fuseGpsYaw(gps_sample.yaw_offset);
-				}
-
-				if (_control_status.flags.gps_yaw) {
-					ECL_INFO("starting GPS yaw fusion");
-				}
-			}
-		}
-
-	} else if (_control_status.flags.gps_yaw
-		   && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
-
-		// No yaw data in the message anymore. Stop until it comes back.
-		stopGpsYawFusion();
-	}
-}
-
-void Ekf::stopGpsYawFusion()
-{
-	if (_control_status.flags.gps_yaw) {
-
-		_control_status.flags.gps_yaw = false;
-
-		ECL_INFO("stopping GPS yaw fusion");
-	}
-}
-#endif // CONFIG_EKF2_GNSS_YAW
-
 void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
@@ -456,7 +365,7 @@ void Ekf::stopGpsFusion()

 	stopGpsHgtFusion();
 #if defined(CONFIG_EKF2_GNSS_YAW)
-	stopGpsYawFusion();
+	stopGnssYawFusion();
 #endif // CONFIG_EKF2_GNSS_YAW

 	_yawEstimator.reset();
@@ -44,7 +44,7 @@
 // Maximum allowable time interval between magnetic field measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL{500'000};

-void Ekf::controlMagFusion()
+void Ekf::controlMagFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *AID_SRC_NAME = "mag";
 	estimator_aid_source3d_s &aid_src = _aid_src_mag;
@@ -62,7 +62,7 @@ void Ekf::controlMagFusion()

 	magSample mag_sample;

-	if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) {
+	if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) {

 		if (mag_sample.reset || (_mag_counter == 0)) {
 			// sensor or calibration has changed, reset low pass filter
@@ -155,7 +155,7 @@ void Ekf::controlMagFusion()
 						       && !_control_status.flags.mag_fault
 						       && !_control_status.flags.mag_field_disturbed
 						       && !_control_status.flags.ev_yaw
-						       && !_control_status.flags.gps_yaw;
+						       && !_control_status.flags.gnss_yaw;

 		_control_status.flags.mag_3D = common_conditions_passing
 					       && (_params.mag_fusion_type == MagFuseType::AUTO)
@@ -165,7 +165,6 @@ void Ekf::controlMagFusion()
 						&& ((_params.mag_fusion_type == MagFuseType::HEADING)
 						    || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D));

-
 		if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) {
 			ECL_INFO("starting mag 3D fusion");

@@ -185,7 +184,7 @@ void Ekf::controlMagFusion()
 				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;

 				} else {
 					// The normal sequence is to fuse the magnetometer data first before fusing
@@ -213,7 +212,7 @@ void Ekf::controlMagFusion()
 					if (no_ne_aiding_or_pre_takeoff) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
-						aid_src.time_last_fuse = _time_delayed_us;
+						aid_src.time_last_fuse = imu_sample.time_us;

 					} else {
 						ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME);
@@ -242,7 +241,7 @@ void Ekf::controlMagFusion()
 					bool reset_heading = !_control_status.flags.yaw_align;

 					resetMagStates(_mag_lpf.getState(), reset_heading);
-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;

 					if (reset_heading) {
 						_control_status.flags.yaw_align = true;
@@ -69,8 +69,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					    : _params.flow_qual_min_gnd;

 		const bool is_quality_good = (flow_sample.quality >= min_quality);
-		const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
-					       && !flow_sample.flow_rate.longerThan(_flow_max_rate);

 		bool is_tilt_good = true;

@@ -117,16 +115,21 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 					&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
 					    || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));

-		const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
+		const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance);
+
+		const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
+					       && !flow_sample.flow_rate.longerThan(_flow_max_rate)
+					       && !flow_compensated.longerThan(_flow_max_rate);

 		const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
 				&& _control_status.flags.tilt_align
-				&& is_within_max_sensor_dist;
+				&& is_within_sensor_dist;

 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& is_quality_good
 				&& is_magnitude_good
 				&& is_tilt_good
+				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching

 		// If the height is relative to the ground, terrain height cannot be observed.
@@ -141,7 +144,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)

 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
-					if (is_flow_required && is_quality_good) {
+					if (is_flow_required && is_quality_good && is_magnitude_good) {
 						resetFlowFusion();

 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
@@ -216,15 +219,29 @@ void Ekf::resetFlowFusion()
 void Ekf::resetTerrainToFlow()
 {
 	ECL_INFO("reset hagl to flow");
+
 	// TODO: use the flow data
-	_state.terrain = fmaxf(0.0f, _state.pos(2));
+	const float new_terrain = fmaxf(0.0f, _state.pos(2));
+	const float delta_terrain = new_terrain - _state.terrain;
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, 100.f);
-	_terrain_vpos_reset_counter++;

 	resetAidSourceStatusZeroInnovation(_aid_src_optical_flow);

 	_innov_check_fail_status.flags.reject_optflow_X = false;
 	_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
 }

 void Ekf::stopFlowFusion()
@@ -39,7 +39,7 @@
 #include "ekf.h"
 #include "ekf_derivation/generated/compute_hagl_innov_var.h"

-void Ekf::controlRangeHaglFusion()
+void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 {
 	static constexpr const char *HGT_SRC_NAME = "RNG";

@@ -47,7 +47,7 @@ void Ekf::controlRangeHaglFusion()

 	if (_range_buffer) {
 		// Get range data from buffer and check validity
-		rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress());
+		rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
 		_range_sensor.setDataReadiness(rng_data_ready);

 		// update range sensor angle parameters in case they have changed
@@ -55,7 +55,7 @@ void Ekf::controlRangeHaglFusion()
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);

-		_range_sensor.runChecks(_time_delayed_us, _R_to_earth);
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);

 		if (_range_sensor.isDataHealthy()) {
 			// correct the range data for position offset relative to the IMU
@@ -72,7 +72,7 @@ void Ekf::controlRangeHaglFusion()

 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
-							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
+							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
 			}

 		} else {
@@ -151,7 +151,7 @@ void Ekf::controlRangeHaglFusion()
 					_control_status.flags.rng_hgt = true;
 					stopRngTerrFusion();

-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;
 				}

 			} else {
@@ -180,10 +180,12 @@ void Ekf::controlRangeHaglFusion()
 					_information_events.flags.reset_hgt_to_rng = true;
 					resetVerticalPositionTo(-(aid_src.observation - _state.terrain));

-					// reset vertical velocity
-					resetVerticalVelocityToZero();
+					// reset vertical velocity if no valid sources available
+					if (!isVerticalVelocityAidingActive()) {
+						resetVerticalVelocityToZero();
+					}

-					aid_src.time_last_fuse = _time_delayed_us;
+					aid_src.time_last_fuse = imu_sample.time_us;

 				} else if (is_fusion_failing) {
 					// Some other height source is still working
@@ -232,21 +234,20 @@ void Ekf::controlRangeHaglFusion()

 void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
 {
-	aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
-	aid_src.innovation = getHagl() - aid_src.observation;
+	const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
+	const float measurement_variance = getRngVar();

-	const float observation_variance = getRngVar();
 	float innovation_variance;
-	sym::ComputeHaglInnovVar(P, observation_variance, &innovation_variance);
+	sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);

 	const float innov_gate = math::max(_params.range_innov_gate, 1.f);
 	updateAidSourceStatus(aid_src,
-			      _range_sensor.getSampleAddress()->time_us,                           // sample timestamp
-			      math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance), // observation
-			      observation_variance,                                                // observation variance
-			      getHagl() - aid_src.observation,                                     // innovation
-			      innovation_variance,                                                 // innovation variance
-			      math::max(_params.range_innov_gate, 1.f));                           // innovation gate
+			      _range_sensor.getSampleAddress()->time_us, // sample timestamp
+			      measurement,                               // observation
+			      measurement_variance,                      // observation variance
+			      getHagl() - measurement,                   // innovation
+			      innovation_variance,                       // innovation variance
+			      innov_gate);                               // innovation gate

 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
@@ -268,9 +269,23 @@ float Ekf::getRngVar() const

 void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src)
 {
-	_state.terrain = _state.pos(2) + aid_src.observation;
+	const float new_terrain = _state.pos(2) + aid_src.observation;
+	const float delta_terrain = new_terrain - _state.terrain;
+
+	_state.terrain = new_terrain;
 	P.uncorrelateCovarianceSetVariance<State::terrain.dof>(State::terrain.idx, aid_src.observation_variance);
-	_terrain_vpos_reset_counter++;
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_terrain;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_terrain;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+

 	aid_src.time_last_fuse = _time_delayed_us;
 }
@@ -41,7 +41,7 @@
 void Ekf::controlZeroInnovationHeadingUpdate()
 {
 	const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D
-				|| _control_status.flags.ev_yaw || _control_status.flags.gps_yaw;
+				|| _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw;

 	// fuse zero innovation at a limited rate if the yaw variance is too large
 	if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, 200'000)) {
@@ -316,7 +316,7 @@ struct parameters {

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

 	// Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value
@@ -553,12 +553,12 @@ union filter_control_status_u {
 		uint64_t fuse_aspd               : 1; ///< 19 - airspeed measurements are being fused
 		uint64_t gnd_effect              : 1; ///< 20 - protection from ground effect induced static pressure rise is active
 		uint64_t rng_stuck               : 1; ///< 21 - rng data wasn't ready and new values haven't changed enough
-		uint64_t gps_yaw                 : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended
+		uint64_t gnss_yaw                : 1; ///< 22 - yaw (not ground course) data fusion from a GPS receiver is intended
 		uint64_t mag_aligned_in_flight   : 1; ///< 23 - the in-flight mag field alignment has been completed
 		uint64_t ev_vel                  : 1; ///< 24 - local frame velocity data fusion from vision measurements is intended
 		uint64_t synthetic_mag_z         : 1; ///< 25 - using a synthesized measurement for the magnetometer Z component
 		uint64_t vehicle_at_rest         : 1; ///< 26 - the vehicle is at rest
-		uint64_t gps_yaw_fault           : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used
+		uint64_t gnss_yaw_fault          : 1; ///< 27 - the GNSS heading has been declared faulty and is no longer being used
 		uint64_t rng_fault               : 1; ///< 28 - the range finder has been declared faulty and is no longer being used
 		uint64_t inertial_dead_reckoning : 1; ///< 29 - longer fusing measurements that constrain horizontal velocity drift
 		uint64_t wind_dead_reckoning     : 1; ///< 30 - navigation reliant on wind relative measurements
@@ -596,16 +596,6 @@ union ekf_solution_status_u {
 	uint16_t value;
 };

-#if defined(CONFIG_EKF2_TERRAIN)
-union terrain_fusion_status_u {
-	struct {
-		bool range_finder : 1;  ///< 0 - fusing range finder data for terrain
-		bool flow         : 1;  ///< 1 - fusing flow data for terrain
-	} flags;
-	uint8_t value;
-};
-#endif // CONFIG_EKF2_TERRAIN
-
 // define structure used to communicate information events
 union information_event_status_u {
 	struct {
@@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control use of observations for aiding
-	controlMagFusion();
+	controlMagFusion(imu_delayed);
 #endif // CONFIG_EKF2_MAGNETOMETER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -137,12 +137,12 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)

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

 #if defined(CONFIG_EKF2_AUXVEL)
 	// Additional horizontal velocity data from an auxiliary sensor can be fused
-	controlAuxVelFusion();
+	controlAuxVelFusion(imu_delayed);
 #endif // CONFIG_EKF2_AUXVEL
 	//
 #if defined(CONFIG_EKF2_TERRAIN)
@@ -135,8 +135,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed)

 	// calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states
 	P = sym::PredictCovariance(_state.vector(), P,
-				   imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var,
-				   imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var,
+				   imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var,
+				   imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var,
 				   dt);

 	// Construct the process noise variance diagonal for those states with a stationary process model
@@ -89,8 +89,6 @@ void Ekf::reset()
 	_control_status.flags.in_air = true;
 	_control_status_prev.flags.in_air = true;

-	_ang_rate_delayed_raw.zero();
-
 	_fault_status.value = 0;
 	_innov_check_fail_status.value = 0;

@@ -264,13 +262,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f);
 	_state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f);

-	// some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication
-	// protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate
-	// due to insufficient averaging
-	if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) {
-		_ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt;
-	}
-

 	// calculate a filtered horizontal acceleration with a 1 sec time constant
 	// this are used for manoeuvre detection elsewhere
@@ -104,9 +104,6 @@ public:
 	float getTerrainVertPos() const { return _state.terrain; };
 	float getHagl() const { return _state.terrain - _state.pos(2); }

-	// get the number of times the vertical terrain position has been reset
-	uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; };
-
 	// get the terrain variance
 	float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); }

@@ -147,7 +144,7 @@ public:

 #if defined(CONFIG_EKF2_GNSS_YAW)

-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.innovation;
 		}

@@ -176,7 +173,7 @@ public:

 #if defined(CONFIG_EKF2_GNSS_YAW)

-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.innovation_variance;
 		}

@@ -205,7 +202,7 @@ public:

 #if defined(CONFIG_EKF2_GNSS_YAW)

-		if (_control_status.flags.gps_yaw) {
+		if (_control_status.flags.gnss_yaw) {
 			return _aid_src_gnss_yaw.test_ratio;
 		}

@@ -358,6 +355,13 @@ public:
 		*counter = _state_reset_status.reset_count.posD;
 	}

+	uint8_t get_hagl_reset_count() const { return _state_reset_status.reset_count.hagl; }
+	void get_hagl_reset(float *delta, uint8_t *counter) const
+	{
+		*delta = _state_reset_status.hagl_change;
+		*counter = _state_reset_status.reset_count.hagl;
+	}
+
 	// return the amount the local vertical velocity changed in the last reset and the number of reset events
 	uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; }
 	void get_velD_reset(float *delta, uint8_t *counter) const
@@ -551,6 +555,7 @@ private:
 		uint8_t posNE{0};	///< number of horizontal position reset events (allow to wrap if count exceeds 255)
 		uint8_t posD{0};	///< number of vertical position reset events (allow to wrap if count exceeds 255)
 		uint8_t quat{0};	///< number of quaternion reset events (allow to wrap if count exceeds 255)
+		uint8_t hagl{0};	///< number of height above ground level reset events (allow to wrap if count exceeds 255)
 	};

 	struct StateResets {
@@ -559,6 +564,7 @@ private:
 		Vector2f posNE_change;	///< North, East position change due to last reset (m)
 		float posD_change;	///< Down position change due to last reset (m)
 		Quatf quat_change;	///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion
+		float hagl_change;	///< Height above ground level change due to last reset (m)

 		StateResetCounts reset_count{};
 	};
@@ -566,8 +572,6 @@ private:
 	StateResets _state_reset_status{};	///< reset event monitoring structure containing velocity, position, height and yaw reset information
 	StateResetCounts _state_reset_count_prev{};

-	Vector3f _ang_rate_delayed_raw{};	///< uncorrected angular rate vector at fusion time horizon (rad/sec)
-
 	StateSample _state{};		///< state struct of the ekf running at the delayed time horizon

 	bool _filter_initialised{false};	///< true when the EKF sttes and covariances been initialised
@@ -601,8 +605,6 @@ private:

 #if defined(CONFIG_EKF2_TERRAIN)
 	// Terrain height state estimation
-	uint8_t _terrain_vpos_reset_counter{0};	///< number of times _terrain_vpos has been reset
-
 	float _last_on_ground_posD{0.0f};	///< last vertical position when the in_air status was false (m)
 #endif // CONFIG_EKF2_TERRAIN

@@ -841,7 +843,7 @@ private:

 #if defined(CONFIG_EKF2_RANGE_FINDER)
 	// range height
-	void controlRangeHaglFusion();
+	void controlRangeHaglFusion(const imuSample &imu_delayed);
 	bool isConditionalRangeAidSuitable();
 	void stopRngHgtFusion();
 	void stopRngTerrFusion();
@@ -934,16 +936,20 @@ private:

 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	// control fusion of external vision observations
-	void controlExternalVisionFusion();
+	void controlExternalVisionFusion(const imuSample &imu_sample);
 	void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset);
-	void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				   const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
-	void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src);
-	void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src);
-	void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing,
-				const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src);
+	void controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				   const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				   estimator_aid_source1d_s &aid_src);
+	void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source2d_s &aid_src);
+	void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source3d_s &aid_src);
+	void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample,
+				const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
+				estimator_aid_source1d_s &aid_src);
 	void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame);
 	Vector3f rotateVarianceToEkf(const Vector3f &measurement_var);

@@ -966,7 +972,7 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion(const imuSample &imu_delayed);
 	void stopGpsFusion();
-	void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
+	void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src);
 	void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src);
 	void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel);
 	bool tryYawEmergencyReset();
@@ -987,17 +993,17 @@ private:
 	void resetGpsDriftCheckFilters();

 # if defined(CONFIG_EKF2_GNSS_YAW)
-	void controlGpsYawFusion(const gnssSample &gps_sample);
-	void stopGpsYawFusion();
+	void controlGnssYawFusion(const gnssSample &gps_sample);
+	void stopGnssYawFusion();

 	// fuse the yaw angle obtained from a dual antenna GPS unit
-	void fuseGpsYaw(float antenna_yaw_offset);
+	void fuseGnssYaw(float antenna_yaw_offset);

 	// reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit
 	// return true if the reset was successful
-	bool resetYawToGps(float gnss_yaw, float gnss_yaw_offset);
+	bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset);

-	void updateGpsYaw(const gnssSample &gps_sample);
+	void updateGnssYaw(const gnssSample &gps_sample);

 # endif // CONFIG_EKF2_GNSS_YAW

@@ -1015,7 +1021,7 @@ private:

 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// control fusion of magnetometer observations
-	void controlMagFusion();
+	void controlMagFusion(const imuSample &imu_sample);

 	bool checkHaglYawResetReq() const;

@@ -1048,7 +1054,7 @@ private:

 #if defined(CONFIG_EKF2_AUXVEL)
 	// control fusion of auxiliary velocity observations
-	void controlAuxVelFusion();
+	void controlAuxVelFusion(const imuSample &imu_sample);
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL

@@ -1061,13 +1067,13 @@ private:
 	void checkHeightSensorRefFallback();

 #if defined(CONFIG_EKF2_BAROMETER)
-	void controlBaroHeightFusion();
+	void controlBaroHeightFusion(const imuSample &imu_sample);
 	void stopBaroHgtFusion();

 	void updateGroundEffect();

 # if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
+	float compensateBaroForDynamicPressure(const imuSample &imu_sample, float baro_alt_uncompensated) const;
 # endif // CONFIG_EKF2_BARO_COMPENSATION

 #endif // CONFIG_EKF2_BAROMETER
@@ -333,7 +333,7 @@ float Ekf::getHeadingInnovationTestRatio() const

 #if defined(CONFIG_EKF2_GNSS_YAW)

-	if (_control_status.flags.gps_yaw) {
+	if (_control_status.flags.gnss_yaw) {
 		test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered));
 	}

@@ -94,7 +94,17 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)

 		_imu_updated = true;

-		_imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset());
+		imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset();
+
+		// as a precaution constrain the integration delta time to prevent numerical problems
+		const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f;
+		const float imu_min_dt = 0.5f * filter_update_period_s;
+		const float imu_max_dt = 2.0f * filter_update_period_s;
+
+		imu_downsampled.delta_ang_dt = math::constrain(imu_downsampled.delta_ang_dt, imu_min_dt, imu_max_dt);
+		imu_downsampled.delta_vel_dt = math::constrain(imu_downsampled.delta_vel_dt, imu_min_dt, imu_max_dt);
+
+		_imu_buffer.push(imu_downsampled);

 		// get the oldest data from the buffer
 		_time_delayed_us = _imu_buffer.get_oldest().time_us;
@@ -183,7 +193,7 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 #if defined(CONFIG_EKF2_GNSS_YAW)

 		if (PX4_ISFINITE(gnss_sample.yaw)) {
-			_time_last_gps_yaw_buffer_push = _time_latest_us;
+			_time_last_gnss_yaw_buffer_push = _time_latest_us;
 		}

 #endif // CONFIG_EKF2_GNSS_YAW
@@ -397,7 +397,7 @@ protected:

 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// innovation consistency check monitoring ratios
-	uint64_t _time_last_gps_yaw_buffer_push{0};
+	uint64_t _time_last_gnss_yaw_buffer_push{0};
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS

@@ -45,7 +45,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #if defined(CONFIG_EKF2_BAROMETER)
 	updateGroundEffect();

-	controlBaroHeightFusion();
+	controlBaroHeightFusion(imu_delayed);
 #endif // CONFIG_EKF2_BAROMETER

 #if defined(CONFIG_EKF2_GNSS)
@@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 #endif // CONFIG_EKF2_GNSS

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	controlRangeHaglFusion();
+	controlRangeHaglFusion(imu_delayed);
 #endif // CONFIG_EKF2_RANGE_FINDER

 	checkHeightSensorRefFallback();
@@ -167,7 +167,18 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v

 #if defined(CONFIG_EKF2_TERRAIN)
 	_state.terrain += delta_z;
-#endif
+
+	// record the state change
+	if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) {
+		_state_reset_status.hagl_change = delta_z;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.hagl_change += delta_z;
+	}
+
+	_state_reset_status.reset_count.hagl++;
+#endif // CONFIG_EKF2_TERRAIN

 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
@@ -1,6 +1,6 @@
 matplotlib==3.5.1
 numpy==1.22.2
 pyulog==0.9.0
-quaternion==3.5.2.post4
+numpy-quaternion==2023.0.4
 scipy==1.8.0
 sympy==1.10.1
@@ -1191,6 +1191,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
 			global_pos.terrain_alt_valid = true;
 		}

+		float delta_hagl = 0.f;
+		_ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter);
+		global_pos.delta_terrain = -delta_z;
 #endif // CONFIG_EKF2_TERRAIN

 		global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
@@ -1619,9 +1622,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)

 #if defined(CONFIG_EKF2_TERRAIN)
 	// Distance to bottom surface (ground) in meters, must be positive
+	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
 	lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f);
 	lpos.dist_bottom_var = _ekf.getTerrainVariance();
-	lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid();
+	_ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter);

 	lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE;

@@ -1885,36 +1889,36 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		estimator_status_flags_s status_flags{};
 		status_flags.timestamp_sample = _ekf.time_delayed_us();

-		status_flags.control_status_changes   = _filter_control_status_changes;
-		status_flags.cs_tilt_align            = _ekf.control_status_flags().tilt_align;
-		status_flags.cs_yaw_align             = _ekf.control_status_flags().yaw_align;
-		status_flags.cs_gps                   = _ekf.control_status_flags().gps;
-		status_flags.cs_opt_flow              = _ekf.control_status_flags().opt_flow;
-		status_flags.cs_mag_hdg               = _ekf.control_status_flags().mag_hdg;
-		status_flags.cs_mag_3d                = _ekf.control_status_flags().mag_3D;
-		status_flags.cs_mag_dec               = _ekf.control_status_flags().mag_dec;
-		status_flags.cs_in_air                = _ekf.control_status_flags().in_air;
-		status_flags.cs_wind                  = _ekf.control_status_flags().wind;
-		status_flags.cs_baro_hgt              = _ekf.control_status_flags().baro_hgt;
-		status_flags.cs_rng_hgt               = _ekf.control_status_flags().rng_hgt;
-		status_flags.cs_gps_hgt               = _ekf.control_status_flags().gps_hgt;
-		status_flags.cs_ev_pos                = _ekf.control_status_flags().ev_pos;
-		status_flags.cs_ev_yaw                = _ekf.control_status_flags().ev_yaw;
-		status_flags.cs_ev_hgt                = _ekf.control_status_flags().ev_hgt;
-		status_flags.cs_fuse_beta             = _ekf.control_status_flags().fuse_beta;
-		status_flags.cs_mag_field_disturbed   = _ekf.control_status_flags().mag_field_disturbed;
-		status_flags.cs_fixed_wing            = _ekf.control_status_flags().fixed_wing;
-		status_flags.cs_mag_fault             = _ekf.control_status_flags().mag_fault;
-		status_flags.cs_fuse_aspd             = _ekf.control_status_flags().fuse_aspd;
-		status_flags.cs_gnd_effect            = _ekf.control_status_flags().gnd_effect;
-		status_flags.cs_rng_stuck             = _ekf.control_status_flags().rng_stuck;
-		status_flags.cs_gps_yaw               = _ekf.control_status_flags().gps_yaw;
-		status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight;
-		status_flags.cs_ev_vel                = _ekf.control_status_flags().ev_vel;
-		status_flags.cs_synthetic_mag_z       = _ekf.control_status_flags().synthetic_mag_z;
-		status_flags.cs_vehicle_at_rest       = _ekf.control_status_flags().vehicle_at_rest;
-		status_flags.cs_gps_yaw_fault         = _ekf.control_status_flags().gps_yaw_fault;
-		status_flags.cs_rng_fault             = _ekf.control_status_flags().rng_fault;
+		status_flags.control_status_changes     = _filter_control_status_changes;
+		status_flags.cs_tilt_align              = _ekf.control_status_flags().tilt_align;
+		status_flags.cs_yaw_align               = _ekf.control_status_flags().yaw_align;
+		status_flags.cs_gps                     = _ekf.control_status_flags().gps;
+		status_flags.cs_opt_flow                = _ekf.control_status_flags().opt_flow;
+		status_flags.cs_mag_hdg                 = _ekf.control_status_flags().mag_hdg;
+		status_flags.cs_mag_3d                  = _ekf.control_status_flags().mag_3D;
+		status_flags.cs_mag_dec                 = _ekf.control_status_flags().mag_dec;
+		status_flags.cs_in_air                  = _ekf.control_status_flags().in_air;
+		status_flags.cs_wind                    = _ekf.control_status_flags().wind;
+		status_flags.cs_baro_hgt                = _ekf.control_status_flags().baro_hgt;
+		status_flags.cs_rng_hgt                 = _ekf.control_status_flags().rng_hgt;
+		status_flags.cs_gps_hgt                 = _ekf.control_status_flags().gps_hgt;
+		status_flags.cs_ev_pos                  = _ekf.control_status_flags().ev_pos;
+		status_flags.cs_ev_yaw                  = _ekf.control_status_flags().ev_yaw;
+		status_flags.cs_ev_hgt                  = _ekf.control_status_flags().ev_hgt;
+		status_flags.cs_fuse_beta               = _ekf.control_status_flags().fuse_beta;
+		status_flags.cs_mag_field_disturbed     = _ekf.control_status_flags().mag_field_disturbed;
+		status_flags.cs_fixed_wing              = _ekf.control_status_flags().fixed_wing;
+		status_flags.cs_mag_fault               = _ekf.control_status_flags().mag_fault;
+		status_flags.cs_fuse_aspd               = _ekf.control_status_flags().fuse_aspd;
+		status_flags.cs_gnd_effect              = _ekf.control_status_flags().gnd_effect;
+		status_flags.cs_rng_stuck               = _ekf.control_status_flags().rng_stuck;
+		status_flags.cs_gnss_yaw                = _ekf.control_status_flags().gnss_yaw;
+		status_flags.cs_mag_aligned_in_flight   = _ekf.control_status_flags().mag_aligned_in_flight;
+		status_flags.cs_ev_vel                  = _ekf.control_status_flags().ev_vel;
+		status_flags.cs_synthetic_mag_z         = _ekf.control_status_flags().synthetic_mag_z;
+		status_flags.cs_vehicle_at_rest         = _ekf.control_status_flags().vehicle_at_rest;
+		status_flags.cs_gnss_yaw_fault          = _ekf.control_status_flags().gnss_yaw_fault;
+		status_flags.cs_rng_fault               = _ekf.control_status_flags().rng_fault;
 		status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning;
 		status_flags.cs_wind_dead_reckoning     = _ekf.control_status_flags().wind_dead_reckoning;
 		status_flags.cs_rng_kin_consistent      = _ekf.control_status_flags().rng_kin_consistent;
@@ -1925,8 +1929,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_ev_yaw_fault            = _ekf.control_status_flags().ev_yaw_fault;
 		status_flags.cs_mag_heading_consistent  = _ekf.control_status_flags().mag_heading_consistent;
 		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
-		status_flags.cs_rng_terrain    = _ekf.control_status_flags().rng_terrain;
-		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
+		status_flags.cs_rng_terrain             = _ekf.control_status_flags().rng_terrain;
+		status_flags.cs_opt_flow_terrain        = _ekf.control_status_flags().opt_flow_terrain;

 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -479,18 +479,32 @@ void EKF2Selector::PublishVehicleLocalPosition()
 				_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
 			}

+			// HAGL (dist_bottom) reset
+			if (!instance_change
+			    && (local_position.dist_bottom_reset_counter == _local_position_last.dist_bottom_reset_counter + 1)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.delta_dist_bottom;
+
+			} else if (instance_change
+				   || (local_position.dist_bottom_reset_counter != _local_position_last.dist_bottom_reset_counter)) {
+				++_hagl_reset_counter;
+				_delta_hagl_reset = local_position.dist_bottom - _local_position_last.dist_bottom;
+			}
+
 		} else {
 			_xy_reset_counter = local_position.xy_reset_counter;
 			_z_reset_counter = local_position.z_reset_counter;
 			_vxy_reset_counter = local_position.vxy_reset_counter;
 			_vz_reset_counter = local_position.vz_reset_counter;
 			_heading_reset_counter = local_position.heading_reset_counter;
+			_hagl_reset_counter = local_position.dist_bottom_reset_counter;

 			_delta_xy_reset = Vector2f{local_position.delta_xy};
 			_delta_z_reset = local_position.delta_z;
 			_delta_vxy_reset = Vector2f{local_position.delta_vxy};
 			_delta_vz_reset = local_position.delta_vz;
 			_delta_heading_reset = local_position.delta_heading;
+			_delta_hagl_reset = local_position.delta_dist_bottom;
 		}

 		bool publish = true;
@@ -513,6 +527,7 @@ void EKF2Selector::PublishVehicleLocalPosition()
 			local_position.vxy_reset_counter = _vxy_reset_counter;
 			local_position.vz_reset_counter = _vz_reset_counter;
 			local_position.heading_reset_counter = _heading_reset_counter;
+			local_position.dist_bottom_reset_counter = _hagl_reset_counter;

 			_delta_xy_reset.copyTo(local_position.delta_xy);
 			local_position.delta_z = _delta_z_reset;
@@ -612,6 +627,16 @@ void EKF2Selector::PublishVehicleGlobalPosition()
 				_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
 			}

+			// terrain reset
+			if (!instance_change && (global_position.terrain_reset_counter == _global_position_last.terrain_reset_counter + 1)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain;
+
+			} else if (instance_change || (global_position.terrain_reset_counter != _global_position_last.terrain_reset_counter)) {
+				++_terrain_reset_counter;
+				_delta_terrain_reset = global_position.delta_terrain - _global_position_last.delta_terrain;
+			}
+
 		} else {
 			_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
 			_alt_reset_counter = global_position.alt_reset_counter;
@@ -202,11 +202,14 @@ private:
 	matrix::Vector2f _delta_vxy_reset{};
 	float _delta_vz_reset{0.f};
 	float _delta_heading_reset{0};
+	float _delta_hagl_reset{0.f};
+
 	uint8_t _xy_reset_counter{0};
 	uint8_t _z_reset_counter{0};
 	uint8_t _vxy_reset_counter{0};
 	uint8_t _vz_reset_counter{0};
 	uint8_t _heading_reset_counter{0};
+	uint8_t _hagl_reset_counter{0};

 	// vehicle_odometry
 	vehicle_odometry_s _odometry_last{};
@@ -217,8 +220,11 @@ private:
 	double _delta_lat_reset{0};
 	double _delta_lon_reset{0};
 	float _delta_alt_reset{0.f};
+	float _delta_terrain_reset{0.f};
+
 	uint8_t _lat_lon_reset_counter{0};
 	uint8_t _alt_reset_counter{0};
+	uint8_t _terrain_reset_counter{0};

 	// wind estimate
 	wind_s _wind_last{};
@@ -45,7 +45,7 @@ px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_s
 px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
-px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
+px4_add_unit_gtest(SRC test_EKF_gnss_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
 px4_add_unit_gtest(SRC test_EKF_gnss_yaw_generated.cpp LINKLIBS ecl_EKF ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper)
 px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim)
@@ -132,7 +132,7 @@ void EkfWrapper::disableGpsHeadingFusion()

 bool EkfWrapper::isIntendingGpsHeadingFusion() const
 {
-	return _ekf->control_status_flags().gps_yaw;
+	return _ekf->control_status_flags().gnss_yaw;
 }

 void EkfWrapper::enableFlowFusion()
@@ -123,7 +123,7 @@ TEST_F(EkfBasicsTest, initialControlMode)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);
@@ -178,7 +178,7 @@ TEST_F(EkfBasicsTest, gpsFusion)
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
-	EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
+	EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
 	EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);
@@ -207,9 +207,9 @@ TEST_F(EkfTerrainTest, testHeightReset)
 	const float new_baro_height = _sensor_simulator._baro.getData() + 50.f;
 	_sensor_simulator._baro.setData(new_baro_height);
 	_sensor_simulator.stopGps(); // prevent from switching to GNSS height
-	_sensor_simulator.runSeconds(6);
+	_sensor_simulator.runSeconds(10);

-	// THEN: a height reset occured and the estimated distance to the ground remains constant
+	// THEN: a height reset occurred and the estimated distance to the ground remains constant
 	reset_logging_checker.capturePostResetState();
 	EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
 	EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f);
@@ -632,29 +632,27 @@ bool FlightTaskAuto::_evaluateGlobalReference()
 State FlightTaskAuto::_getCurrentState()
 {
 	// Calculate the vehicle current state based on the Navigator triplets and the current position.
-	const Vector2f u_prev_to_target_xy = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero();
-	const Vector2f pos_to_target_xy = Vector2f(_triplet_target - _position);
-	const Vector2f prev_to_pos_xy = Vector2f(_position - _triplet_prev_wp);
+	const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero();
+	const Vector3f prev_to_pos = _position - _triplet_prev_wp;
+	const Vector3f pos_to_target = _triplet_target - _position;
 	// Calculate the closest point to the vehicle position on the line prev_wp - target
-	const Vector2f closest_pt_xy = Vector2f(_triplet_prev_wp) + u_prev_to_target_xy * (prev_to_pos_xy *
-				       u_prev_to_target_xy);
-	_closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2));
+	_closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target);

 	State return_state = State::none;

-	if (u_prev_to_target_xy.length() < FLT_EPSILON) {
+	if (!u_prev_to_target.longerThan(FLT_EPSILON)) {
 		// Previous and target are the same point, so we better don't try to do any special line following
 		return_state = State::none;

-	} else if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) {
+	} else if (u_prev_to_target * pos_to_target < 0.0f) {
 		// Target is behind
 		return_state = State::target_behind;

-	} else if (u_prev_to_target_xy * prev_to_pos_xy < 0.0f && prev_to_pos_xy.longerThan(_target_acceptance_radius)) {
+	} else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) {
 		// Previous is in front
 		return_state = State::previous_infront;

-	} else if (Vector2f(_position - _closest_pt).longerThan(_target_acceptance_radius)) {
+	} else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) {
 		// Vehicle too far from the track
 		return_state = State::offtrack;

@@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters()
 		_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
 	}

+	if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) {
+		_ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom);
+		_reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter;
+	}
+
 	if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) {
 		_ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz);
 		_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
@@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition()
 		// distance to bottom
 		if (_sub_vehicle_local_position.get().dist_bottom_valid
 		    && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) {
-			_dist_to_bottom =  _sub_vehicle_local_position.get().dist_bottom;
+			_dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom;
 		}

 		// global frame reference coordinates to enable conversions
@@ -61,6 +61,7 @@ struct ekf_reset_counters_s {
 	uint8_t z;
 	uint8_t vz;
 	uint8_t heading;
+	uint8_t hagl;
 };

 class FlightTask : public ModuleParams
@@ -191,6 +192,7 @@ protected:
 	virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {};
 	virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {};
 	virtual void _ekfResetHandlerPositionZ(float delta_z) {};
+	virtual void _ekfResetHandlerHagl(float delta_hagl) {};
 	virtual void _ekfResetHandlerVelocityZ(float delta_vz) {};
 	virtual void _ekfResetHandlerHeading(float delta_psi) {};
				`@@ -0,0 +1 @@`
				`# Same as default, only defconfig is different`