ICM20602 and ICM20608-G: new standalone driver (8 kHz gyro, 4 kHz accel)

- uses the FIFO and SPI DMA - currently implemented on px4_fmu-v4 only - clip counters - new low pass filter that operates on an array of data (LowPassFilter2pArray) - new sensor messages for better visibility - sensor_{accel, gyro}_fifo: full raw data for optional logging and analysis - sensor_{accel, gyro}_status: metadata, clipping, etc - sensor_{accel, gyro}_integrated: delta angles and delta velocities - eventually these will replace the existing sensor_{accel, gyro} monolith
PX4 · Dec 4, 2019 · 99e1c75 · 99e1c75
1 parent 2badea3
commit 99e1c75
Show file tree

Hide file tree

Showing 26 changed files with 1,895 additions and 114 deletions.
diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake
@@ -53,7 +53,7 @@ px4_add_board(
 		#pca9685
 		#protocol_splitter
 		#pwm_input
-		pwm_out_sim
+		#pwm_out_sim
 		px4fmu
 		px4io
 		#roboclaw

diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake
@@ -28,6 +28,8 @@ px4_add_board(
 		imu/adis16448
 		imu/adis16477
 		imu/adis16497
+		imu/invensense/icm20602
+		imu/invensense/icm20608-g
 		imu/mpu6000
 		imu/mpu9250
 		irlock

diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors
@@ -49,5 +49,9 @@ then
 	mpu6000 -R 2 -T 20608 start
 fi
 
+# new sensor drivers (in testing)
+#icm20602 -R 8 start
+#icm20608g -R 8 start
+
 # mpu9250 internal SPI bus mpu9250 is rotated 90 deg yaw
 mpu9250 -R 2 start
diff --git a/boards/px4/fmu-v4/nuttx-config/include/board.h b/boards/px4/fmu-v4/nuttx-config/include/board.h
@@ -40,6 +40,7 @@
 /************************************************************************************
  * Included Files
  ************************************************************************************/
+#include "board_dma_map.h"
 
 #include <nuttx/config.h>
 #ifndef __ASSEMBLY__
@@ -195,17 +196,6 @@
 #  define SDIO_SDXFR_CLKDIV     (2 << SDIO_CLKCR_CLKDIV_SHIFT)
 #endif
 
-/* DMA Channl/Stream Selections *****************************************************/
-/* Stream selections are arbitrary for now but might become important in the future
- * is we set aside more DMA channels/streams.
- *
- * SDIO DMA
- *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
- *   DMAMAP_SDIO_2 = Channel 4, Stream 6
- */
-
-#define DMAMAP_SDIO DMAMAP_SDIO_1
-
 /* Alternate function pin selections ************************************************/
 
 /*
@@ -235,9 +225,6 @@
 
 /* UART8 has no alternate pin config */
 
-/* UART RX DMA configurations */
-#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1	/*DMA2 Stream 2*/
-#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1	/*DMA2 Stream 1*/
 
 /*
  * CAN

diff --git a/boards/px4/fmu-v4/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v4/nuttx-config/include/board_dma_map.h
@@ -0,0 +1,23 @@
+#pragma once
+
+/*   DMA Channel/Stream Selections
+ *
+ *   DMAMAP_USART3_RX   = DMA1, Stream 1, Channel 4
+ *   DMAMAP_UART4_RX    = DMA1, Stream 2, Channel 4
+ *   DMAMAP_UART7_RX    = DMA1, Stream 3, Channel 5
+ *   DMAMAP_USART2_RX   = DMA1, Stream 5, Channel 4
+ *   DMAMAP_TIM4_UP     = DMA1, Stream 6, Channel 2
+ *
+ *   DMAMAP_SPI1_RX_1   = DMA2, Stream 0, Channel 3
+ *   DMAMAP_USART6_RX_1 = DMA2, Stream 1, Channel 4
+ *   DMAMAP_USART1_RX_1 = DMA2, Stream 2, Channel 4
+ *   DMAMAP_SPI1_TX_1   = DMA2, Stream 3, Channel 3
+ *   DMAMAP_TIM1_UP     = DMA2, Stream 5, Channel 6
+ *   DMAMAP_SDIO_2      = DMA2, Stream 6, Channel 4
+ */
+
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1
+#define DMAMAP_SDIO DMAMAP_SDIO_2
diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig
@@ -186,8 +186,10 @@ 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_SPI2=y
 CONFIG_STM32_SPI4=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v4/nuttx-config/stackcheck/defconfig
@@ -187,8 +187,10 @@ 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_SPI2=y
 CONFIG_STM32_SPI4=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4/src/board_config.h b/boards/px4/fmu-v4/src/board_config.h
@@ -76,7 +76,6 @@
  *  PE15 HMC5983                                         PE12
  *  ---- ----------------------------------------------- -----
  */
-
 #define GPIO_SPI1_CS_PORTC_PIN2      (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 #define GPIO_SPI1_CS_PORTC_PIN15     (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
 #define GPIO_SPI1_CS_PORTE_PIN15     (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN15)
@@ -325,7 +324,7 @@
 #define BOARD_HAS_PWM    DIRECT_PWM_OUTPUT_CHANNELS
 
 /* This board provides a DMA pool and APIs. */
-#define BOARD_DMA_ALLOC_POOL_SIZE    5120
+#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1008)	// 5120 fat + 512 + 1008 spi
 
 #define BOARD_HAS_ON_RESET 1
 

diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake
@@ -30,6 +30,8 @@ px4_add_board(
 		#imu # all available imu drivers
 		imu/mpu6000
 		imu/mpu9250
+		imu/invensense/icm20602
+		imu/invensense/icm20608-g
 		irlock
 		lights/blinkm
 		lights/rgbled

diff --git a/boards/px4/fmu-v4pro/nuttx-config/include/board.h b/boards/px4/fmu-v4pro/nuttx-config/include/board.h
@@ -36,24 +36,21 @@
  *
  ************************************************************************************/
 
-#ifndef __NUTTX_CONFIG_PX4FMUV4_PRO_INCLUDE_BOARD_H
-#define __NUTTX_CONFIG_PX4FMUV4_PRO_INCLUDE_BOARD_H
+#ifndef __ARCH_BOARD_BOARD_H
+#define __ARCH_BOARD_BOARD_H
 
 /************************************************************************************
  * Included Files
  ************************************************************************************/
+#include "board_dma_map.h"
 
 #include <nuttx/config.h>
-
 #ifndef __ASSEMBLY__
 # include <stdint.h>
 #endif
 
-
 #include <stm32.h>
 
-
-
 /************************************************************************************
  * Definitions
  ************************************************************************************/
@@ -201,7 +198,7 @@
  * to service FIFOs in interrupt driven mode.  These values have not been
  * tuned!!!
  *
- * SDIOCLK =48MHz, SDMMC_CK=SDIOCLK/(118+2)=400 KHz
+ * SDIOCLK=48MHz, SDMMC_CK=SDIOCLK/(118+2)=400 KHz
  */
 
 /* Use the Falling edge of the SDIO_CLK clock to change the edge the
@@ -210,16 +207,16 @@
 
 #define SDIO_CLKCR_EDGE SDIO_CLKCR_NEGEDGE
 
-#define SDIO_INIT_CLKDIV         (118 << SDIO_CLKCR_CLKDIV_SHIFT)
+#define SDIO_INIT_CLKDIV        (118 << SDIO_CLKCR_CLKDIV_SHIFT)
 
 /* DMA ON:  SDIOCLK=48MHz, SDMMC_CK=SDIOCLK/(1+2)=16 MHz
  * DMA OFF: SDIOCLK=48MHz, SDMMC_CK=SDIOCLK/(2+2)=12 MHz
  */
 
 #ifdef CONFIG_STM32_SDIO_DMA
-#  define SDIO_MMCXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#  define SDIO_MMCXFR_CLKDIV    (1 << SDIO_CLKCR_CLKDIV_SHIFT)
 #else
-#  define SDIO_MMCXFR_CLKDIV     (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#  define SDIO_MMCXFR_CLKDIV    (2 << SDIO_CLKCR_CLKDIV_SHIFT)
 #endif
 
 /* DMA ON:  SDIOCLK=48MHz, SDMMC_CK=SDIOCLK/(1+2)=16 MHz
@@ -228,23 +225,11 @@
 //TODO #warning "Check Freq for 24mHz"
 
 #ifdef CONFIG_STM32_SDIO_DMA
-#  define SDIO_SDXFR_CLKDIV      (1 << SDIO_CLKCR_CLKDIV_SHIFT)
+#  define SDIO_SDXFR_CLKDIV     (1 << SDIO_CLKCR_CLKDIV_SHIFT)
 #else
-#  define SDIO_SDXFR_CLKDIV      (2 << SDIO_CLKCR_CLKDIV_SHIFT)
+#  define SDIO_SDXFR_CLKDIV     (2 << SDIO_CLKCR_CLKDIV_SHIFT)
 #endif
 
-/* DMA Channel/Stream Selections *****************************************************/
-/* Stream selections are arbitrary for now but might become important in the future
- * is we set aside more DMA channels/streams.
- *
- * SDIO DMA
- *   DMAMAP_SDIO_1 = Channel 4, Stream 3 <- may later be used by SPI DMA
- *   DMAMAP_SDIO_2 = Channel 4, Stream 6
- */
-
-#define DMAMAP_SDIO DMAMAP_SDIO_1
-
-
 /* FLASH wait states
  *
  *  --------- ---------- ----------- ---------
@@ -260,58 +245,6 @@
 
 #define BOARD_FLASH_WAITSTATES 5
 
-/* LED definitions ******************************************************************/
-/* The px4_fmu-v4pro board has numerous LEDs.
- *  FMU_LED_RED, FMU_LED_GREEN & FMU_LED_BLUE are directly connected and
- * can be controlled by software.
- *
- * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way.
- * The following definitions are used to access individual LEDs.
- */
-
-/* LED index values for use with board_userled() */
-
-#define BOARD_LED1        0
-#define BOARD_LED2        1
-#define BOARD_LED3        2
-#define BOARD_NLEDS       3
-
-#define BOARD_LED_RED     BOARD_LED1
-#define BOARD_LED_GREEN   BOARD_LED2
-#define BOARD_LED_BLUE    BOARD_LED3
-
-/* LED bits for use with board_userled_all() */
-
-#define BOARD_LED1_BIT    (1 << BOARD_LED1)
-#define BOARD_LED2_BIT    (1 << BOARD_LED2)
-#define BOARD_LED3_BIT    (1 << BOARD_LED3)
-
-/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in
- * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related
- * events as follows:
- *
- *
- *   SYMBOL                     Meaning                      LED state
- *                                                        Red   Green Blue
- *   ----------------------  --------------------------  ------ ------ ----*/
-
-#define LED_STARTED        0 /* NuttX has been started   OFF    OFF   OFF  */
-#define LED_HEAPALLOCATE   1 /* Heap has been allocated  OFF    OFF   ON   */
-#define LED_IRQSENABLED    2 /* Interrupts enabled       OFF    ON    OFF  */
-#define LED_STACKCREATED   3 /* Idle stack created       OFF    ON    ON   */
-#define LED_INIRQ          4 /* In an interrupt          N/C    N/C   GLOW */
-#define LED_SIGNAL         5 /* In a signal handler      N/C    GLOW  N/C  */
-#define LED_ASSERTION      6 /* An assertion failed      GLOW   N/C   GLOW */
-#define LED_PANIC          7 /* The system has crashed   Blink  OFF   N/C  */
-#define LED_IDLE           8 /* MCU is is sleep mode     ON     OFF   OFF  */
-
-/* Thus if the Green LED is statically on, NuttX has successfully booted and
- * is, apparently, running normally.  If the Red LED is flashing at
- * approximately 2Hz, then a fatal error has been detected and the system
- * has halted.
- */
-
-
 /* Alternate function pin selections ************************************************/
 
 /*
@@ -341,9 +274,6 @@
 
 /* UART8 has no alternate pin config */
 
-/* UART RX DMA configurations */
-#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
-#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
 
 /*
  * CAN
@@ -383,7 +313,6 @@
 #define GPIO_SPI1_MOSI  GPIO_SPI1_MOSI_1
 #define GPIO_SPI1_SCK   GPIO_SPI1_SCK_1
 
-
 #define GPIO_SPI2_MISO  GPIO_SPI2_MISO_1
 #define GPIO_SPI2_MOSI  GPIO_SPI2_MOSI_1
 #define GPIO_SPI2_SCK   GPIO_SPI2_SCK_1
@@ -422,7 +351,6 @@
 # define PROBE_MARK(n)
 #endif
 
-
 /************************************************************************************
  * Public Data
  ************************************************************************************/
@@ -458,4 +386,4 @@ EXTERN void stm32_boardinitialize(void);
 #endif
 
 #endif /* __ASSEMBLY__ */
-#endif  /* __NUTTX_CONFIG_PX4FMUV4_PRO_INCLUDE_BOARD_H */
+#endif  /* __ARCH_BOARD_BOARD_H */
diff --git a/boards/px4/fmu-v4pro/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v4pro/nuttx-config/include/board_dma_map.h
@@ -0,0 +1,23 @@
+#pragma once
+
+/*   DMA Channel/Stream Selections
+ *
+ *   DMAMAP_USART3_RX   = DMA1, Stream 1, Channel 4
+ *   DMAMAP_UART4_RX    = DMA1, Stream 2, Channel 4
+ *   DMAMAP_UART7_RX    = DMA1, Stream 3, Channel 5
+ *   DMAMAP_USART2_RX   = DMA1, Stream 5, Channel 4
+ *   DMAMAP_UART8_RX    = DMA1, Stream 6, Channel 5
+ *
+ *   DMAMAP_SPI1_RX_1   = DMA2, Stream 0, Channel 3
+ *   DMAMAP_USART6_RX_2 = DMA2, Stream 2, Channel 5
+ *   DMAMAP_SPI1_TX_1   = DMA2, Stream 3, Channel 3
+ *   DMAMAP_USART1_RX_2 = DMA2, Stream 5, Channel 4
+ *   DMAMAP_SDIO_2      = DMA2, Stream 6, Channel 4
+ *   DMAMAP_USART6_TX_2 = DMA2, Stream 7, Channel 5
+ */
+#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1
+#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2
+#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1
+#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2
+#define DMAMAP_SDIO DMAMAP_SDIO_2
+#define DMAMAP_USART6_RX DMAMAP_USART6_TX_2
diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig
@@ -190,9 +190,11 @@ 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_SPI2=y
 CONFIG_STM32_SPI5=y
 CONFIG_STM32_SPI6=y
+CONFIG_STM32_SPI_DMA=y
 CONFIG_STM32_TIM10=y
 CONFIG_STM32_TIM11=y
 CONFIG_STM32_TIM1=y

diff --git a/boards/px4/fmu-v4pro/src/board_config.h b/boards/px4/fmu-v4pro/src/board_config.h
@@ -326,9 +326,8 @@
 
 #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 a DMA pool and APIs. */
+#define BOARD_DMA_ALLOC_POOL_SIZE (5120 + 512 + 1008)	// 5120 fat + 512 + 1008 spi
 
 #define BOARD_HAS_ON_RESET 1
 

diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg
@@ -7,12 +7,9 @@ float32 temperature
 
 uint8 rotation
 
-# clipping per axis?
-uint64[3] clipping
+uint64[3] clipping	# clipping per axis
 
 uint16 measure_rate
 uint16 sample_rate
 
 float32 full_scale_range
-
-float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)
diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg
@@ -7,14 +7,9 @@ float32 temperature
 
 uint8 rotation
 
-# clipping per axis?
-uint64[3] clipping
+uint64[3] clipping	# clipping per axis
 
 uint16 measure_rate
 uint16 sample_rate
 
 float32 full_scale_range
-
-float32 coning_vibration	# Level of coning vibration in the IMU delta angles (rad^2)
-
-float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad)