diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 7e95b1fff0..a90d51804e 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -11,6 +11,8 @@ on: jobs: build: runs-on: ubuntu-latest + env: + ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index 812f63fec4..020caaa2d0 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -11,6 +11,8 @@ on: jobs: build: runs-on: ubuntu-latest + env: + ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann new file mode 100644 index 0000000000..91c44baff3 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9277ad3039..33d6d857e8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -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 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 881558c8c2..312cd002ff 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 881558c8c274d0d9f21970de24333122e050b561 +Subproject commit 312cd002ff9602644efef58eef93e447c10dcbe8 diff --git a/boards/cubepilot/cubeorange/console.px4board b/boards/cubepilot/cubeorange/console.px4board new file mode 100644 index 0000000000..89afc484a1 --- /dev/null +++ b/boards/cubepilot/cubeorange/console.px4board @@ -0,0 +1 @@ +# Same as default, only defconfig is different diff --git a/boards/cubepilot/cubeorange/nuttx-config/console/defconfig b/boards/cubepilot/cubeorange/nuttx-config/console/defconfig new file mode 100644 index 0000000000..e8e0422f43 --- /dev/null +++ b/boards/cubepilot/cubeorange/nuttx-config/console/defconfig @@ -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 diff --git a/boards/cubepilot/cubeorangeplus/console.px4board b/boards/cubepilot/cubeorangeplus/console.px4board new file mode 100644 index 0000000000..89afc484a1 --- /dev/null +++ b/boards/cubepilot/cubeorangeplus/console.px4board @@ -0,0 +1 @@ +# Same as default, only defconfig is different diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig new file mode 100644 index 0000000000..bb6a7cee41 --- /dev/null +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig @@ -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 diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 2d6a7c39a1..23e6e15fd4 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -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 diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index c7d9ee7812..e37155fe25 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/VehicleGlobalPosition.msg @@ -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) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 25551ab4fb..0e74ac0f4b 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -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 diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 609d2aebe6..2e3add770e 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -47,6 +47,7 @@ set(SRCS_COMMON Subscription.cpp Subscription.hpp SubscriptionCallback.hpp + SubscriptionInterval.cpp SubscriptionInterval.hpp SubscriptionMultiArray.hpp uORB.cpp diff --git a/platforms/common/uORB/SubscriptionInterval.cpp b/platforms/common/uORB/SubscriptionInterval.cpp new file mode 100644 index 0000000000..8f8af333f2 --- /dev/null +++ b/platforms/common/uORB/SubscriptionInterval.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 diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 07572f72f2..fd6ccdfb61 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -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(); } diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index ba90108c38..5bb31ec345 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -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 diff --git a/src/drivers/barometer/Kconfig b/src/drivers/barometer/Kconfig index e13c24c272..da9c2dd714 100644 --- a/src/drivers/barometer/Kconfig +++ b/src/drivers/barometer/Kconfig @@ -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 diff --git a/src/drivers/barometer/bmp581/CMakeLists.txt b/src/drivers/barometer/bmp581/CMakeLists.txt new file mode 100644 index 0000000000..c5de8e20b3 --- /dev/null +++ b/src/drivers/barometer/bmp581/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/barometer/bmp581/Kconfig b/src/drivers/barometer/bmp581/Kconfig new file mode 100644 index 0000000000..49f99b08b2 --- /dev/null +++ b/src/drivers/barometer/bmp581/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_BMP581 + bool "bmp581" + default n + ---help--- + Enable support for bmp581 diff --git a/src/drivers/barometer/bmp581/bmp581.cpp b/src/drivers/barometer/bmp581/bmp581.cpp new file mode 100644 index 0000000000..a4a085397c --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581.cpp @@ -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 + +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; +} diff --git a/src/drivers/barometer/bmp581/bmp581.h b/src/drivers/barometer/bmp581/bmp581.h new file mode 100644 index 0000000000..56d87fa3f1 --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581.h @@ -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 +#include +#include +#include +#include +#include +#include +#include + +#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 +{ +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_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; diff --git a/src/drivers/barometer/bmp581/bmp581_i2c.cpp b/src/drivers/barometer/bmp581/bmp581_i2c.cpp new file mode 100644 index 0000000000..3e4a0add5f --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581_i2c.cpp @@ -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 + +#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); +} + diff --git a/src/drivers/barometer/bmp581/bmp581_main.cpp b/src/drivers/barometer/bmp581/bmp581_main.cpp new file mode 100644 index 0000000000..d92ba376ac --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581_main.cpp @@ -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 +#include +#include + +#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; + + + + +} diff --git a/src/drivers/barometer/bmp581/bmp581_spi.cpp b/src/drivers/barometer/bmp581/bmp581_spi.cpp new file mode 100644 index 0000000000..cda214e623 --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581_spi.cpp @@ -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 + +#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); +} diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 604ab4946b..378c92b673 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 0c26470683..4a6d3593e8 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 7441e76f08..d1bc06a742 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -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 diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 3f5d9d142f..c4d0d11625 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -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) diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 1134d1942b..f404817ef4 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -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) diff --git a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp index 14d333f364..6531c5c65a 100644 --- a/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -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 diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index 2ad273427d..2b3271fbcb 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -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); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 64591addc9..c1ed9b7cdf 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index a69906eef4..e7b6f13f11 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -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(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) { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 53ee21e09b..590a289da3 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -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); diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index 62781e5081..b94f24f45e 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -41,8 +41,9 @@ #include #include -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 { diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index a285d3e455..ad15c4f6bb 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -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"; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index 49bdf960e3..14d519bc76 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -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(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) { diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp similarity index 56% rename from src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp index d66fbac7c6..aea098ed41 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -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 + * @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 #include #include -void Ekf::updateGpsYaw(const gnssSample &gps_sample) +#include + +void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) +{ + if (!(_params.gnss_ctrl & static_cast(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"); + } +} diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 2abfade116..6d2a5220bb 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -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(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(); diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 29ea8ef242..70168d596c 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index 2d6be39fec..d58f2dba7f 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -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.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() diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 469ac7847c..29ba94ee5d 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -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.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; } diff --git a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp index 162ce2f1a3..853300cb36 100644 --- a/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp @@ -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)) { diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index ce3bbb6e5a..2039a7cca2 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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 { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 5215570d14..b8aca027e7 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -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) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 8918c872de..bad586e8ba 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -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 diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 4479dc37b8..1b439c8879 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -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 diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5cb8cfcd43..f276bc77a3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 98e72f6e2c..e495159434 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -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)); } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 0ea4a66600..0223ada370 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -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 diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 4477f17b51..75e6526830 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -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 diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 854bf7058f..1e1e573697 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -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(); diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 6cc503de73..cf8e7de04b 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -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; diff --git a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt index 10d3fbc549..42c9528231 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt +++ b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt @@ -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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index cee124db0c..14721c8a47 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1191,6 +1191,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) 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 ×tamp) #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 ×tamp) 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 ×tamp) 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; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 99fb863eed..f5f67e2cde 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -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; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 6507233bd3..535af97e6d 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -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{}; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 424a2a208a..800b568d96 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -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) diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 26a1dd73c6..9fd62885e3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -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() diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index c7a8ffa836..e954433442 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -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); diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp similarity index 100% rename from src/modules/ekf2/test/test_EKF_gps_yaw.cpp rename to src/modules/ekf2/test/test_EKF_gnss_yaw.cpp diff --git a/src/modules/ekf2/test/test_EKF_terrain.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp index c3c2ad6714..5549eb8030 100644 --- a/src/modules/ekf2/test/test_EKF_terrain.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -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); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 606776aea8..dad05e3c31 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -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; diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 063b78dcf6..55b587d270 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -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 diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 7e938bb0a9..6ee861f3cd 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -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) {}; diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index da3223ff93..d65952bacc 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit da3223ff9380bfe8e496fab8df2cbb06d5f8d5c3 +Subproject commit d65952bacc02c4a5a1ed8249be73e6a66fa800ab