mirror of
https://gitee.com/xiaohuolufeihua/bizhang_-obav.git
synced 2026-05-21 01:12:11 +00:00
boards: enable NuttX SPI DMA buffers
- update to NuttX with stm32f4 and stm32f7 SPI DMA internal buffers - remove explicit DMA buffer allocations from new IMU drivers - restore original BOARD_DMA_ALLOC_POOL_SIZE - decrease SPI DMA thresholds based on fmu-v2/v3/v4/v5 bench testing
This commit is contained in:
@@ -188,10 +188,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM3=y
|
||||
|
||||
@@ -231,8 +231,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -211,12 +211,14 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI2_DMA=y
|
||||
CONFIG_STM32F7_SPI2_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM14=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -235,8 +235,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -448,7 +448,6 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
@@ -162,8 +162,9 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM1=y
|
||||
|
||||
@@ -191,12 +191,14 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI2_DMA=y
|
||||
CONFIG_STM32F7_SPI2_DMA_BUFFER=2048
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
|
||||
@@ -362,8 +362,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 1024 + 1024) // 5120 fat + 1024 + 1024 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -188,11 +188,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI5_DMA=y
|
||||
CONFIG_STM32F7_SPI5_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
|
||||
@@ -248,7 +248,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
@@ -188,9 +188,10 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
CONFIG_STM32F7_TIM9=y
|
||||
|
||||
@@ -178,7 +178,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -187,9 +187,10 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM3=y
|
||||
|
||||
@@ -176,7 +176,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -400,7 +400,6 @@ __END_DECLS
|
||||
#define LED_TIM3_CH4OUT /* PTC8 RGB_B */ PIN_FTM3_CH4_1
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
@@ -484,7 +484,6 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
@@ -166,11 +166,12 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI2_DMA=y
|
||||
CONFIG_STM32_SPI3=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM1=y
|
||||
CONFIG_STM32_TIM5=y
|
||||
CONFIG_STM32_UART4=y
|
||||
|
||||
@@ -230,7 +230,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 1008) // 5120 fat + 1008 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
|
||||
@@ -188,8 +188,14 @@ CONFIG_STM32_SDIO_CARD=y
|
||||
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI4_DMA=y
|
||||
CONFIG_STM32_SPI4_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM3=y
|
||||
|
||||
@@ -188,9 +188,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI4_DMA=y
|
||||
CONFIG_STM32_SPI4_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_TIM10=y
|
||||
|
||||
@@ -188,8 +188,14 @@ CONFIG_STM32_SDIO_CARD=y
|
||||
CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI4_DMA=y
|
||||
CONFIG_STM32_SPI4_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM3=y
|
||||
|
||||
@@ -421,7 +421,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 512) // 5120 fat + 512 + 512 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
|
||||
@@ -187,10 +187,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
|
||||
@@ -188,10 +188,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
|
||||
@@ -188,10 +188,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
|
||||
@@ -277,8 +277,8 @@
|
||||
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs. */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1024) // 5120 fat + 512 + 1024 spi
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
|
||||
@@ -191,11 +191,12 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI5=y
|
||||
CONFIG_STM32_SPI6=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
|
||||
@@ -284,8 +284,8 @@
|
||||
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs. */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1024) // 5120 fat + 512 + 1024 spi
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
|
||||
@@ -192,12 +192,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -192,12 +192,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -191,12 +191,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -192,12 +192,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -192,12 +192,13 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI4=y
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_UART4=y
|
||||
|
||||
@@ -548,8 +548,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 1024 + 1024) // 5120 fat + 1024 + 1024 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -221,14 +221,17 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI2_DMA=y
|
||||
CONFIG_STM32F7_SPI2_DMA_BUFFER=4096
|
||||
CONFIG_STM32F7_SPI3=y
|
||||
CONFIG_STM32F7_SPI3_DMA=y
|
||||
CONFIG_STM32F7_SPI3_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
|
||||
@@ -222,14 +222,17 @@ CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32F7_SPI1=y
|
||||
CONFIG_STM32F7_SPI1_DMA=y
|
||||
CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI2=y
|
||||
CONFIG_STM32F7_SPI2_DMA=y
|
||||
CONFIG_STM32F7_SPI2_DMA_BUFFER=4096
|
||||
CONFIG_STM32F7_SPI3=y
|
||||
CONFIG_STM32F7_SPI3_DMA=y
|
||||
CONFIG_STM32F7_SPI3_DMA_BUFFER=1024
|
||||
CONFIG_STM32F7_SPI5=y
|
||||
CONFIG_STM32F7_SPI6=y
|
||||
CONFIG_STM32F7_SPI_DMA=y
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32F7_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32F7_TIM10=y
|
||||
CONFIG_STM32F7_TIM11=y
|
||||
CONFIG_STM32F7_TIM3=y
|
||||
|
||||
@@ -540,8 +540,7 @@
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs */
|
||||
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 4096 + 1024 + 1024) // 5120 fat + 4096 + 1024 + 1024 spi
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
/* This board provides the board_on_reset interface */
|
||||
|
||||
|
||||
@@ -187,10 +187,11 @@ CONFIG_STM32_SERIALBRK_BSDCOMPAT=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_SPI1=y
|
||||
CONFIG_STM32_SPI1_DMA=y
|
||||
CONFIG_STM32_SPI1_DMA_BUFFER=1024
|
||||
CONFIG_STM32_SPI2=y
|
||||
CONFIG_STM32_SPI4=y
|
||||
CONFIG_STM32_SPI_DMA=y
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=32
|
||||
CONFIG_STM32_SPI_DMATHRESHOLD=8
|
||||
CONFIG_STM32_TIM10=y
|
||||
CONFIG_STM32_TIM11=y
|
||||
CONFIG_STM32_TIM8=y
|
||||
|
||||
@@ -277,8 +277,8 @@
|
||||
|
||||
#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS
|
||||
|
||||
/* This board provides a DMA pool and APIs. */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1024) // 5120 fat + 512 + 1024 spi
|
||||
/* This board provides a DMA pool and APIs */
|
||||
#define BOARD_DMA_ALLOC_POOL_SIZE 5120
|
||||
|
||||
#define BOARD_HAS_ON_RESET 1
|
||||
|
||||
|
||||
@@ -50,13 +50,13 @@ namespace wq_configurations
|
||||
{
|
||||
static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 1600, 0}; // PX4 inner loop highest priority
|
||||
|
||||
static constexpr wq_config_t SPI0{"wq:SPI0", 2000, -1};
|
||||
static constexpr wq_config_t SPI1{"wq:SPI1", 2000, -2};
|
||||
static constexpr wq_config_t SPI2{"wq:SPI2", 2000, -3};
|
||||
static constexpr wq_config_t SPI3{"wq:SPI3", 2000, -4};
|
||||
static constexpr wq_config_t SPI4{"wq:SPI4", 2000, -5};
|
||||
static constexpr wq_config_t SPI5{"wq:SPI5", 2000, -6};
|
||||
static constexpr wq_config_t SPI6{"wq:SPI6", 2000, -7};
|
||||
static constexpr wq_config_t SPI0{"wq:SPI0", 2200, -1};
|
||||
static constexpr wq_config_t SPI1{"wq:SPI1", 2200, -2};
|
||||
static constexpr wq_config_t SPI2{"wq:SPI2", 2200, -3};
|
||||
static constexpr wq_config_t SPI3{"wq:SPI3", 2200, -4};
|
||||
static constexpr wq_config_t SPI4{"wq:SPI4", 2200, -5};
|
||||
static constexpr wq_config_t SPI5{"wq:SPI5", 2200, -6};
|
||||
static constexpr wq_config_t SPI6{"wq:SPI6", 2200, -7};
|
||||
|
||||
static constexpr wq_config_t I2C0{"wq:I2C0", 1400, -8};
|
||||
static constexpr wq_config_t I2C1{"wq:I2C1", 1400, -9};
|
||||
|
||||
Submodule platforms/nuttx/NuttX/nuttx updated: befe53e7f1...9d00617188
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "ICM20602.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -59,10 +57,6 @@ ICM20602::~ICM20602()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -79,14 +73,6 @@ bool ICM20602::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -529,14 +515,11 @@ uint16_t ICM20602::FIFOReadCount()
|
||||
|
||||
bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
@@ -546,9 +529,9 @@ bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
|
||||
bool bad_data = false;
|
||||
|
||||
ProcessGyro(timestamp_sample, report, samples);
|
||||
ProcessGyro(timestamp_sample, buffer, samples);
|
||||
|
||||
if (!ProcessAccel(timestamp_sample, report, samples)) {
|
||||
if (!ProcessAccel(timestamp_sample, buffer, samples)) {
|
||||
bad_data = true;
|
||||
}
|
||||
|
||||
@@ -556,7 +539,7 @@ bool ICM20602::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
|
||||
_temperature_update_timestamp = timestamp_sample;
|
||||
|
||||
if (!ProcessTemperature(report, samples)) {
|
||||
if (!ProcessTemperature(buffer, samples)) {
|
||||
bad_data = true;
|
||||
}
|
||||
}
|
||||
@@ -593,7 +576,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -605,12 +588,12 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 3) {
|
||||
if (fifo_accel_equal(report->f[0], report->f[1])) {
|
||||
if (fifo_accel_equal(buffer.f[0], buffer.f[1])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1;
|
||||
|
||||
} else if (fifo_accel_equal(report->f[1], report->f[2])) {
|
||||
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0;
|
||||
@@ -624,7 +607,7 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + 2) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
|
||||
@@ -644,7 +627,7 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -652,7 +635,7 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
|
||||
@@ -668,12 +651,12 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
_px4_gyro.updateFIFO(gyro);
|
||||
}
|
||||
|
||||
bool ICM20602::ProcessTemperature(const TransferBuffer *const report, uint8_t samples)
|
||||
bool ICM20602::ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
int16_t temperature[samples];
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L);
|
||||
}
|
||||
|
||||
|
||||
@@ -73,12 +73,12 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
@@ -112,11 +112,9 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
bool ProcessTemperature(const TransferBuffer *const report, uint8_t samples);
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
bool ProcessTemperature(const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "ICM20608G.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -60,10 +58,6 @@ ICM20608G::~ICM20608G()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -80,14 +74,6 @@ bool ICM20608G::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -513,14 +499,11 @@ uint16_t ICM20608G::FIFOReadCount()
|
||||
|
||||
bool ICM20608G::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
@@ -528,8 +511,8 @@ bool ICM20608G::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
ProcessGyro(timestamp_sample, report, samples);
|
||||
return ProcessAccel(timestamp_sample, report, samples);
|
||||
ProcessGyro(timestamp_sample, buffer, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer, samples);
|
||||
}
|
||||
|
||||
void ICM20608G::FIFOReset()
|
||||
@@ -562,7 +545,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -574,12 +557,12 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 3) {
|
||||
if (fifo_accel_equal(report->f[0], report->f[1])) {
|
||||
if (fifo_accel_equal(buffer.f[0], buffer.f[1])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1;
|
||||
|
||||
} else if (fifo_accel_equal(report->f[1], report->f[2])) {
|
||||
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0;
|
||||
@@ -593,7 +576,7 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + 2) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
|
||||
@@ -613,7 +596,7 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -621,7 +604,7 @@ void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferB
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
|
||||
|
||||
@@ -73,12 +73,12 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
@@ -112,12 +112,10 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "ICM20689.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -59,10 +57,6 @@ ICM20689::~ICM20689()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -79,14 +73,6 @@ bool ICM20689::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -507,14 +493,11 @@ uint16_t ICM20689::FIFOReadCount()
|
||||
|
||||
bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
@@ -522,8 +505,8 @@ bool ICM20689::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
ProcessGyro(timestamp_sample, report, samples);
|
||||
return ProcessAccel(timestamp_sample, report, samples);
|
||||
ProcessGyro(timestamp_sample, buffer, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer, samples);
|
||||
}
|
||||
|
||||
void ICM20689::FIFOReset()
|
||||
@@ -556,7 +539,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -568,12 +551,12 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 3) {
|
||||
if (fifo_accel_equal(report->f[0], report->f[1])) {
|
||||
if (fifo_accel_equal(buffer.f[0], buffer.f[1])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1;
|
||||
|
||||
} else if (fifo_accel_equal(report->f[1], report->f[2])) {
|
||||
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0;
|
||||
@@ -587,7 +570,7 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + 2) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
|
||||
@@ -607,7 +590,7 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferB
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -615,7 +598,7 @@ void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
|
||||
|
||||
@@ -73,12 +73,12 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
@@ -112,12 +112,10 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "ICM40609D.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -60,10 +58,6 @@ ICM40609D::~ICM40609D()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -80,14 +74,6 @@ bool ICM40609D::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -514,14 +500,11 @@ uint16_t ICM40609D::FIFOReadCount()
|
||||
|
||||
bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
@@ -529,12 +512,12 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
if (report->INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
|
||||
if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
|
||||
perf_count(_fifo_overflow_perf);
|
||||
FIFOReset();
|
||||
}
|
||||
|
||||
const uint16_t fifo_count_bytes = combine(report->FIFO_COUNTH, report->FIFO_COUNTL);
|
||||
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
|
||||
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
|
||||
|
||||
if (fifo_count_samples == 0) {
|
||||
@@ -549,7 +532,7 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
bool valid = true;
|
||||
|
||||
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
|
||||
const uint8_t FIFO_HEADER = report->f[i].FIFO_Header;
|
||||
const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
|
||||
|
||||
if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
|
||||
// FIFO sample empty if HEADER_MSG set
|
||||
@@ -574,8 +557,8 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
}
|
||||
|
||||
if (valid_samples > 0) {
|
||||
ProcessGyro(timestamp_sample, report, valid_samples);
|
||||
ProcessAccel(timestamp_sample, report, valid_samples);
|
||||
ProcessGyro(timestamp_sample, buffer, valid_samples);
|
||||
ProcessAccel(timestamp_sample, buffer, valid_samples);
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -594,7 +577,7 @@ void ICM40609D::FIFOReset()
|
||||
_fifo_read_samples.store(0);
|
||||
}
|
||||
|
||||
void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -603,7 +586,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_DATA_X1, fifo_sample.ACCEL_DATA_X0);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_DATA_Y1, fifo_sample.ACCEL_DATA_Y0);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_DATA_Z1, fifo_sample.ACCEL_DATA_Z0);
|
||||
@@ -621,7 +604,7 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -629,7 +612,7 @@ void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferB
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_DATA_X1, fifo_sample.GYRO_DATA_X0);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_DATA_Y1, fifo_sample.GYRO_DATA_Y0);
|
||||
|
||||
@@ -72,15 +72,15 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
|
||||
uint8_t INT_STATUS;
|
||||
uint8_t FIFO_COUNTH;
|
||||
uint8_t FIFO_COUNTL;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_bank0_config_t {
|
||||
Register::BANK_0 reg;
|
||||
@@ -113,12 +113,10 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
@@ -154,7 +152,7 @@ private:
|
||||
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
|
||||
|
||||
uint8_t _checked_register_bank0{0};
|
||||
static constexpr uint8_t size_register_bank0_cfg{11};
|
||||
static constexpr uint8_t size_register_bank0_cfg{10};
|
||||
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
|
||||
@@ -168,5 +166,4 @@ private:
|
||||
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
|
||||
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "ICM42688P.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -60,10 +58,6 @@ ICM42688P::~ICM42688P()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -80,14 +74,6 @@ bool ICM42688P::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -285,22 +271,22 @@ void ICM42688P::ConfigureAccel()
|
||||
|
||||
switch (ACCEL_FS_SEL) {
|
||||
case ACCEL_FS_SEL_2G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384);
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
|
||||
_px4_accel.set_range(2 * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_4G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192);
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
|
||||
_px4_accel.set_range(4 * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_8G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096);
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
|
||||
_px4_accel.set_range(8 * CONSTANTS_ONE_G);
|
||||
break;
|
||||
|
||||
case ACCEL_FS_SEL_16G:
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
|
||||
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
|
||||
_px4_accel.set_range(16 * CONSTANTS_ONE_G);
|
||||
break;
|
||||
}
|
||||
@@ -312,7 +298,7 @@ void ICM42688P::ConfigureGyro()
|
||||
|
||||
switch (GYRO_FS_SEL) {
|
||||
case GYRO_FS_SEL_125_DPS:
|
||||
_px4_gyro.set_scale(math::radians(1.0f / 262.f));
|
||||
_px4_gyro.set_scale(math::radians(1.f / 262.f));
|
||||
_px4_gyro.set_range(math::radians(125.f));
|
||||
break;
|
||||
|
||||
@@ -515,14 +501,11 @@ uint16_t ICM42688P::FIFOReadCount()
|
||||
|
||||
bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
return false;
|
||||
@@ -530,12 +513,12 @@ bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
if (report->INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
|
||||
if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) {
|
||||
perf_count(_fifo_overflow_perf);
|
||||
FIFOReset();
|
||||
}
|
||||
|
||||
const uint16_t fifo_count_bytes = combine(report->FIFO_COUNTH, report->FIFO_COUNTL);
|
||||
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
|
||||
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
|
||||
|
||||
if (fifo_count_samples == 0) {
|
||||
@@ -550,7 +533,7 @@ bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
bool valid = true;
|
||||
|
||||
// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx
|
||||
const uint8_t FIFO_HEADER = report->f[i].FIFO_Header;
|
||||
const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header;
|
||||
|
||||
if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) {
|
||||
// FIFO sample empty if HEADER_MSG set
|
||||
@@ -575,8 +558,8 @@ bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
}
|
||||
|
||||
if (valid_samples > 0) {
|
||||
ProcessGyro(timestamp_sample, report, valid_samples);
|
||||
ProcessAccel(timestamp_sample, report, valid_samples);
|
||||
ProcessGyro(timestamp_sample, buffer, valid_samples);
|
||||
ProcessAccel(timestamp_sample, buffer, valid_samples);
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -591,12 +574,11 @@ void ICM42688P::FIFOReset()
|
||||
RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH);
|
||||
|
||||
// reset while FIFO is disabled
|
||||
_data_ready_count.store(0);
|
||||
_fifo_watermark_interrupt_timestamp = 0;
|
||||
_fifo_read_samples.store(0);
|
||||
}
|
||||
|
||||
void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -605,7 +587,7 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_DATA_X1, fifo_sample.ACCEL_DATA_X0);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_DATA_Y1, fifo_sample.ACCEL_DATA_Y0);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_DATA_Z1, fifo_sample.ACCEL_DATA_Z0);
|
||||
@@ -623,7 +605,7 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const Transfer
|
||||
_px4_accel.updateFIFO(accel);
|
||||
}
|
||||
|
||||
void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -631,7 +613,7 @@ void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferB
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_DATA_X1, fifo_sample.GYRO_DATA_X0);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_DATA_Y1, fifo_sample.GYRO_DATA_Y0);
|
||||
|
||||
@@ -72,15 +72,15 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::BANK_0::INT_STATUS) | DIR_READ};
|
||||
uint8_t INT_STATUS;
|
||||
uint8_t FIFO_COUNTH;
|
||||
uint8_t FIFO_COUNTL;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_bank0_config_t {
|
||||
Register::BANK_0 reg;
|
||||
@@ -113,12 +113,10 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
void ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
@@ -135,7 +133,6 @@ private:
|
||||
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
|
||||
hrt_abstime _temperature_update_timestamp{0};
|
||||
|
||||
px4::atomic<uint8_t> _data_ready_count{0};
|
||||
px4::atomic<uint8_t> _fifo_read_samples{0};
|
||||
bool _data_ready_interrupt_enabled{false};
|
||||
|
||||
@@ -155,7 +152,7 @@ private:
|
||||
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
|
||||
|
||||
uint8_t _checked_register_bank0{0};
|
||||
static constexpr uint8_t size_register_bank0_cfg{11};
|
||||
static constexpr uint8_t size_register_bank0_cfg{10};
|
||||
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY },
|
||||
@@ -163,12 +160,10 @@ private:
|
||||
{ Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 },
|
||||
{ Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
|
||||
{ Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 },
|
||||
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 },
|
||||
{ Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TEMP_EN },
|
||||
{ Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime
|
||||
{ Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime
|
||||
{ Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 },
|
||||
{ Register::BANK_0::INT_CONFIG1, INT_CONFIG1_BIT::INT_TPULSE_DURATION, 0 },
|
||||
{ Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 },
|
||||
};
|
||||
|
||||
};
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "MPU6000.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace InvenSense_MPU6000;
|
||||
|
||||
@@ -66,10 +64,6 @@ MPU6000::~MPU6000()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_fifo_empty_perf);
|
||||
perf_free(_fifo_overflow_perf);
|
||||
@@ -101,14 +95,6 @@ bool MPU6000::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return true;
|
||||
@@ -285,20 +271,12 @@ void MPU6000::Run()
|
||||
}
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[16]; // max 16 samples
|
||||
};
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding
|
||||
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
@@ -317,7 +295,7 @@ void MPU6000::Run()
|
||||
|
||||
// accel data is duplicated 8 times
|
||||
for (int i = 0; i < accel.samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
// coordinate convention (x forward, y right, z down)
|
||||
accel.x[i] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
|
||||
@@ -326,7 +304,7 @@ void MPU6000::Run()
|
||||
}
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
// coordinate convention (x forward, y right, z down)
|
||||
gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
|
||||
|
||||
@@ -65,6 +65,15 @@ public:
|
||||
void PrintInfo();
|
||||
|
||||
private:
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
int probe() override;
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
@@ -79,8 +88,6 @@ private:
|
||||
|
||||
void ResetFIFO();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
|
||||
@@ -33,8 +33,6 @@
|
||||
|
||||
#include "MPU9250.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
@@ -59,10 +57,6 @@ MPU9250::~MPU9250()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
@@ -79,14 +73,6 @@ bool MPU9250::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
return Reset();
|
||||
}
|
||||
|
||||
@@ -507,15 +493,14 @@ uint16_t MPU9250::FIFOReadCount()
|
||||
|
||||
bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
{
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
FIFOTransferBuffer buffer{};
|
||||
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
set_frequency(SPI_SPEED_SENSOR);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
set_frequency(SPI_SPEED); // restore normal speed
|
||||
perf_end(_transfer_perf);
|
||||
perf_count(_bad_transfer_perf);
|
||||
@@ -525,8 +510,8 @@ bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples)
|
||||
set_frequency(SPI_SPEED); // restore normal speed
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
ProcessGyro(timestamp_sample, report, samples);
|
||||
return ProcessAccel(timestamp_sample, report, samples);
|
||||
ProcessGyro(timestamp_sample, buffer, samples);
|
||||
return ProcessAccel(timestamp_sample, buffer, samples);
|
||||
}
|
||||
|
||||
void MPU9250::FIFOReset()
|
||||
@@ -559,7 +544,7 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
|
||||
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
|
||||
}
|
||||
|
||||
bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Accelerometer::FIFOSample accel;
|
||||
accel.timestamp_sample = timestamp_sample;
|
||||
@@ -571,12 +556,12 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
int accel_first_sample = 1;
|
||||
|
||||
if (samples >= 3) {
|
||||
if (fifo_accel_equal(report->f[0], report->f[1])) {
|
||||
if (fifo_accel_equal(buffer.f[0], buffer.f[1])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0==A1, A2==A3
|
||||
accel_first_sample = 1;
|
||||
|
||||
} else if (fifo_accel_equal(report->f[1], report->f[2])) {
|
||||
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
|
||||
// [A0, A1, A2, A3]
|
||||
// A0, A1==A2, A3
|
||||
accel_first_sample = 0;
|
||||
@@ -590,7 +575,7 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
int accel_samples = 0;
|
||||
|
||||
for (int i = accel_first_sample; i < samples; i = i + 2) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
|
||||
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
|
||||
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
|
||||
@@ -610,7 +595,7 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBu
|
||||
return !bad_data;
|
||||
}
|
||||
|
||||
void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples)
|
||||
void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples)
|
||||
{
|
||||
PX4Gyroscope::FIFOSample gyro;
|
||||
gyro.timestamp_sample = timestamp_sample;
|
||||
@@ -618,7 +603,7 @@ void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuf
|
||||
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
|
||||
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
|
||||
|
||||
@@ -73,12 +73,12 @@ private:
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES];
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_R_W) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
@@ -112,12 +112,10 @@ private:
|
||||
bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples);
|
||||
void FIFOReset();
|
||||
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples);
|
||||
bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples);
|
||||
void UpdateTemperature();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
|
||||
@@ -33,10 +33,7 @@
|
||||
|
||||
#include "ISM330DLC.hpp"
|
||||
|
||||
#include <px4_platform/board_dma_alloc.h>
|
||||
|
||||
using namespace time_literals;
|
||||
using namespace ST_ISM330DLC;
|
||||
|
||||
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
|
||||
|
||||
@@ -60,10 +57,6 @@ ISM330DLC::~ISM330DLC()
|
||||
{
|
||||
Stop();
|
||||
|
||||
if (_dma_data_buffer != nullptr) {
|
||||
board_dma_free(_dma_data_buffer, FIFO::SIZE);
|
||||
}
|
||||
|
||||
perf_free(_interval_perf);
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_fifo_empty_perf);
|
||||
@@ -97,14 +90,6 @@ bool ISM330DLC::Init()
|
||||
return false;
|
||||
}
|
||||
|
||||
// allocate DMA capable buffer
|
||||
_dma_data_buffer = (uint8_t *)board_dma_alloc(FIFO::SIZE);
|
||||
|
||||
if (_dma_data_buffer == nullptr) {
|
||||
PX4_ERR("DMA alloc failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return true;
|
||||
@@ -292,21 +277,12 @@ void ISM330DLC::Run()
|
||||
return;
|
||||
}
|
||||
|
||||
// Transfer data
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
FIFO::DATA f[16]; // max 16 samples
|
||||
};
|
||||
static_assert(sizeof(TransferBuffer) == (sizeof(uint8_t) + 16 * sizeof(FIFO::DATA))); // ensure no struct padding
|
||||
|
||||
TransferBuffer *report = (TransferBuffer *)_dma_data_buffer;
|
||||
FIFOTransferBuffer buffer{};
|
||||
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
|
||||
memset(report, 0, transfer_size);
|
||||
report->cmd = static_cast<uint8_t>(Register::FIFO_DATA_OUT_L) | DIR_READ;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
|
||||
if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) {
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
@@ -324,7 +300,7 @@ void ISM330DLC::Run()
|
||||
gyro.dt = 1000000 / ST_ISM330DLC::G_ODR;
|
||||
|
||||
for (int i = 0; i < samples; i++) {
|
||||
const FIFO::DATA &fifo_sample = report->f[i];
|
||||
const FIFO::DATA &fifo_sample = buffer.f[i];
|
||||
|
||||
// sensor Z is up (RHC), flip y & z for publication
|
||||
gyro.x[i] = combine(fifo_sample.OUTX_L_G, fifo_sample.OUTX_H_G);
|
||||
|
||||
@@ -50,7 +50,7 @@
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
using ST_ISM330DLC::Register;
|
||||
using namespace ST_ISM330DLC;
|
||||
|
||||
class ISM330DLC : public device::SPI, public px4::ScheduledWorkItem
|
||||
{
|
||||
@@ -65,6 +65,20 @@ public:
|
||||
void PrintInfo();
|
||||
|
||||
private:
|
||||
|
||||
// Sensor Configuration
|
||||
static constexpr uint32_t GYRO_RATE{ST_ISM330DLC::G_ODR};
|
||||
static constexpr uint32_t ACCEL_RATE{ST_ISM330DLC::LA_ODR};
|
||||
static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))};
|
||||
|
||||
// Transfer data
|
||||
struct FIFOTransferBuffer {
|
||||
uint8_t cmd{static_cast<uint8_t>(Register::FIFO_DATA_OUT_L) | DIR_READ};
|
||||
FIFO::DATA f[FIFO_MAX_SAMPLES] {};
|
||||
};
|
||||
// ensure no struct padding
|
||||
static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA)));
|
||||
|
||||
int probe() override;
|
||||
|
||||
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
|
||||
@@ -79,8 +93,6 @@ private:
|
||||
|
||||
void ResetFIFO();
|
||||
|
||||
uint8_t *_dma_data_buffer{nullptr};
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
|
||||
Reference in New Issue
Block a user