From c391509c23de4ba701fb135304865322279beaef Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Wed, 10 Jul 2024 14:15:45 +0200 Subject: [PATCH 01/22] ackermann: add SITL airframe --- .../airframes/4012_gz_rover_ackermann | 47 +++++++++++++++++++ .../init.d-posix/airframes/CMakeLists.txt | 1 + 2 files changed, 48 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann 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 From e0ea91fc2764a2f67e22412501cdf46602eba39c Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Thu, 11 Jul 2024 00:39:09 +0000 Subject: [PATCH 02/22] Update submodule gz to latest Thu Jul 11 00:39:09 UTC 2024 - gz in PX4/Firmware (2c3401dc8367a860cbbabc5e77898ff206f554aa): https://github.com/PX4/PX4-gazebo-models/commit/881558c8c274d0d9f21970de24333122e050b561 - gz current upstream: https://github.com/PX4/PX4-gazebo-models/commit/312cd002ff9602644efef58eef93e447c10dcbe8 - Changes: https://github.com/PX4/PX4-gazebo-models/compare/881558c8c274d0d9f21970de24333122e050b561...312cd002ff9602644efef58eef93e447c10dcbe8 312cd00 2024-07-08 chfriedrich98 - Add rover ackermann model (#46) --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 57e303b11b490e378710ec838dd0e2683fc21314 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 9 Jul 2024 14:38:06 +0200 Subject: [PATCH 03/22] bugfix for failing actions --- .github/workflows/mavros_mission_tests.yml | 2 ++ .github/workflows/mavros_offboard_tests.yml | 2 ++ 2 files changed, 4 insertions(+) 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: From 20137bea40722e202a0b81ffc893567711e6ebcc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 9 Jul 2024 11:03:37 +1200 Subject: [PATCH 04/22] boards: add console build for Cube Orange(+) This adds a build variant which enables the serial console, and therefore disables the ADSB receiver. --- boards/cubepilot/cubeorange/console.px4board | 1 + .../cubeorange/nuttx-config/console/defconfig | 257 +++++++++++++++++ .../cubepilot/cubeorangeplus/console.px4board | 1 + .../nuttx-config/console/defconfig | 259 ++++++++++++++++++ 4 files changed, 518 insertions(+) create mode 100644 boards/cubepilot/cubeorange/console.px4board create mode 100644 boards/cubepilot/cubeorange/nuttx-config/console/defconfig create mode 100644 boards/cubepilot/cubeorangeplus/console.px4board create mode 100644 boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig 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 From f93dc617703d0224540403d46c3e4cda7d052a83 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 12:23:00 -0400 Subject: [PATCH 05/22] ekf2: use bias corrected angular velocity - avoid unnecessarily storing _ang_rate_delayed_raw --- .../barometer/baro_height_control.cpp | 9 +++--- .../external_vision/ev_control.cpp | 11 +++---- .../external_vision/ev_height_control.cpp | 8 +++-- .../external_vision/ev_pos_control.cpp | 5 ++-- .../external_vision/ev_vel_control.cpp | 8 +++-- .../external_vision/ev_yaw_control.cpp | 5 ++-- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 7 +++-- src/modules/ekf2/EKF/control.cpp | 2 +- src/modules/ekf2/EKF/ekf.cpp | 9 ------ src/modules/ekf2/EKF/ekf.h | 30 ++++++++++--------- src/modules/ekf2/EKF/height_control.cpp | 2 +- 11 files changed, 49 insertions(+), 47 deletions(-) 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 80c51e0979..9153856009 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 @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlBaroHeightFusion() +void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "baro"; @@ -52,7 +52,7 @@ void Ekf::controlBaroHeightFusion() if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_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 @@ -195,7 +195,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 @@ -203,7 +203,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 78cc2bd30a..cac1db3f3d 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 @@ -38,7 +38,7 @@ #include "ekf.h" -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); @@ -62,10 +62,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..0684536ae5 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"; @@ -152,7 +153,8 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com 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 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; switch (ev_sample.vel_frame) { 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..a5ba562cad 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 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/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index d5bca4f46a..83f7ef624d 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -86,7 +86,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)) { @@ -173,12 +173,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; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 5215570d14..37d13a32e3 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -137,7 +137,7 @@ 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) 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 1b20c433e2..60c869c575 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -566,8 +566,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 @@ -936,16 +934,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); @@ -968,7 +970,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(); @@ -1063,13 +1065,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/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 5ad5d7db34..56a667840a 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) From ac77049c47b22087bc25d5dcc9efa3a9d1f4ae80 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 27 Jun 2024 12:33:17 -0400 Subject: [PATCH 06/22] ekf2: directly use IMU sample to find corresponding aid source sample - I think this helps make it clear we're using a sensor sample corresponding with a particular IMU sample --- .../ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp | 4 ++-- .../aid_sources/barometer/baro_height_control.cpp | 6 +++--- .../EKF/aid_sources/external_vision/ev_control.cpp | 2 +- .../EKF/aid_sources/magnetometer/mag_control.cpp | 10 +++++----- .../range_finder/range_height_control.cpp | 12 ++++++------ src/modules/ekf2/EKF/control.cpp | 4 ++-- src/modules/ekf2/EKF/ekf.h | 6 +++--- src/modules/ekf2/EKF/height_control.cpp | 2 +- 8 files changed, 23 insertions(+), 23 deletions(-) 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 9153856009..c6c6f220de 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 @@ -49,7 +49,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) 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(imu_sample, baro_sample.hgt); @@ -137,7 +137,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) // reset vertical velocity 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 @@ -166,7 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) 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; } 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 cac1db3f3d..68022677ba 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 @@ -46,7 +46,7 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) // 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); 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 979807f116..2dab0da57e 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -41,7 +41,7 @@ #include -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; @@ -59,7 +59,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 @@ -185,7 +185,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 +213,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 +242,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/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 1b7db97e30..94a44883ee 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 { @@ -183,7 +183,7 @@ void Ekf::controlRangeHaglFusion() // reset vertical velocity 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 diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 37d13a32e3..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) @@ -142,7 +142,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #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/ekf.h b/src/modules/ekf2/EKF/ekf.h index 60c869c575..85ea4a7625 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -841,7 +841,7 @@ private: #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - void controlRangeHaglFusion(); + void controlRangeHaglFusion(const imuSample &imu_delayed); bool isConditionalRangeAidSuitable(); void stopRngHgtFusion(); void stopRngTerrFusion(); @@ -1019,7 +1019,7 @@ private: #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations - void controlMagFusion(); + void controlMagFusion(const imuSample &imu_sample); bool checkHaglYawResetReq() const; @@ -1052,7 +1052,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 diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 56a667840a..4db232ca05 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -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(); From 9124a7b47160bc9d4a843047da7aacc66107b6b6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 8 Jul 2024 11:25:21 -0400 Subject: [PATCH 07/22] ekf2: add IMU delta_ang_dt/delta_vel_dt safety constrain before pushing into buffer --- src/modules/ekf2/EKF/covariance.cpp | 4 ++-- src/modules/ekf2/EKF/estimator_interface.cpp | 12 +++++++++++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 8108d26c13..58e83b3bd7 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -134,8 +134,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/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index bce6ec8863..d4a579f5b8 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; From 33be5d8356e57ee98ae70e0725cf8e3b4a6e2238 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Thu, 11 Jul 2024 14:54:22 +0200 Subject: [PATCH 08/22] Survey - fix of survey tracking problem on steep slopes (#23371) * initial working * incoperated review --- .../tasks/Auto/FlightTaskAuto.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) 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; From e2b31454a3843cc0d3a701986bc64c856c5ce818 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 12 Jul 2024 15:17:58 +0200 Subject: [PATCH 09/22] SubscriptionInterval: move updated, update, copy function to a cpp file Saves 17.3 kilobytes of flash :open_mouth: --- platforms/common/uORB/CMakeLists.txt | 1 + .../common/uORB/SubscriptionInterval.cpp | 77 +++++++++++++++++++ .../common/uORB/SubscriptionInterval.hpp | 38 +-------- 3 files changed, 81 insertions(+), 35 deletions(-) create mode 100644 platforms/common/uORB/SubscriptionInterval.cpp 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(); } From b1b0032b8d7ea7ef7a0b7edfe5a9ed787acb3f9c Mon Sep 17 00:00:00 2001 From: Liu1 <108582329+cuav-liu1@users.noreply.github.com> Date: Mon, 15 Jul 2024 06:08:20 +0800 Subject: [PATCH 10/22] BMP581: Add Bosch BMP581 barometer (#23064) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * BMP581: Add Bosch BMP581 barometer * Copyright：fix copyright header year * style: not use pointers and Bool returns, Check the failed condition return * delay: Replace usleep() with ScheduleDelayed() * definitions: Delete unused definitions * comment: Delet redundant comments * constants: Change to uppercase * BMP581: run make format --- src/drivers/barometer/CMakeLists.txt | 3 +- src/drivers/barometer/Kconfig | 1 + src/drivers/barometer/bmp581/CMakeLists.txt | 44 ++ src/drivers/barometer/bmp581/Kconfig | 5 + src/drivers/barometer/bmp581/bmp581.cpp | 727 +++++++++++++++++++ src/drivers/barometer/bmp581/bmp581.h | 320 ++++++++ src/drivers/barometer/bmp581/bmp581_i2c.cpp | 95 +++ src/drivers/barometer/bmp581/bmp581_main.cpp | 127 ++++ src/drivers/barometer/bmp581/bmp581_spi.cpp | 102 +++ src/drivers/drv_sensor.h | 3 +- 10 files changed, 1425 insertions(+), 2 deletions(-) create mode 100644 src/drivers/barometer/bmp581/CMakeLists.txt create mode 100644 src/drivers/barometer/bmp581/Kconfig create mode 100644 src/drivers/barometer/bmp581/bmp581.cpp create mode 100644 src/drivers/barometer/bmp581/bmp581.h create mode 100644 src/drivers/barometer/bmp581/bmp581_i2c.cpp create mode 100644 src/drivers/barometer/bmp581/bmp581_main.cpp create mode 100644 src/drivers/barometer/bmp581/bmp581_spi.cpp 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 From 59b96f4968690ff29fb3ad76462d649b38caa49f Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Mon, 15 Jul 2024 10:42:43 +0200 Subject: [PATCH 11/22] tecs: fast descend: do not shut down throttle while still climbing (#23397) --- src/lib/tecs/TECS.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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); From dc5f8118b06c9e88ff167cbb182caa881cf6ea84 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jul 2024 13:09:20 -0400 Subject: [PATCH 12/22] ekf2: range finder cleanup duplicate logic --- .../range_finder/range_height_control.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) 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 94a44883ee..ce8377e06b 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 @@ -232,21 +232,20 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) 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 From be551097e00f6d0d5a75c57e141ba5dafdd2aa53 Mon Sep 17 00:00:00 2001 From: Boris Date: Sun, 14 Jul 2024 00:04:50 +0200 Subject: [PATCH 13/22] mc_wind_estimator_tuning: Changed Quaternion package installed by requirements.txt --- .../EKF/python/tuning_tools/mc_wind_estimator/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From a5a67315fd447d6703a89462510f6bcf27b650c1 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 12 Jul 2024 13:13:36 -0400 Subject: [PATCH 14/22] ekf2: optical flow magnitude check compensated - additionally don't use flow for reset if magnitude isn't acceptable --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 0e4b8b668e..89bbb6add0 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 @@ -71,8 +71,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; @@ -120,6 +118,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const bool is_within_max_sensor_dist = 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; @@ -142,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()) { From e03aef616c95d42a316fef54228512b62466715d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Jul 2024 10:25:42 -0400 Subject: [PATCH 15/22] ekf2: add terrain/dist_bottom reset deltas (vehicle_local_position/vehicle_global_position) --- msg/VehicleGlobalPosition.msg | 2 ++ msg/VehicleLocalPosition.msg | 6 ++++- .../optical_flow/optical_flow_control.cpp | 18 +++++++++++-- .../range_finder/range_height_control.cpp | 18 +++++++++++-- src/modules/ekf2/EKF/ekf.h | 14 +++++++---- src/modules/ekf2/EKF/position_fusion.cpp | 13 +++++++++- src/modules/ekf2/EKF2.cpp | 6 ++++- src/modules/ekf2/EKF2Selector.cpp | 25 +++++++++++++++++++ src/modules/ekf2/EKF2Selector.hpp | 6 +++++ .../tasks/FlightTask/FlightTask.cpp | 7 +++++- .../tasks/FlightTask/FlightTask.hpp | 2 ++ 11 files changed, 104 insertions(+), 13 deletions(-) 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/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 89bbb6add0..a6c0569ac7 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 @@ -219,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 ce8377e06b..541dede819 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 @@ -267,9 +267,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/ekf.h b/src/modules/ekf2/EKF/ekf.h index 85ea4a7625..f23f514fd6 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); } @@ -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{}; }; @@ -599,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 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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index cee124db0c..44656a2067 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; 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/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) {}; From 9bbfc8715eb241194d8edbb924b08d4e73cb956d Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Mon, 15 Jul 2024 12:39:00 +0000 Subject: [PATCH 16/22] Update submodule mavlink to latest Mon Jul 15 12:39:00 UTC 2024 - mavlink in PX4/Firmware (abcf9ca6f0aadc91b203342db689f07630ec0e3a): https://github.com/mavlink/mavlink/commit/da3223ff9380bfe8e496fab8df2cbb06d5f8d5c3 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/d65952bacc02c4a5a1ed8249be73e6a66fa800ab - Changes: https://github.com/mavlink/mavlink/compare/da3223ff9380bfe8e496fab8df2cbb06d5f8d5c3...d65952bacc02c4a5a1ed8249be73e6a66fa800ab d65952ba 2024-07-05 Peter Barker - common.xml: correct description of GLOBAL_POSIITON_INT.relative_alt frame (#2131) 5fc2ff8e 2024-06-26 Hamish Willee - Add multiplier field to docs, if present (#2125) a13d2317 2024-06-26 Roman Bapst - Added MAV_CMD_EXTERNAL_WIND_ESTIMATE to development (#2122) d8af87fd 2024-06-19 jokalode - Update common.xml (#2116) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 177613eb68731990f3d0c620aad0dbef22216eb3 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:09:33 -0400 Subject: [PATCH 17/22] ekf2: GNSS velocity control should own vertical velocity reset if height faiing - GNSS height control using the velocity sample directly is ignoring potential position offsets --- .../EKF/aid_sources/gnss/gnss_height_control.cpp | 9 --------- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 16 +++++++++++----- 2 files changed, 11 insertions(+), 14 deletions(-) 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 ff04cc3b39..c141d415ee 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 @@ -108,15 +108,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_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 83f7ef624d..b8440595ae 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -132,17 +132,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(); } From 40349fa3dc72b626504e256ed136f7b3f71f4f17 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:20:16 -0400 Subject: [PATCH 18/22] ekf2: EV velocity control should own vertical velocity reset if height failing --- .../external_vision/ev_height_control.cpp | 28 ------------------- .../external_vision/ev_vel_control.cpp | 11 ++++++++ 2 files changed, 11 insertions(+), 28 deletions(-) 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 0684536ae5..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 @@ -149,34 +149,6 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp 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 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; - - 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_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp index a5ba562cad..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 @@ -212,6 +212,17 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample 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 { From 80ee622f77371f6af1344310f2a5b99e3b87bf9b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:24:35 -0400 Subject: [PATCH 19/22] ekf2: baro only reset vz as a last resort --- .../ekf2/EKF/aid_sources/barometer/baro_height_control.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 c6c6f220de..264c826738 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 @@ -134,8 +134,10 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) 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 = imu_sample.time_us; From 8e5f28f834bc0063a36c98485a33bebdff602b7f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 10 Jul 2024 13:25:14 -0400 Subject: [PATCH 20/22] ekf2: rng only reset vz as a last resort --- .../EKF/aid_sources/range_finder/range_height_control.cpp | 6 ++++-- src/modules/ekf2/test/test_EKF_terrain.cpp | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) 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 541dede819..6c33d4db63 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 @@ -180,8 +180,10 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _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 = imu_sample.time_us; 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); From 9d6c2baa908026af000cbdbf0680621f2b0fa777 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 15 Jul 2024 14:38:45 +0200 Subject: [PATCH 21/22] ekf2-flow: only allow flow when in range Also, as the flow makes the link between range and horizontal velocity, only allow it to start if at least one of the two is known. Otherwise the EKF will struggle to estimate both values at the same time. --- .../EKF/aid_sources/optical_flow/optical_flow_control.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 a6c0569ac7..390a957992 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 @@ -116,7 +116,7 @@ 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) @@ -124,12 +124,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) 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. From 1cd7d54170439378a77e17bb7c5c44275a1de2a9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Jul 2024 12:50:51 -0400 Subject: [PATCH 22/22] ekf2: consolidate GNSS yaw in gnss_yaw_control.cpp and fix naming (GPS->GNSS) Co-authored-by: Mathieu Bresciani --- msg/EstimatorStatusFlags.msg | 4 +- .../checks/estimatorCheck.cpp | 2 +- src/modules/ekf2/CMakeLists.txt | 2 +- src/modules/ekf2/EKF/CMakeLists.txt | 2 +- ...ps_yaw_fusion.cpp => gnss_yaw_control.cpp} | 130 +++++++++++++++--- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 106 +------------- .../aid_sources/magnetometer/mag_control.cpp | 2 +- .../zero_innovation_heading_update.cpp | 2 +- src/modules/ekf2/EKF/common.h | 6 +- src/modules/ekf2/EKF/ekf.h | 16 +-- src/modules/ekf2/EKF/ekf_helper.cpp | 2 +- src/modules/ekf2/EKF/estimator_interface.cpp | 2 +- src/modules/ekf2/EKF/estimator_interface.h | 2 +- src/modules/ekf2/EKF2.cpp | 4 +- src/modules/ekf2/test/CMakeLists.txt | 2 +- .../test/sensor_simulator/ekf_wrapper.cpp | 2 +- src/modules/ekf2/test/test_EKF_basics.cpp | 4 +- ..._EKF_gps_yaw.cpp => test_EKF_gnss_yaw.cpp} | 0 18 files changed, 144 insertions(+), 146 deletions(-) rename src/modules/ekf2/EKF/aid_sources/gnss/{gps_yaw_fusion.cpp => gnss_yaw_control.cpp} (57%) rename src/modules/ekf2/test/{test_EKF_gps_yaw.cpp => test_EKF_gnss_yaw.cpp} (100%) 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/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/gnss/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp similarity index 57% 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..456691635a 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; + + const bool is_gnss_yaw_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 + && !is_gnss_yaw_data_intermittent + && !_gps_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 b8440595ae..7bfb2e2831 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -98,7 +98,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); @@ -285,8 +285,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 @@ -354,104 +354,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; - - const bool is_gps_yaw_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 - && !is_gps_yaw_data_intermittent - && !_gps_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) { @@ -465,7 +367,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 2dab0da57e..35dd869e1f 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -153,7 +153,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) && !_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) 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 3c0a2c1d6c..fded4cfbcc 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 diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 05610bca58..da07028edc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -338,7 +338,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 @@ -580,7 +580,7 @@ uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -uint64_t gps_yaw : +uint64_t gnss_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed uint64_t ev_vel : @@ -588,7 +588,7 @@ uint64_t ev_vel : uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest -uint64_t gps_yaw_fault : +uint64_t gnss_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f23f514fd6..7070c71bf3 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -144,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; } @@ -173,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; } @@ -202,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; } @@ -995,17 +995,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 diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index e44d6c0bf6..02675d1f74 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 d4a579f5b8..f7a6c70dec 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -193,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/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 44656a2067..7f4c8d6516 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1912,12 +1912,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) 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_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_gps_yaw_fault = _ekf.control_status_flags().gps_yaw_fault; + 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; 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