diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index d2e2fc39aa4f..2aaca9dba473 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -27,6 +27,8 @@ px4_add_board( gps heater #imu # all available imu drivers + 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 index 67d5fb17132d..125b3a0d2719 100644 --- 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 index cb68f6e3fa65..a4196e7a1fee 100644 --- 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 #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 new file mode 100644 index 000000000000..7bab744b630e --- /dev/null +++ b/boards/px4/fmu-v4/nuttx-config/include/board_dma_map.h @@ -0,0 +1,22 @@ +#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 index dee3bcd7a352..edf54b3cf9c8 100644 --- 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 index 60aae744f3ca..beab6e326c0d 100644 --- 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 index 0c808db94034..b22f3a03366b 100644 --- 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 index bd4c86569a22..12d5e1a52029 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -31,6 +31,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 index 59585d700acd..7feb6c51be48 100644 --- 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 - #ifndef __ASSEMBLY__ # include #endif - #include - - /************************************************************************************ * 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 new file mode 100644 index 000000000000..b8ee26a686ba --- /dev/null +++ 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 index 8b62313780d1..a534a9bf028e 100644 --- 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 index cdbb5584dbae..4430e92e7f71 100644 --- 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/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake index 69a602e2a4d8..4d18f36c92ad 100644 --- a/boards/px4/fmu-v5/stackcheck.cmake +++ b/boards/px4/fmu-v5/stackcheck.cmake @@ -50,9 +50,9 @@ px4_add_board( rc_input #roboclaw safety_button - tap_esc + #tap_esc telemetry # all available telemetry drivers - test_ppm + #test_ppm tone_alarm #uavcan diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 7404cf5e71ba..0213e01facab 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -42,8 +42,8 @@ set(msg_files camera_capture.msg camera_trigger.msg cellular_status.msg - collision_report.msg collision_constraints.msg + collision_report.msg commander_state.msg cpuload.msg debug_array.msg @@ -101,12 +101,16 @@ set(msg_files safety.msg satellite_info.msg sensor_accel.msg + sensor_accel_fifo.msg + sensor_accel_status.msg sensor_baro.msg sensor_bias.msg sensor_combined.msg sensor_correction.msg sensor_gyro.msg sensor_gyro_control.msg + sensor_gyro_fifo.msg + sensor_gyro_status.msg sensor_mag.msg sensor_preflight.msg sensor_selection.msg diff --git a/msg/sensor_accel.msg b/msg/sensor_accel.msg index 18ca1b96d17b..119fa7fd124b 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -7,14 +7,16 @@ float32 x # acceleration in the NED X board axis in m/s^2 float32 y # acceleration in the NED Y board axis in m/s^2 float32 z # acceleration in the NED Z board axis in m/s^2 -uint32 integral_dt # integration time (microseconds) +uint32 integral_dt # integration time (microseconds) +uint8 integral_samples # number of samples integrated float32 x_integral # delta velocity in the NED X board axis in m/s over the integration time frame (integral_dt) float32 y_integral # delta velocity in the NED Y board axis in m/s over the integration time frame (integral_dt) float32 z_integral # delta velocity in the NED Z board axis in m/s over the integration time frame (integral_dt) +uint8 integral_clip_count # total clip count per integration period on any axis float32 temperature # temperature in degrees celsius -float32 scaling # scaling from raw to m/s^s +float32 scaling # scaling from raw to m/s^2 int16 x_raw int16 y_raw int16 z_raw diff --git a/msg/sensor_accel_fifo.msg b/msg/sensor_accel_fifo.msg new file mode 100644 index 000000000000..e110ac994fc9 --- /dev/null +++ b/msg/sensor_accel_fifo.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 timestamp_sample # time since system start (microseconds) + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[4] x # acceleration in the NED X board axis in m/s/s +int16[4] y # acceleration in the NED Y board axis in m/s/s +int16[4] z # acceleration in the NED Z board axis in m/s/s diff --git a/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg new file mode 100644 index 000000000000..d670906c7f1a --- /dev/null +++ b/msg/sensor_accel_status.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 error_count + +float32 temperature + +uint8 rotation + +# clipping per axis? +uint64[3] clipping + +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.msg b/msg/sensor_gyro.msg index 5a7ec71d7887..b4ff35c666cc 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -1,6 +1,5 @@ -uint32 device_id # unique device ID for the sensor that does not change between power cycles - uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles uint64 error_count @@ -9,9 +8,11 @@ float32 y # angular velocity in the NED Y board axis in rad/s float32 z # angular velocity in the NED Z board axis in rad/s uint32 integral_dt # integration time (microseconds) +uint8 integral_samples # number of samples integrated float32 x_integral # delta angle in the NED X board axis in rad/s over the integration time frame (integral_dt) float32 y_integral # delta angle in the NED Y board axis in rad/s over the integration time frame (integral_dt) float32 z_integral # delta angle in the NED Z board axis in rad/s over the integration time frame (integral_dt) +uint8 integral_clip_count # total clip count per integration period on any axis float32 temperature # temperature in degrees celsius diff --git a/msg/sensor_gyro_fifo.msg b/msg/sensor_gyro_fifo.msg new file mode 100644 index 000000000000..15024bff968e --- /dev/null +++ b/msg/sensor_gyro_fifo.msg @@ -0,0 +1,13 @@ +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[8] x # angular velocity in the NED X board axis in rad/s +int16[8] y # angular velocity in the NED Y board axis in rad/s +int16[8] z # angular velocity in the NED Z board axis in rad/s diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg new file mode 100644 index 000000000000..dfa8521e2d4e --- /dev/null +++ b/msg/sensor_gyro_status.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 error_count + +float32 temperature + +uint8 rotation + +# clipping per axis? +uint64[3] clipping + +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) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index dd5148e4fec4..11d1a239005e 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -264,6 +264,14 @@ rtps: id: 116 - msg: cellular_status id: 117 + - msg: sensor_accel_fifo + id: 118 + - msg: sensor_accel_status + id: 119 + - msg: sensor_gyro_fifo + id: 120 + - msg: sensor_gyro_status + id: 121 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 150 diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h index 4a3ab76a2ba5..4de616f40e7b 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_dma_alloc.h @@ -46,6 +46,8 @@ #include +__BEGIN_DECLS + /************************************************************************************ * Name: board_dma_alloc_init * @@ -114,7 +116,7 @@ __EXPORT int board_get_dma_usage(uint16_t *dma_total, uint16_t *dma_used, uint16 #if defined(BOARD_DMA_ALLOC_POOL_SIZE) __EXPORT void *board_dma_alloc(size_t size); #else -#define board_dma_alloc(size) (NULL) +#define board_dma_alloc(size) malloc(size) #endif /************************************************************************************ @@ -131,5 +133,7 @@ __EXPORT void *board_dma_alloc(size_t size); #if defined(BOARD_DMA_ALLOC_POOL_SIZE) __EXPORT void board_dma_free(FAR void *memory, size_t size); #else -#define board_dma_free(memory, size) () +#define board_dma_free(memory, size) free(memory) #endif + +__END_DECLS diff --git a/src/drivers/distance_sensor/tfmini/CMakeLists.txt b/src/drivers/distance_sensor/tfmini/CMakeLists.txt index 51ad9c5f4b52..37d965611d36 100644 --- a/src/drivers/distance_sensor/tfmini/CMakeLists.txt +++ b/src/drivers/distance_sensor/tfmini/CMakeLists.txt @@ -40,4 +40,6 @@ px4_add_module( tfmini_parser.cpp MODULE_CONFIG module.yaml + DEPENDS + drivers_rangefinder ) diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt new file mode 100644 index 000000000000..8b3fcf4050a2 --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2019 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__imu__invensense__icm20602 + MAIN icm20602 + COMPILE_FLAGS + SRCS + ICM20602.cpp + ICM20602.hpp + icm20602_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp new file mode 100644 index 000000000000..24dbebb97422 --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -0,0 +1,468 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "ICM20602.hpp" + +#include + +using namespace time_literals; +using namespace InvenSense_ICM20602; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } + +static constexpr uint32_t _fifo_interval{1000}; // 1000 us / 1000 Hz interval +static constexpr uint32_t _gyro_rate{8000}; // 8 kHz gyro +static constexpr uint32_t _gyro_readings_per_sample{_fifo_interval / (1000000 / _gyro_rate)}; +static constexpr uint32_t _accel_rate{4000}; // 4 kHz accel +static constexpr uint32_t _accel_readings_per_sample{_fifo_interval / (1000000 / _accel_rate)}; + +ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) : + SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) +{ + set_device_type(DRV_ACC_DEVTYPE_ICM20602); + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20602); + _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602); + + _px4_accel.set_sample_rate(_accel_rate); + _px4_gyro.set_sample_rate(_gyro_rate); + + _px4_accel.set_update_rate(1000000 / _fifo_interval); + _px4_gyro.set_update_rate(1000000 / _fifo_interval); +} + +ICM20602::~ICM20602() +{ + 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); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_count_perf); + perf_free(_drdy_interval_perf); +} + +int +ICM20602::probe() +{ + uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != ICM20602_WHOAMI) { + PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return OK; +} + +bool +ICM20602::Init() +{ + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init failed"); + return false; + } + + if (!Reset()) { + PX4_ERR("reset failed"); + 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; +} + +bool +ICM20602::Reset() +{ + for (int i = 0; i < 5; i++) { + + // PWR_MGMT_1: Device Reset + // CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + usleep(1000); + + // PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); + usleep(1000); + + // ACCEL_CONFIG: Accel 16 G range + RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G); + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + + // GYRO_CONFIG: Gyro 2000 degrees/second + RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS); + _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + + const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::DEVICE_RESET); + const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0); + const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT); + + // reset done once data is ready + if (reset_done && clksel_done && data_ready) { + return true; + } + } + + return false; +} + +void +ICM20602::ResetFIFO() +{ + perf_count(_fifo_reset_perf); + + // ACCEL_CONFIG2: Accel DLPF disabled for full rate (4 kHz) + RegisterSetBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF); + + // GYRO_CONFIG: Gyro DLPF disabled for full rate (8 kHz) + RegisterClearBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF); + + // FIFO_EN: disable FIFO + RegisterWrite(Register::FIFO_EN, 0); + RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RST); + + // USER_CTRL: reset FIFO then re-enable + RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST); + up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock + RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN); + + // CONFIG: should ensure that bit 7 of register 0x1A is set to 0 before using FIFO watermark feature + RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE); + RegisterClearBits(Register::CONFIG, CONFIG_BIT::FIFO_WM); + RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + + // FIFO_EN: enable both gyro and accel + _data_ready_count = 0; + RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::GYRO_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN); + up_udelay(10); +} + +uint8_t +ICM20602::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void +ICM20602::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void +ICM20602::RegisterSetBits(Register reg, uint8_t setbits) +{ + uint8_t val = RegisterRead(reg); + + if (!(val & setbits)) { + val |= setbits; + RegisterWrite(reg, val); + } +} + +void +ICM20602::RegisterClearBits(Register reg, uint8_t clearbits) +{ + uint8_t val = RegisterRead(reg); + + if (val & clearbits) { + val &= !clearbits; + RegisterWrite(reg, val); + } +} + +int +ICM20602::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + ICM20602 *dev = reinterpret_cast(arg); + + dev->DataReady(); + + return 0; +} + +void +ICM20602::DataReady() +{ + perf_count(_drdy_count_perf); + perf_count(_drdy_interval_perf); + + _data_ready_count++; + + if (_data_ready_count >= 8) { + _time_data_ready = hrt_absolute_time(); + + _data_ready_count = 0; + + // make another measurement + ScheduleNow(); + } +} + +void +ICM20602::Start() +{ + Stop(); + + ResetFIFO(); + + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20602::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY4_ICM20602) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, true, false, true, &ICM20602::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_DRDY_ICM_2060X) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20602::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#else + ScheduleOnInterval(_fifo_interval, _fifo_interval); +#endif +} + +void +ICM20602::Stop() +{ + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY4_ICM20602) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_SPI1_DRDY4_ICM20602, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_SPI1_DRDY1_ICM20602) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_SPI1_DRDY1_ICM20602, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_DRDY_ICM_2060X) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#else + ScheduleClear(); +#endif + +} + +void +ICM20602::Run() +{ + perf_count(_interval_perf); + + // check for FIFO overflow and reset + const bool fifo_overflow = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT); + + if (fifo_overflow) { + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // // check FIFO count + // uint8_t fifo_count_buf[3] {}; + // fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; + // const hrt_abstime timestamp_fifo_check = hrt_absolute_time(); + + // if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + // return; + // } + + // const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); + // const int samples = fifo_count / sizeof(FIFO::DATA); + + // if (samples < 1) { + // perf_count(_fifo_empty_perf); + // return; + + // } else if (samples > 32) { + // // check for FIFO overflow and reset + // const bool fifo_overflow = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT); + + // if (fifo_overflow) { + // perf_count(_fifo_overflow_perf); + // } + + // ResetFIFO(); + // return; + // } + + // only read 8 samples + const int samples = 8; + + // Transfer data + // 1 (cmd) + sizeof(FIFO_TRANSFER) * 32 = 449 bytes + struct ICM_Report { + uint8_t cmd; + FIFO::DATA f[32]; // we never transfer more than 32 samples + }; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + ICM_Report *report = (ICM_Report *)_dma_data_buffer; + memset(report, 0, transfer_size); + report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; + + perf_begin(_transfer_perf); + + if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + return; + } + + perf_end(_transfer_perf); + + + // Process data (only the latest 8 samples) + const int num_samples = 8; + const int first_sample = math::min(0, num_samples - 8); + const int last_sample = num_samples; + + + static constexpr uint32_t gyro_dt = _fifo_interval / _gyro_readings_per_sample; + // estimate timestamp of first sample in the FIFO from number of samples and fill rate + const hrt_abstime timestamp_sample = _time_data_ready - ((num_samples - 1) * gyro_dt); + + PX4Accelerometer::FIFOSample accel{}; + accel.timestamp_sample = timestamp_sample; + accel.dt = _fifo_interval / _accel_readings_per_sample; + + PX4Gyroscope::FIFOSample gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = num_samples; + gyro.dt = _fifo_interval / _gyro_readings_per_sample; + + int accel_samples = 0; + int16_t temperature[samples] {}; + + for (int i = first_sample; i < last_sample; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + + // accel data is doubled + if (i % 2) { + accel.x[accel_samples] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); + accel.y[accel_samples] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); + accel.z[accel_samples] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); + + accel_samples++; + } + + temperature[i] = combine(fifo_sample.TEMP_OUT_H, fifo_sample.TEMP_OUT_L); + + gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); + gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); + gyro.z[i] = -combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); + } + + accel.samples = accel_samples; + + // Temperature + int32_t temperature_sum{0}; + + for (auto t : temperature) { + temperature_sum += t; + } + + const int16_t temperature_avg = temperature_sum / samples; + + for (auto t : temperature) { + // temperature changing wildly is likely a sign of a transfer error + if (abs(t - temperature_avg) > 1000) { + return; + } + } + + // use average temperature reading + const float temperature_C = temperature_avg / 326.8f + 25.0f; // 326.8 LSB/C + _px4_accel.set_temperature(temperature_C); + _px4_gyro.set_temperature(temperature_C); + + + _px4_gyro.updateFIFO(gyro); + _px4_accel.updateFIFO(accel); +} + +void +ICM20602::PrintInfo() +{ + perf_print_counter(_interval_perf); + perf_print_counter(_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_count_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_accel.print_status(); + _px4_gyro.print_status(); +} diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp new file mode 100644 index 000000000000..9a4aa5d024f6 --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -0,0 +1,100 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 ICM20602.hpp + * + * Driver for the Invensense ICM20602 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM20602_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using InvenSense_ICM20602::Register; + +class ICM20602 : public device::SPI, public px4::ScheduledWorkItem +{ +public: + ICM20602(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + virtual ~ICM20602(); + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + + int probe() override; + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + + void Run() override; + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetBits(Register reg, uint8_t setbits); + void RegisterClearBits(Register reg, uint8_t clearbits); + + void ResetFIFO(); + + + uint8_t *_dma_data_buffer{nullptr}; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")}; + perf_counter_t _drdy_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")}; + perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; + + hrt_abstime _time_data_ready{0}; + int _data_ready_count{0}; + +}; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp new file mode 100644 index 000000000000..cf6e522fdf26 --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 InvenSense_ICM20602_registers.hpp + * + * Invensense ICM-20602 registers. + * + */ + +#pragma once + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace InvenSense_ICM20602 +{ + +static constexpr uint8_t DIR_READ = 0x80; +static constexpr uint8_t DIR_WRITE = 0x00; + +static constexpr uint8_t ICM20602_WHOAMI = 0x12; + +static constexpr uint32_t SPI_SPEED = 10 * 1000 * + 1000; // 10MHz SPI serial interface for communicating with all registers + +enum class +Register : uint8_t { + + CONFIG = 0x1A, + GYRO_CONFIG = 0x1B, + ACCEL_CONFIG = 0x1C, + ACCEL_CONFIG2 = 0x1D, + + FIFO_EN = 0x23, + + INT_STATUS = 0x3A, + + INT_PIN_CFG = 0x37, + INT_ENABLE = 0x38, + + TEMP_OUT_H = 0x41, + TEMP_OUT_L = 0x42, + + USER_CTRL = 0x6A, + PWR_MGMT_1 = 0x6B, + + FIFO_COUNTH = 0x72, + FIFO_COUNTL = 0x73, + FIFO_R_W = 0x74, + WHO_AM_I = 0x75, + +}; + +// CONFIG +enum +CONFIG_BIT : uint8_t { + FIFO_WM = Bit7, + FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO + + DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0] +}; + +// GYRO_CONFIG +enum +GYRO_CONFIG_BIT : uint8_t { + // FS_SEL [4:3] + FS_SEL_250_DPS = 0, // 0b00000 + FS_SEL_500_DPS = Bit3, // 0b01000 + FS_SEL_1000_DPS = Bit4, // 0b10000 + FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 + + // FCHOICE_B [1:0] + FCHOICE_B_32KHZ_BYPASS_DLPF = Bit0, // 0b01 - 3-dB BW: 8173 Noise BW (Hz): 8595.1 32 kHz + FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz +}; + +// ACCEL_CONFIG +enum +ACCEL_CONFIG_BIT : uint8_t { + // ACCEL_FS_SEL [4:3] + ACCEL_FS_SEL_2G = 0, // 0b00000 + ACCEL_FS_SEL_4G = Bit3, // 0b01000 + ACCEL_FS_SEL_8G = Bit4, // 0b10000 + ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000 +}; + +// ACCEL_CONFIG2 +enum +ACCEL_CONFIG2_BIT : uint8_t { + ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3, +}; + +// FIFO_EN +enum +FIFO_EN_BIT : uint8_t { + GYRO_FIFO_EN = Bit4, + ACCEL_FIFO_EN = Bit3, +}; + +// INT_ENABLE +enum +INT_ENABLE_BIT : uint8_t { + FIFO_OFLOW_EN = Bit4, + + DATA_RDY_INT_EN = Bit0 +}; + +// INT_STATUS +enum +INT_STATUS_BIT : uint8_t { + FIFO_OFLOW_INT = Bit4, + DATA_RDY_INT = Bit0, +}; + +// USER_CTRL +enum +USER_CTRL_BIT : uint8_t { + FIFO_EN = Bit6, + FIFO_RST = Bit2, +}; + +// PWR_MGMT_1 +enum +PWR_MGMT_1_BIT : uint8_t { + DEVICE_RESET = Bit7, + CLKSEL_2 = Bit2, + CLKSEL_1 = Bit1, + CLKSEL_0 = Bit0, +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 1008; + +// FIFO_DATA layout when FIFO_EN has both GYRO_FIFO_EN and ACCEL_FIFO_EN set +struct DATA { + uint8_t ACCEL_XOUT_H; + uint8_t ACCEL_XOUT_L; + uint8_t ACCEL_YOUT_H; + uint8_t ACCEL_YOUT_L; + uint8_t ACCEL_ZOUT_H; + uint8_t ACCEL_ZOUT_L; + uint8_t TEMP_OUT_H; + uint8_t TEMP_OUT_L; + uint8_t GYRO_XOUT_H; + uint8_t GYRO_XOUT_L; + uint8_t GYRO_YOUT_H; + uint8_t GYRO_YOUT_L; + uint8_t GYRO_ZOUT_H; + uint8_t GYRO_ZOUT_L; +}; +static_assert(sizeof(DATA) == 14); +} + +} // namespace InvenSense_ICM20602 diff --git a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp new file mode 100644 index 000000000000..3a11950c059e --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "ICM20602.hpp" + +#include + +namespace icm20602 +{ +ICM20602 *g_dev{nullptr}; + +static int start(enum Rotation rotation) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return 0; + } + + // create the driver +#if defined(PX4_SPI_BUS_SENSORS) + g_dev = new ICM20602(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ICM_20602, rotation); +#elif defined(PX4_SPI_BUS_SENSORS1) + g_dev = new ICM20602(PX4_SPI_BUS_SENSORS1, PX4_SPIDEV_ICM_20602, rotation); +#endif + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return -1; + } + + if (!g_dev->Init()) { + PX4_ERR("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return -1; + } + + g_dev->Stop(); + delete g_dev; + g_dev = nullptr; + + return 0; +} + +static int reset() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return 0; + } + + return g_dev->Reset(); +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_INFO("driver not running"); + return 0; + } + + g_dev->PrintInfo(); + + return 0; +} + +static int usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); + + return 0; +} + +} // namespace icm20602 + +extern "C" int icm20602_main(int argc, char *argv[]) +{ + enum Rotation rotation = ROTATION_NONE; + int myoptind = 1; + int ch = 0; + const char *myoptarg = nullptr; + + // start options + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + return icm20602::usage(); + } + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return icm20602::start(rotation); + + } else if (!strcmp(verb, "stop")) { + return icm20602::stop(); + + } else if (!strcmp(verb, "status")) { + return icm20602::status(); + + } else if (!strcmp(verb, "reset")) { + return icm20602::reset(); + } + + return icm20602::usage(); +} diff --git a/src/drivers/imu/invensense/icm20608-g/CMakeLists.txt b/src/drivers/imu/invensense/icm20608-g/CMakeLists.txt new file mode 100644 index 000000000000..57560c6f1f55 --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2019 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__imu__invensense__icm20608g + MAIN icm20608g + COMPILE_FLAGS + SRCS + ICM20608G.cpp + ICM20608G.hpp + icm20608g_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp new file mode 100644 index 000000000000..0eb66b4193f8 --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp @@ -0,0 +1,442 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "ICM20608G.hpp" + +#include + +using namespace time_literals; +using namespace InvenSense_ICM20608G; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } + +static constexpr uint32_t _fifo_interval{1000}; // 1000 us / 1000 Hz interval +static constexpr uint32_t _gyro_rate{8000}; // 8 kHz gyro +static constexpr uint32_t _gyro_readings_per_sample{_fifo_interval / (1000000 / _gyro_rate)}; +static constexpr uint32_t _accel_rate{4000}; // 4 kHz accel +static constexpr uint32_t _accel_readings_per_sample{_fifo_interval / (1000000 / _accel_rate)}; + +ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) : + SPI(MODULE_NAME, nullptr, bus, device, SPIDEV_MODE3, SPI_SPEED), + ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())), + _px4_accel(get_device_id(), ORB_PRIO_VERY_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) +{ + set_device_type(DRV_ACC_DEVTYPE_ICM20608); + _px4_accel.set_device_type(DRV_ACC_DEVTYPE_ICM20608); + _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608); + + _px4_accel.set_sample_rate(_accel_rate); + _px4_gyro.set_sample_rate(_gyro_rate); + + _px4_accel.set_update_rate(1000000 / _fifo_interval); + _px4_gyro.set_update_rate(1000000 / _fifo_interval); +} + +ICM20608G::~ICM20608G() +{ + 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); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_count_perf); + perf_free(_drdy_interval_perf); +} + +int +ICM20608G::probe() +{ + uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != ICM20608G_WHOAMI) { + PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +bool +ICM20608G::Init() +{ + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init failed"); + return false; + } + + if (!Reset()) { + PX4_ERR("reset failed"); + 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; +} + +bool +ICM20608G::Reset() +{ + for (int i = 0; i < 5; i++) { + + // PWR_MGMT_1: Device Reset + // CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET); + usleep(1000); + + // PWR_MGMT_1: CLKSEL[2:0] must be set to 001 to achieve full gyroscope performance. + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); + usleep(1000); + + // ACCEL_CONFIG: Accel 16 G range + RegisterSetBits(Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G); + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16.0f * CONSTANTS_ONE_G); + + // GYRO_CONFIG: Gyro 2000 degrees/second + RegisterSetBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS); + _px4_gyro.set_scale(math::radians(1.0f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + + const bool reset_done = !(RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::DEVICE_RESET); + const bool clksel_done = (RegisterRead(Register::PWR_MGMT_1) & PWR_MGMT_1_BIT::CLKSEL_0); + const bool data_ready = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::DATA_RDY_INT); + + // reset done once data is ready + if (reset_done && clksel_done && data_ready) { + return true; + } + } + + return false; +} + +void +ICM20608G::ResetFIFO() +{ + perf_count(_fifo_reset_perf); + + // ACCEL_CONFIG2: Accel DLPF disabled for full rate (4 kHz) + RegisterSetBits(Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF); + + // GYRO_CONFIG: Gyro DLPF disabled for full rate (8 kHz) + RegisterClearBits(Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF); + + // FIFO_EN: disable FIFO + RegisterWrite(Register::FIFO_EN, 0); + RegisterClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::FIFO_RST); + + // USER_CTRL: reset FIFO then re-enable + RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST); + up_udelay(1); // bit auto clears after one clock cycle of the internal 20 MHz clock + RegisterSetBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN); + + // CONFIG: should ensure that bit 7 of register 0x1A is set to 0 before using FIFO watermark feature + RegisterSetBits(Register::CONFIG, CONFIG_BIT::FIFO_MODE); + RegisterSetBits(Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ); + + // FIFO_EN: enable both gyro and accel + _data_ready_count = 0; + RegisterWrite(Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | + FIFO_EN_BIT::ACCEL_FIFO_EN); + up_udelay(10); +} + +uint8_t +ICM20608G::RegisterRead(Register reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void +ICM20608G::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void +ICM20608G::RegisterSetBits(Register reg, uint8_t setbits) +{ + uint8_t val = RegisterRead(reg); + + if (!(val & setbits)) { + val |= setbits; + RegisterWrite(reg, val); + } +} + +void +ICM20608G::RegisterClearBits(Register reg, uint8_t clearbits) +{ + uint8_t val = RegisterRead(reg); + + if (val & clearbits) { + val &= !clearbits; + RegisterWrite(reg, val); + } +} + +int +ICM20608G::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + ICM20608G *dev = reinterpret_cast(arg); + dev->DataReady(); + return 0; +} + +void +ICM20608G::DataReady() +{ + perf_count(_drdy_count_perf); + perf_count(_drdy_interval_perf); + + _data_ready_count++; + + if (_data_ready_count >= 8) { + _time_data_ready = hrt_absolute_time(); + + _data_ready_count = 0; + + // make another measurement + ScheduleNow(); + } +} + +void +ICM20608G::Start() +{ + Stop(); + + ResetFIFO(); + + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, true, false, true, &ICM20608G::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_DRDY_ICM_2060X) + // Setup data ready on rising edge + px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, true, false, true, &ICM20608G::DataReadyInterruptCallback, this); + RegisterSetBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#else + ScheduleOnInterval(_fifo_interval, _fifo_interval); +#endif +} + +void +ICM20608G::Stop() +{ + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_DRDY_PORTC_PIN14) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_DRDY_PORTC_PIN14, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#elif defined(GPIO_DRDY_ICM_2060X) + // Disable data ready callback + px4_arch_gpiosetevent(GPIO_DRDY_ICM_2060X, false, false, false, nullptr, nullptr); + RegisterClearBits(Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN); +#else + ScheduleClear(); +#endif + +} + +void +ICM20608G::Run() +{ + perf_count(_interval_perf); + + // check for FIFO overflow and reset + const bool fifo_overflow = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT); + + if (fifo_overflow) { + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // // check FIFO count + // uint8_t fifo_count_buf[3] {}; + // fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; + // const hrt_abstime timestamp_fifo_check = hrt_absolute_time(); + + // if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + // return; + // } + + // const size_t fifo_count = combine(fifo_count_buf[1], fifo_count_buf[2]); + // const int samples = fifo_count / sizeof(FIFO::DATA); + + // if (samples < 1) { + // perf_count(_fifo_empty_perf); + // return; + + // } else if (samples > 32) { + // // check for FIFO overflow and reset + // const bool fifo_overflow = (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT); + + // if (fifo_overflow) { + // perf_count(_fifo_overflow_perf); + // } + + // ResetFIFO(); + // return; + // } + + // only read 8 samples + const int samples = 8; + + // Transfer data + // 1 (cmd) + sizeof(FIFO_TRANSFER) * 32 = 449 bytes + struct ICM_Report { + uint8_t cmd; + FIFO::DATA f[32]; // we never transfer more than 32 samples + }; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + + ICM_Report *report = (ICM_Report *)_dma_data_buffer; + memset(report, 0, transfer_size); + report->cmd = static_cast(Register::FIFO_R_W) | DIR_READ; + + perf_begin(_transfer_perf); + + if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + return; + } + + perf_end(_transfer_perf); + + + // Process data (only the latest 8 samples) + const int num_samples = 8; + const int first_sample = math::min(0, num_samples - 8); + const int last_sample = num_samples; + + + static constexpr uint32_t gyro_dt = _fifo_interval / _gyro_readings_per_sample; + // estimate timestamp of first sample in the FIFO from number of samples and fill rate + const hrt_abstime timestamp_sample = _time_data_ready - ((num_samples - 1) * gyro_dt); + + PX4Accelerometer::FIFOSample accel{}; + accel.timestamp_sample = timestamp_sample; + accel.dt = _fifo_interval / _accel_readings_per_sample; + + PX4Gyroscope::FIFOSample gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = num_samples; + gyro.dt = _fifo_interval / _gyro_readings_per_sample; + + int accel_samples = 0; + + for (int i = first_sample; i < last_sample; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + + // accel data is doubled + if (i % 2) { + accel.x[accel_samples] = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L); + accel.y[accel_samples] = -combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L); + accel.z[accel_samples] = -combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L); + + accel_samples++; + } + + gyro.x[i] = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L); + gyro.y[i] = -combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L); + gyro.z[i] = -combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L); + } + + accel.samples = accel_samples; + + if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { + + // get current temperature + uint8_t temperature_buf[3] {}; + temperature_buf[0] = static_cast(Register::TEMP_OUT_H) | DIR_READ; + + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); + + // Room Temperature Offset 25°C + static constexpr float RoomTemp_Offset = 25.0f; + + // Sensitivity 326.8 LSB/°C + static constexpr float Temp_Sensitivity = 326.8f; + + const float TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 25.0f; + + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + } + + + _px4_gyro.updateFIFO(gyro); + _px4_accel.updateFIFO(accel); +} + +void +ICM20608G::PrintInfo() +{ + perf_print_counter(_interval_perf); + perf_print_counter(_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_count_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_accel.print_status(); + _px4_gyro.print_status(); +} diff --git a/src/drivers/imu/invensense/icm20608-g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608-g/ICM20608G.hpp new file mode 100644 index 000000000000..b3c7a04e9d0f --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.hpp @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 ICM20608g.hpp + * + * Driver for the Invensense ICM20608G connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM20608G_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using InvenSense_ICM20608G::Register; + +class ICM20608G : public device::SPI, public px4::ScheduledWorkItem +{ +public: + ICM20608G(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + virtual ~ICM20608G(); + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + + int probe() override; + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + + void Run() override; + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetBits(Register reg, uint8_t setbits); + void RegisterClearBits(Register reg, uint8_t clearbits); + + void ResetFIFO(); + + + uint8_t *_dma_data_buffer{nullptr}; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")}; + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": fifo reset")}; + perf_counter_t _drdy_count_perf{perf_alloc(PC_COUNT, MODULE_NAME": drdy count")}; + perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": drdy interval")}; + + hrt_abstime _time_data_ready{0}; + hrt_abstime _time_last_temperature_update{0}; + int _data_ready_count{0}; + +}; diff --git a/src/drivers/imu/invensense/icm20608-g/InvenSense_ICM20608G_registers.hpp b/src/drivers/imu/invensense/icm20608-g/InvenSense_ICM20608G_registers.hpp new file mode 100644 index 000000000000..5a3ca6d1a07f --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/InvenSense_ICM20608G_registers.hpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 InvenSense_ICM20608G_registers.hpp + * + * Invensense ICM-20608-G registers. + * + */ + +#pragma once + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace InvenSense_ICM20608G +{ + +static constexpr uint8_t DIR_READ = 0x80; +static constexpr uint8_t DIR_WRITE = 0x00; + +static constexpr uint8_t ICM20608G_WHOAMI = 0xAF; + +static constexpr uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface for communicating with all registers + +enum class +Register : uint8_t { + + CONFIG = 0x1A, + GYRO_CONFIG = 0x1B, + ACCEL_CONFIG = 0x1C, + ACCEL_CONFIG2 = 0x1D, + + FIFO_EN = 0x23, + + INT_STATUS = 0x3A, + + INT_PIN_CFG = 0x37, + INT_ENABLE = 0x38, + + TEMP_OUT_H = 0x41, + TEMP_OUT_L = 0x42, + + USER_CTRL = 0x6A, + PWR_MGMT_1 = 0x6B, + + FIFO_COUNTH = 0x72, + FIFO_COUNTL = 0x73, + FIFO_R_W = 0x74, + WHO_AM_I = 0x75, + +}; + +// CONFIG +enum +CONFIG_BIT : uint8_t { + FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO + + DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0] +}; + +// GYRO_CONFIG +enum +GYRO_CONFIG_BIT : uint8_t { + // FS_SEL [4:3] + FS_SEL_250_DPS = 0, // 0b00000 + FS_SEL_500_DPS = Bit3, // 0b01000 + FS_SEL_1000_DPS = Bit4, // 0b10000 + FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 + + // FCHOICE_B [1:0] + FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b10 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz +}; + +// ACCEL_CONFIG +enum +ACCEL_CONFIG_BIT : uint8_t { + // ACCEL_FS_SEL [4:3] + ACCEL_FS_SEL_2G = 0, // 0b00000 + ACCEL_FS_SEL_4G = Bit3, // 0b01000 + ACCEL_FS_SEL_8G = Bit4, // 0b10000 + ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000 +}; + +// ACCEL_CONFIG2 +enum +ACCEL_CONFIG2_BIT : uint8_t { + ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3, +}; + +// FIFO_EN +enum +FIFO_EN_BIT : uint8_t { + TEMP_FIFO_EN = Bit7, + XG_FIFO_EN = Bit6, + YG_FIFO_EN = Bit5, + ZG_FIFO_EN = Bit4, + ACCEL_FIFO_EN = Bit3, +}; + +// INT_ENABLE +enum +INT_ENABLE_BIT : uint8_t { + FIFO_OFLOW_EN = Bit4, + + DATA_RDY_INT_EN = Bit0 +}; + +// INT_STATUS +enum +INT_STATUS_BIT : uint8_t { + FIFO_OFLOW_INT = Bit4, + DATA_RDY_INT = Bit0, +}; + +// USER_CTRL +enum +USER_CTRL_BIT : uint8_t { + FIFO_EN = Bit6, + FIFO_RST = Bit2, +}; + +// PWR_MGMT_1 +enum +PWR_MGMT_1_BIT : uint8_t { + DEVICE_RESET = Bit7, + CLKSEL_2 = Bit2, + CLKSEL_1 = Bit1, + CLKSEL_0 = Bit0, +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 512; + +// FIFO_DATA layout when FIFO_EN has both {X, Y, Z}G_FIFO_EN and ACCEL_FIFO_EN set +struct DATA { + uint8_t ACCEL_XOUT_H; + uint8_t ACCEL_XOUT_L; + uint8_t ACCEL_YOUT_H; + uint8_t ACCEL_YOUT_L; + uint8_t ACCEL_ZOUT_H; + uint8_t ACCEL_ZOUT_L; + uint8_t GYRO_XOUT_H; + uint8_t GYRO_XOUT_L; + uint8_t GYRO_YOUT_H; + uint8_t GYRO_YOUT_L; + uint8_t GYRO_ZOUT_H; + uint8_t GYRO_ZOUT_L; +}; +static_assert(sizeof(DATA) == 12); +} + +} // namespace InvenSense_ICM20608G diff --git a/src/drivers/imu/invensense/icm20608-g/icm20608g_main.cpp b/src/drivers/imu/invensense/icm20608-g/icm20608g_main.cpp new file mode 100644 index 000000000000..347e44778e7a --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/icm20608g_main.cpp @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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 "ICM20608G.hpp" + +#include + +namespace icm20608g +{ +ICM20608G *g_dev{nullptr}; + +static int start(enum Rotation rotation) +{ + if (g_dev != nullptr) { + PX4_WARN("already started"); + return 0; + } + + // create the driver + g_dev = new ICM20608G(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ICM_20608, rotation); + + if (g_dev == nullptr) { + PX4_ERR("driver start failed"); + return -1; + } + + if (!g_dev->Init()) { + PX4_ERR("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -1; + } + + return 0; +} + +static int stop() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return -1; + } + + g_dev->Stop(); + delete g_dev; + g_dev = nullptr; + + return 0; +} + +static int reset() +{ + if (g_dev == nullptr) { + PX4_WARN("driver not running"); + return 0; + } + + return g_dev->Reset(); +} + +static int status() +{ + if (g_dev == nullptr) { + PX4_INFO("driver not running"); + return 0; + } + + g_dev->PrintInfo(); + + return 0; +} + +static int usage() +{ + PX4_INFO("missing command: try 'start', 'stop', 'reset', 'status'"); + PX4_INFO("options:"); + PX4_INFO(" -R rotation"); + + return 0; +} + +} // namespace icm20608g + +extern "C" int icm20608g_main(int argc, char *argv[]) +{ + enum Rotation rotation = ROTATION_NONE; + int myoptind = 1; + int ch = 0; + const char *myoptarg = nullptr; + + // start options + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'R': + rotation = (enum Rotation)atoi(myoptarg); + break; + + default: + return icm20608g::usage(); + } + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return icm20608g::start(rotation); + + } else if (!strcmp(verb, "stop")) { + return icm20608g::stop(); + + } else if (!strcmp(verb, "status")) { + return icm20608g::status(); + + } else if (!strcmp(verb, "reset")) { + return icm20608g::reset(); + } + + return icm20608g::usage(); +} diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 8b74a8122e11..c5ccc253327f 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -36,20 +36,23 @@ #include +using namespace time_literals; +using matrix::Vector3f; + PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) : CDev(nullptr), ModuleParams(nullptr), - _sensor_accel_pub{ORB_ID(sensor_accel), priority}, + _sensor_pub{ORB_ID(sensor_accel), priority}, + _sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority}, + _sensor_status_pub{ORB_ID(sensor_accel_status), priority}, + _device_id{device_id}, _rotation{rotation} { _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); - _sensor_accel_pub.get().device_id = device_id; - _sensor_accel_pub.get().scaling = 1.0f; - // set software low pass filter for controllers updateParams(); - configure_filter(_param_imu_accel_cutoff.get()); + ConfigureFilter(_param_imu_accel_cutoff.get()); } PX4Accelerometer::~PX4Accelerometer() @@ -68,14 +71,14 @@ PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) accel_calibration_s cal{}; memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal)); - _calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; - _calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; + _calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; + _calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; } return PX4_OK; case DEVIOCGDEVICEID: - return _sensor_accel_pub.get().device_id; + return _device_id; default: return -ENOTTY; @@ -87,45 +90,58 @@ PX4Accelerometer::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; - device_id.devid = _sensor_accel_pub.get().device_id; + device_id.devid = _device_id; // update to new device type device_id.devid_s.devtype = devtype; // copy back to report - _sensor_accel_pub.get().device_id = device_id.devid; + _device_id = device_id.devid; } void -PX4Accelerometer::set_sample_rate(unsigned rate) +PX4Accelerometer::set_sample_rate(uint16_t rate) { _sample_rate = rate; - _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); + + ConfigureFilter(_filter.get_cutoff_freq()); } void -PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) +PX4Accelerometer::set_update_rate(uint16_t rate) { - sensor_accel_s &report = _sensor_accel_pub.get(); - report.timestamp = timestamp; + const uint32_t update_interval = 1000000 / rate; + + _integrator_reset_samples = 4000 / update_interval; +} +void +PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) +{ // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); - const matrix::Vector3f raw{x, y, z}; + const Vector3f raw{x, y, z}; // Apply range scale and the calibrating offset/scale - const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))}; + const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; // Filtered values - const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)}; + const Vector3f val_filtered{_filter.apply(val_calibrated)}; // Integrated values - matrix::Vector3f integrated_value; + Vector3f integrated_value; uint32_t integral_dt = 0; if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) { + sensor_accel_s report{}; + report.timestamp = timestamp; + report.device_id = _device_id; + report.temperature = _temperature; + report.scaling = _scale; + report.error_count = _error_count; + // Raw values (ADC units 0 - 65535) report.x_raw = x; report.y_raw = y; @@ -140,9 +156,204 @@ PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z) report.y_integral = integrated_value(1); report.z_integral = integrated_value(2); - poll_notify(POLLIN); - _sensor_accel_pub.update(); + _sensor_pub.publish(report); + } +} + +void +PX4Accelerometer::updateFIFO(const FIFOSample &sample) +{ + // filtered data (control) + float x_filtered = _filterArrayX.apply(sample.x, sample.samples); + float y_filtered = _filterArrayY.apply(sample.y, sample.samples); + float z_filtered = _filterArrayZ.apply(sample.z, sample.samples); + + // Apply rotation (before scaling) + rotate_3f(_rotation, x_filtered, y_filtered, z_filtered); + + const Vector3f raw{x_filtered, y_filtered, z_filtered}; + + // Apply range scale and the calibrating offset/scale + const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; + + + // status + { + sensor_accel_status_s &status = _sensor_status_pub.get(); + + const int16_t clip_limit = (_range / _scale) * 0.9f; + + // x clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.x[n]) > clip_limit) { + status.clipping[0]++; + _integrator_clipping++; + } + } + + // y clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.y[n]) > clip_limit) { + status.clipping[1]++; + _integrator_clipping++; + } + } + + // z clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.z[n]) > clip_limit) { + status.clipping[2]++; + _integrator_clipping++; + } + } + + + // // Accel high frequency vibe = filtered length of (delta_velocity - prev_delta_velocity) + // for (int n = 1; n < sample.samples; n++) { + + // // float delta_vel = (fifo.x[n] - fifo.x[n-1]) / fifo.dt; // ADC units and time in microseconds + // matrix::Vector3f delta_velocity; + + // // calculate a metric which indicates the amount of high frequency accelerometer vibration + // const matrix::Vector3f temp = delta_velocity - _delta_velocity_prev; + + // _high_frequency_vibration = 0.99f * _high_frequency_vibration + 0.01f * temp.norm(); + + // _delta_velocity_prev = delta_velocity; + // } + status.high_frequency_vibration = _high_frequency_vibration; + + status.device_id = _device_id; + status.error_count = _error_count; + status.full_scale_range = _range; + status.rotation = _rotation; + status.measure_rate = _update_rate; + status.sample_rate = _sample_rate; + status.temperature = _temperature; + status.timestamp = hrt_absolute_time(); + _sensor_status_pub.publish(status); } + + + // integrated data (INS) + { + // reset integrator if previous sample was too long ago + if ((sample.timestamp_sample > _timestamp_sample_prev) + && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) { + + ResetIntegrator(); + } + + if (_integrator_samples == 0) { + _integrator_timestamp_sample = sample.timestamp_sample; + } + + // integrate + _integrator_samples += 1; + _integrator_fifo_samples += sample.samples; + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[0] += sample.x[n]; + } + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[1] += sample.y[n]; + } + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[2] += sample.z[n]; + } + + if (_integrator_samples >= _integrator_reset_samples) { + + const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds + + // average integrated values to apply calibration + float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples; + float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples; + float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples; + + // Apply rotation (before scaling) + rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg); + + const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg}; + + // Apply range scale and the calibrating offset/scale + Vector3f val_int_calibrated{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))}; + val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore + + // publish + sensor_accel_s report{}; + report.device_id = _device_id; + report.temperature = _temperature; + report.scaling = _scale; + report.error_count = _error_count; + + // Raw values (ADC units 0 - 65535) + report.x_raw = sample.x[0]; + report.y_raw = sample.y[0]; + report.z_raw = sample.z[0]; + + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + + report.integral_dt = integrator_dt_us; + report.integral_samples = _integrator_samples; + report.x_integral = val_int_calibrated(0); + report.y_integral = val_int_calibrated(1); + report.z_integral = val_int_calibrated(2); + report.integral_clip_count = _integrator_clipping; + + report.timestamp = _integrator_timestamp_sample; + _sensor_pub.publish(report); + + + // reset integrator + ResetIntegrator(); + } + + _timestamp_sample_prev = sample.timestamp_sample; + } + + sensor_accel_fifo_s fifo{}; + + fifo.device_id = _device_id; + fifo.timestamp_sample = sample.timestamp_sample; + fifo.dt = sample.dt; + fifo.scale = _scale; + fifo.samples = sample.samples; + + memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples); + memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples); + memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples); + + fifo.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(fifo); +} + +void +PX4Accelerometer::ResetIntegrator() +{ + _integrator_samples = 0; + _integrator_fifo_samples = 0; + _integrator_accum[0] = 0; + _integrator_accum[1] = 0; + _integrator_accum[2] = 0; + _integrator_clipping = 0; + + _integrator_timestamp_sample = 0; + _timestamp_sample_prev = 0; +} + +void +PX4Accelerometer::ConfigureFilter(float cutoff_freq) +{ + _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); + + _filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq); + _filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq); + _filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq); } void @@ -157,5 +368,4 @@ PX4Accelerometer::print_status() PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1), (double)_calibration_offset(2)); - print_message(_sensor_accel_pub.get()); } diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 5dbf15c2873e..ac01d0d61b5f 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -33,16 +33,19 @@ #pragma once -#include #include #include #include #include -#include +#include +#include +#include +#include #include -#include #include #include +#include +#include class PX4Accelerometer : public cdev::CDev, public ModuleParams { @@ -53,34 +56,87 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; + void set_device_id(uint32_t device_id) { _device_id = device_id; } + uint32_t get_device_id() const { return _device_id; } + void set_device_type(uint8_t devtype); - void set_error_count(uint64_t error_count) { _sensor_accel_pub.get().error_count = error_count; } - void set_scale(float scale) { _sensor_accel_pub.get().scaling = scale; } - void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; } - void set_sample_rate(unsigned rate); + void set_error_count(uint64_t error_count) { _error_count += error_count; } + void set_range(float range) { _range = range; } + void set_scale(float scale) { _scale = scale; } + void set_temperature(float temperature) { _temperature = temperature; } + + void set_sample_rate(uint16_t rate); + void set_update_rate(uint16_t rate); void update(hrt_abstime timestamp, float x, float y, float z); void print_status(); + struct FIFOSample { + hrt_abstime timestamp_sample; + uint8_t samples; // number of samples + float dt; // in microseconds + + int16_t x[4]; + int16_t y[4]; + int16_t z[4]; + }; + static_assert(sizeof(FIFOSample::x) == sizeof(sensor_accel_fifo_s::x), "FIFOSample.x invalid size"); + static_assert(sizeof(FIFOSample::y) == sizeof(sensor_accel_fifo_s::y), "FIFOSample.y invalid size"); + static_assert(sizeof(FIFOSample::z) == sizeof(sensor_accel_fifo_s::z), "FIFOSample.z invalid size"); + + void updateFIFO(const FIFOSample &sample); + private: - void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + void ConfigureFilter(float cutoff_freq); + void ResetIntegrator(); - uORB::PublicationMultiData _sensor_accel_pub; + uORB::PublicationMulti _sensor_pub; // legacy message + uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMultiData _sensor_status_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; - Integrator _integrator{4000, false}; - const enum Rotation _rotation; + math::LowPassFilter2pArray _filterArrayX{4000, 100}; + math::LowPassFilter2pArray _filterArrayY{4000, 100}; + math::LowPassFilter2pArray _filterArrayZ{4000, 100}; + + Integrator _integrator{4000, false}; matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f}; matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f}; int _class_device_instance{-1}; - unsigned _sample_rate{1000}; + + uint32_t _device_id{0}; + + const enum Rotation _rotation; + + float _range{16.0f * CONSTANTS_ONE_G}; + float _scale{1.0f}; + float _temperature{0.0f}; + + uint64_t _error_count{0}; + + uint16_t _sample_rate{1000}; + uint16_t _update_rate{1000}; + + // IMU vibration metrics + // high frequency vibration level in the IMU delta velocity data (m/s) + float _high_frequency_vibration{0.0f}; + matrix::Vector3f _delta_velocity_prev; + + // integrator + hrt_abstime _integrator_timestamp_sample{0}; + hrt_abstime _timestamp_sample_prev{0}; + int32_t _integrator_accum[3] {}; + uint8_t _integrator_reset_samples{4}; + uint8_t _integrator_samples{0}; + uint8_t _integrator_fifo_samples{0}; + uint8_t _integrator_clipping{0}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_accel_cutoff diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 07a11d07c95f..0be3a5998c65 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -36,22 +36,24 @@ #include +using namespace time_literals; +using matrix::Vector3f; + PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) : CDev(nullptr), ModuleParams(nullptr), - _sensor_gyro_pub{ORB_ID(sensor_gyro), priority}, - _sensor_gyro_control_pub{ORB_ID(sensor_gyro_control), priority}, + _sensor_pub{ORB_ID(sensor_gyro), priority}, + _sensor_control_pub{ORB_ID(sensor_gyro_control), priority}, + _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority}, + _sensor_status_pub{ORB_ID(sensor_gyro_status), priority}, + _device_id{device_id}, _rotation{rotation} { _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); - _sensor_gyro_pub.get().device_id = device_id; - _sensor_gyro_pub.get().scaling = 1.0f; - _sensor_gyro_control_pub.get().device_id = device_id; - // set software low pass filter for controllers updateParams(); - configure_filter(_param_imu_gyro_cutoff.get()); + ConfigureFilter(_param_imu_gyro_cutoff.get()); } PX4Gyroscope::~PX4Gyroscope() @@ -70,14 +72,14 @@ PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) gyro_calibration_s cal{}; memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal)); - _calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; - _calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; + _calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset}; + _calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale}; } return PX4_OK; case DEVIOCGDEVICEID: - return _sensor_gyro_pub.get().device_id; + return _device_id; default: return -ENOTTY; @@ -89,67 +91,82 @@ PX4Gyroscope::set_device_type(uint8_t devtype) { // current DeviceStructure union device::Device::DeviceId device_id; - device_id.devid = _sensor_gyro_pub.get().device_id; + device_id.devid = _device_id; // update to new device type device_id.devid_s.devtype = devtype; // copy back to report - _sensor_gyro_pub.get().device_id = device_id.devid; - _sensor_gyro_control_pub.get().device_id = device_id.devid; + _device_id = device_id.devid; } void -PX4Gyroscope::set_sample_rate(unsigned rate) +PX4Gyroscope::set_sample_rate(uint16_t rate) { _sample_rate = rate; - _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); + + ConfigureFilter(_filter.get_cutoff_freq()); } void -PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) +PX4Gyroscope::set_update_rate(uint16_t rate) { - sensor_gyro_s &report = _sensor_gyro_pub.get(); - report.timestamp = timestamp; + const uint32_t update_interval = 1000000 / rate; + + _integrator_reset_samples = 4000 / update_interval; +} +void +PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) +{ // Apply rotation (before scaling) rotate_3f(_rotation, x, y, z); - const matrix::Vector3f raw{x, y, z}; + const Vector3f raw{x, y, z}; // Apply range scale and the calibrating offset/scale - const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))}; + const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; // Filtered values - const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)}; + const Vector3f val_filtered{_filter.apply(val_calibrated)}; - // publish control data (filtered gyro) immediately + // publish control data (filtered) immediately bool publish_control = true; - sensor_gyro_control_s &control = _sensor_gyro_control_pub.get(); + sensor_gyro_control_s control{}; if (_param_imu_gyro_rate_max.get() > 0) { const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); - if (hrt_elapsed_time(&control.timestamp_sample) < interval) { + if (hrt_elapsed_time(&_control_last_publish) < interval) { publish_control = false; } } if (publish_control) { control.timestamp_sample = timestamp; + control.device_id = _device_id; val_filtered.copyTo(control.xyz); control.timestamp = hrt_absolute_time(); - _sensor_gyro_control_pub.update(); // publish + _sensor_control_pub.publish(control); + + _control_last_publish = control.timestamp_sample; } // Integrated values - matrix::Vector3f integrated_value; + Vector3f integrated_value; uint32_t integral_dt = 0; if (_integrator.put(timestamp, val_calibrated, integrated_value, integral_dt)) { + sensor_gyro_s report{}; + report.timestamp = timestamp; + report.device_id = _device_id; + report.temperature = _temperature; + report.scaling = _scale; + report.error_count = _error_count; + // Raw values (ADC units 0 - 65535) report.x_raw = x; report.y_raw = y; @@ -164,9 +181,236 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z) report.y_integral = integrated_value(1); report.z_integral = integrated_value(2); - poll_notify(POLLIN); - _sensor_gyro_pub.update(); // publish + _sensor_pub.publish(report); + } +} + +void +PX4Gyroscope::updateFIFO(const FIFOSample &sample) +{ + // filtered data (control) + float x_filtered = _filterArrayX.apply(sample.x, sample.samples); + float y_filtered = _filterArrayY.apply(sample.y, sample.samples); + float z_filtered = _filterArrayZ.apply(sample.z, sample.samples); + + // Apply rotation (before scaling) + rotate_3f(_rotation, x_filtered, y_filtered, z_filtered); + + const Vector3f raw{x_filtered, y_filtered, z_filtered}; + + // Apply range scale and the calibrating offset/scale + const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))}; + + + // control + { + // publish control data (filtered) immediately + bool publish_control = true; + sensor_gyro_control_s control{}; + + if (_param_imu_gyro_rate_max.get() > 0) { + const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + + if (hrt_elapsed_time(&_control_last_publish) < interval) { + publish_control = false; + } + } + + if (publish_control) { + control.timestamp_sample = sample.timestamp_sample + ((sample.samples - 1) * sample.dt); // timestamp of last sample + control.device_id = _device_id; + val_calibrated.copyTo(control.xyz); + control.timestamp = hrt_absolute_time(); + _sensor_control_pub.publish(control); + + _control_last_publish = control.timestamp_sample; + } } + + + // status + { + sensor_gyro_status_s &status = _sensor_status_pub.get(); + + const int16_t clip_limit = (_range / _scale) * 0.9f; + + // x clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.x[n]) > clip_limit) { + status.clipping[0]++; + _integrator_clipping++; + } + } + + // y clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.y[n]) > clip_limit) { + status.clipping[1]++; + _integrator_clipping++; + } + } + + // z clipping + for (int n = 0; n < sample.samples; n++) { + if (abs(sample.z[n]) > clip_limit) { + status.clipping[2]++; + _integrator_clipping++; + } + } + + + // // 0 : Gyro delta angle coning metric = filtered length of (delta_angle x prev_delta_angle) + // // 1 : Gyro high frequency vibe = filtered length of (delta_angle - prev_delta_angle) + // for (int n = 1; n < sample.samples; n++) { + // // TODO: create delta angle from fifo + // // float delta_vel = (fifo.x[n] - fifo.x[n-1]) / fifo.dt; // ADC units and time in microseconds + // const Vector3f delta_angle{0.0f, 0.0f, 0.0f}; + + // // calculate a metric which indicates the amount of coning vibration + // const Vector3f temp = delta_angle.cross(_delta_angle_prev); + // _delta_angle_coning_metric = 0.99f * _delta_angle_coning_metric + 0.01f * temp.norm(); + + // // calculate a metric which indicates the amount of high frequency gyro vibration + // const Vector3f temp2 = delta_angle - _delta_angle_prev; + + // _high_frequency_vibration = 0.99f * _high_frequency_vibration + 0.01f * temp2.norm(); + + // _delta_angle_prev = delta_angle; + // } + status.coning_vibration = _delta_angle_coning_metric; + status.high_frequency_vibration = _high_frequency_vibration; + + status.device_id = _device_id; + status.error_count = _error_count; + status.full_scale_range = _range; + status.rotation = _rotation; + status.measure_rate = _update_rate; + status.sample_rate = _sample_rate; + status.temperature = _temperature; + status.timestamp = hrt_absolute_time(); + _sensor_status_pub.publish(status); + } + + + // integrated data (INS) + { + // reset integrator if previous sample was too long ago + if ((sample.timestamp_sample > _timestamp_sample_prev) + && ((sample.timestamp_sample - _timestamp_sample_prev) > (sample.samples * sample.dt * 2))) { + + ResetIntegrator(); + } + + if (_integrator_samples == 0) { + _integrator_timestamp_sample = sample.timestamp_sample; + } + + // integrate + _integrator_samples += 1; + _integrator_fifo_samples += sample.samples; + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[0] += sample.x[n]; + } + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[1] += sample.y[n]; + } + + for (int n = 0; n < sample.samples; n++) { + _integrator_accum[2] += sample.z[n]; + } + + if (_integrator_samples >= _integrator_reset_samples) { + + const uint32_t integrator_dt_us = _integrator_fifo_samples * sample.dt; // time span in microseconds + + // average integrated values to apply calibration + float x_int_avg = _integrator_accum[0] / _integrator_fifo_samples; + float y_int_avg = _integrator_accum[1] / _integrator_fifo_samples; + float z_int_avg = _integrator_accum[2] / _integrator_fifo_samples; + + // Apply rotation (before scaling) + rotate_3f(_rotation, x_int_avg, y_int_avg, z_int_avg); + + const Vector3f raw_int{x_int_avg, y_int_avg, z_int_avg}; + + // Apply range scale and the calibrating offset/scale + Vector3f val_int_calibrated{(((raw_int * _scale) - _calibration_offset).emult(_calibration_scale))}; + val_int_calibrated *= (_integrator_fifo_samples * sample.dt * 1e-6f); // restore + + // publish + sensor_gyro_s report{}; + report.device_id = _device_id; + report.temperature = _temperature; + report.scaling = _scale; + report.error_count = _error_count; + + // Raw values (ADC units 0 - 65535) + report.x_raw = sample.x[0]; + report.y_raw = sample.y[0]; + report.z_raw = sample.z[0]; + + report.x = val_calibrated(0); + report.y = val_calibrated(1); + report.z = val_calibrated(2); + + report.integral_dt = integrator_dt_us; + report.integral_samples = _integrator_samples; + report.x_integral = val_int_calibrated(0); + report.y_integral = val_int_calibrated(1); + report.z_integral = val_int_calibrated(2); + report.integral_clip_count = _integrator_clipping; + + report.timestamp = _integrator_timestamp_sample; + _sensor_pub.publish(report); + + + // reset integrator + ResetIntegrator(); + } + + _timestamp_sample_prev = sample.timestamp_sample; + } + + sensor_gyro_fifo_s fifo{}; + + fifo.device_id = _device_id; + fifo.timestamp_sample = sample.timestamp_sample; + fifo.dt = sample.dt; + fifo.scale = _scale; + fifo.samples = sample.samples; + + memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * sample.samples); + memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * sample.samples); + memcpy(fifo.z, sample.z, sizeof(sample.z[0]) * sample.samples); + + fifo.timestamp = hrt_absolute_time(); + _sensor_fifo_pub.publish(fifo); +} + +void +PX4Gyroscope::ResetIntegrator() +{ + _integrator_samples = 0; + _integrator_fifo_samples = 0; + _integrator_accum[0] = 0; + _integrator_accum[1] = 0; + _integrator_accum[2] = 0; + _integrator_clipping = 0; + + _integrator_timestamp_sample = 0; + _timestamp_sample_prev = 0; +} + +void +PX4Gyroscope::ConfigureFilter(float cutoff_freq) +{ + _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); + + _filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq); + _filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq); + _filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq); } void @@ -181,6 +425,4 @@ PX4Gyroscope::print_status() PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1), (double)_calibration_offset(2)); - print_message(_sensor_gyro_pub.get()); - print_message(_sensor_gyro_control_pub.get()); } diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index eaf8ac295325..25a61b7cb5fd 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -33,17 +33,19 @@ #pragma once -#include #include #include #include #include -#include +#include +#include +#include #include -#include #include #include #include +#include +#include class PX4Gyroscope : public cdev::CDev, public ModuleParams { @@ -54,35 +56,92 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; + void set_device_id(uint32_t device_id) { _device_id = device_id; } + uint32_t get_device_id() const { return _device_id; } + void set_device_type(uint8_t devtype); - void set_error_count(uint64_t error_count) { _sensor_gyro_pub.get().error_count = error_count; } - void set_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; } - void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; } - void set_sample_rate(unsigned rate); + void set_error_count(uint64_t error_count) { _error_count += error_count; } + void set_range(float range) { _range = range; } + void set_scale(float scale) { _scale = scale; } + void set_temperature(float temperature) { _temperature = temperature; } + + void set_sample_rate(uint16_t rate); + void set_update_rate(uint16_t rate); void update(hrt_abstime timestamp, float x, float y, float z); void print_status(); + struct FIFOSample { + hrt_abstime timestamp_sample; + uint8_t samples; // number of samples + float dt; // in microseconds + + int16_t x[8]; + int16_t y[8]; + int16_t z[8]; + }; + static_assert(sizeof(FIFOSample::x) == sizeof(sensor_gyro_fifo_s::x), "FIFOSample.x invalid size"); + static_assert(sizeof(FIFOSample::y) == sizeof(sensor_gyro_fifo_s::y), "FIFOSample.y invalid size"); + static_assert(sizeof(FIFOSample::z) == sizeof(sensor_gyro_fifo_s::z), "FIFOSample.z invalid size"); + + void updateFIFO(const FIFOSample &sample); + private: - void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + void ConfigureFilter(float cutoff_freq); + void ResetIntegrator(); - uORB::PublicationMultiData _sensor_gyro_pub; - uORB::PublicationMultiData _sensor_gyro_control_pub; + uORB::PublicationMulti _sensor_pub; // legacy message + uORB::PublicationMulti _sensor_control_pub; + uORB::PublicationMulti _sensor_fifo_pub; + uORB::PublicationMultiData _sensor_status_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; - Integrator _integrator{4000, true}; - const enum Rotation _rotation; + hrt_abstime _control_last_publish{0}; + + math::LowPassFilter2pArray _filterArrayX{8000, 100}; + math::LowPassFilter2pArray _filterArrayY{8000, 100}; + math::LowPassFilter2pArray _filterArrayZ{8000, 100}; + + Integrator _integrator{4000, true}; matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f}; matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f}; int _class_device_instance{-1}; - unsigned _sample_rate{1000}; + + uint32_t _device_id{0}; + + const enum Rotation _rotation; + + float _range{math::radians(2000.0f)}; + float _scale{1.0f}; + float _temperature{0.0f}; + + uint64_t _error_count{0}; + + uint16_t _sample_rate{1000}; + uint16_t _update_rate{1000}; + + // IMU vibration metrics + // [0] Level of coning vibration in the IMU delta angles (rad^2) + // [1] high frequency vibration level in the IMU delta angle data (rad) + float _delta_angle_coning_metric{0.0f}; + float _high_frequency_vibration{0.0f}; + matrix::Vector3f _delta_angle_prev; + + // integrator + hrt_abstime _integrator_timestamp_sample{0}; + hrt_abstime _timestamp_sample_prev{0}; + int32_t _integrator_accum[3] {}; + uint8_t _integrator_reset_samples{4}; + uint8_t _integrator_samples{0}; + uint8_t _integrator_fifo_samples{0}; + uint8_t _integrator_clipping{0}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff, diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 6ba96573eb82..7d6c77caaed0 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -66,7 +66,7 @@ class __EXPORT LowPassFilter2p // Reset the filter state to this value float reset(float sample); -private: +protected: float _cutoff_freq{0.0f}; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp new file mode 100644 index 000000000000..7ce89829d05c --- /dev/null +++ b/src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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 LowPassFilter2pArray.hpp +/// @brief A class to implement a second order low pass filter + +#pragma once + +#include "LowPassFilter2p.hpp" + +namespace math +{ + +class LowPassFilter2pArray : public LowPassFilter2p +{ +public: + + LowPassFilter2pArray(float sample_freq, float cutoff_freq) : LowPassFilter2p(sample_freq, cutoff_freq) + { + } + + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + inline float apply(const int16_t samples[], uint8_t num_samples) + { + float output = 0.0f; + + for (int n = 0; n < num_samples; n++) { + // do the filtering + float delay_element_0 = samples[n] - _delay_element_1 * _a1 - _delay_element_2 * _a2; + + if (n == num_samples - 1) { + output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; + } + + _delay_element_2 = _delay_element_1; + _delay_element_1 = delay_element_0; + } + + // don't allow bad values to propagate via the filter + if (!PX4_ISFINITE(output)) { + reset(samples[num_samples - 1]); + output = samples[num_samples - 1]; + } + + // return the value. Should be no need to check limits + return output; + } + +}; + +} // namespace math diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b1ca8301ee8f..55103beeba38 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -553,6 +553,8 @@ void Logger::add_default_topics() add_topic_multi("actuator_outputs", 100); add_topic_multi("battery_status", 500); add_topic_multi("distance_sensor", 100); + add_topic_multi("sensor_accel_status", 1000); + add_topic_multi("sensor_gyro_status", 1000); add_topic_multi("telemetry_status"); add_topic_multi("vehicle_gps_position"); add_topic_multi("wind_estimate", 200); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d1a46bf70d34..d2e34b4e6e16 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -81,6 +81,7 @@ #include #include #include +#include #include #include #include @@ -2664,6 +2665,10 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream MavlinkOrbSubscription *_est_sub; uint64_t _est_time; + MavlinkOrbSubscription *_sensor_accel_status_0_sub; + MavlinkOrbSubscription *_sensor_accel_status_1_sub; + MavlinkOrbSubscription *_sensor_accel_status_2_sub; + /* do not allow top copying this class */ MavlinkStreamEstimatorStatus(MavlinkStreamEstimatorStatus &) = delete; MavlinkStreamEstimatorStatus &operator = (const MavlinkStreamEstimatorStatus &) = delete; @@ -2671,7 +2676,10 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream protected: explicit MavlinkStreamEstimatorStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), - _est_time(0) + _est_time(0), + _sensor_accel_status_0_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 0)), + _sensor_accel_status_1_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 1)), + _sensor_accel_status_2_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel_status), 2)) {} bool send(const hrt_abstime t) override @@ -2694,11 +2702,30 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream mavlink_msg_estimator_status_send_struct(_mavlink->get_channel(), &est_msg); // VIBRATION - mavlink_vibration_t msg = {}; + mavlink_vibration_t msg{}; msg.time_usec = est.timestamp; msg.vibration_x = est.vibe[0]; msg.vibration_y = est.vibe[1]; msg.vibration_z = est.vibe[2]; + + sensor_accel_status_s acc_status_0; + + if (_sensor_accel_status_0_sub->update(&acc_status_0)) { + msg.clipping_0 = acc_status_0.clipping[0] + acc_status_0.clipping[1] + acc_status_0.clipping[2]; + } + + sensor_accel_status_s acc_status_1; + + if (_sensor_accel_status_1_sub->update(&acc_status_1)) { + msg.clipping_1 = acc_status_1.clipping[0] + acc_status_1.clipping[1] + acc_status_1.clipping[2]; + } + + sensor_accel_status_s acc_status_2; + + if (_sensor_accel_status_2_sub->update(&acc_status_2)) { + msg.clipping_2 = acc_status_2.clipping[0] + acc_status_2.clipping[1] + acc_status_2.clipping[2]; + } + mavlink_msg_vibration_send_struct(_mavlink->get_channel(), &msg); return true;