diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 970ac1cf95..7821fdd894 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -53,7 +53,7 @@ px4_add_board( #pca9685 #protocol_splitter #pwm_input - pwm_out_sim + #pwm_out_sim px4fmu px4io #roboclaw diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 1550c0dc0a..7c523e2f15 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -28,6 +28,8 @@ px4_add_board( imu/adis16448 imu/adis16477 imu/adis16497 + imu/invensense/icm20602 + imu/invensense/icm20608-g imu/mpu6000 imu/mpu9250 irlock diff --git a/boards/px4/fmu-v4/init/rc.board_sensors b/boards/px4/fmu-v4/init/rc.board_sensors index 67d5fb1713..125b3a0d27 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 cb68f6e3fa..a4196e7a1f 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 0000000000..07befe094e --- /dev/null +++ b/boards/px4/fmu-v4/nuttx-config/include/board_dma_map.h @@ -0,0 +1,23 @@ +#pragma once + +/* DMA Channel/Stream Selections + * + * DMAMAP_USART3_RX = DMA1, Stream 1, Channel 4 + * DMAMAP_UART4_RX = DMA1, Stream 2, Channel 4 + * DMAMAP_UART7_RX = DMA1, Stream 3, Channel 5 + * DMAMAP_USART2_RX = DMA1, Stream 5, Channel 4 + * DMAMAP_TIM4_UP = DMA1, Stream 6, Channel 2 + * + *   DMAMAP_SPI1_RX_1 = DMA2, Stream 0, Channel 3 + *   DMAMAP_USART6_RX_1 = DMA2, Stream 1, Channel 4 + *   DMAMAP_USART1_RX_1 = DMA2, Stream 2, Channel 4 + *   DMAMAP_SPI1_TX_1 = DMA2, Stream 3, Channel 3 + * DMAMAP_TIM1_UP = DMA2, Stream 5, Channel 6 + *   DMAMAP_SDIO_2 = DMA2, Stream 6, Channel 4 + */ + +#define DMACHAN_SPI1_RX DMAMAP_SPI1_RX_1 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_1 +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_1 +#define DMACHAN_SPI1_TX DMAMAP_SPI1_TX_1 +#define DMAMAP_SDIO DMAMAP_SDIO_2 diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index dee3bcd7a3..edf54b3cf9 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 60aae744f3..beab6e326c 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 0c808db940..b22f3a0336 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 0c58cec706..d889b7ad65 100644 --- a/boards/px4/fmu-v4pro/default.cmake +++ b/boards/px4/fmu-v4pro/default.cmake @@ -30,6 +30,8 @@ px4_add_board( #imu # all available imu drivers imu/mpu6000 imu/mpu9250 + imu/invensense/icm20602 + imu/invensense/icm20608-g irlock lights/blinkm lights/rgbled diff --git a/boards/px4/fmu-v4pro/nuttx-config/include/board.h b/boards/px4/fmu-v4pro/nuttx-config/include/board.h index 59585d700a..7feb6c51be 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 0000000000..d1927aef0d --- /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 8b62313780..a534a9bf02 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 cdbb5584db..4430e92e7f 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/msg/sensor_accel_status.msg b/msg/sensor_accel_status.msg index d670906c7f..eb3a677a42 100644 --- a/msg/sensor_accel_status.msg +++ b/msg/sensor_accel_status.msg @@ -7,12 +7,9 @@ float32 temperature uint8 rotation -# clipping per axis? -uint64[3] clipping +uint64[3] clipping # clipping per axis uint16 measure_rate uint16 sample_rate float32 full_scale_range - -float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad) diff --git a/msg/sensor_gyro_status.msg b/msg/sensor_gyro_status.msg index dfa8521e2d..eb3a677a42 100644 --- a/msg/sensor_gyro_status.msg +++ b/msg/sensor_gyro_status.msg @@ -7,14 +7,9 @@ float32 temperature uint8 rotation -# clipping per axis? -uint64[3] clipping +uint64[3] clipping # clipping per axis uint16 measure_rate uint16 sample_rate float32 full_scale_range - -float32 coning_vibration # Level of coning vibration in the IMU delta angles (rad^2) - -float32 high_frequency_vibration # high frequency vibration level in the IMU delta angle data (rad) diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt new file mode 100644 index 0000000000..8b3fcf4050 --- /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 0000000000..97deb8c048 --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -0,0 +1,437 @@ +/**************************************************************************** + * + * 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 GYRO_RATE{8000}; // 8 kHz gyro +static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel + +static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval + +static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)}; +static constexpr uint32_t FIFO_ACCEL_SAMPLES{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() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_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); + + // read 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) / 2) * 2; // round down to nearest 2 + + if (samples < 2) { + perf_count(_fifo_empty_perf); + return; + + } else if (samples > 32) { + // not a real overflow, but something went wrong + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // check for FIFO overflow + if (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT) { + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // Transfer data + struct ICM_Report { + uint8_t cmd; + FIFO::DATA f[32]; // max 32 samples + }; + static_assert(sizeof(ICM_Report) == (sizeof(uint8_t) + 32 * sizeof(FIFO::DATA))); // ensure no struct padding + + ICM_Report *report = (ICM_Report *)_dma_data_buffer; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + memset(report, 0, transfer_size); + report->cmd = static_cast(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); + + static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; + // estimate timestamp of first sample in the FIFO from number of samples and fill rate + const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt); + + PX4Accelerometer::FIFOSample accel{}; + accel.timestamp_sample = timestamp_sample; + accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES; + + PX4Gyroscope::FIFOSample gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; + + int accel_samples = 0; + int16_t temperature[samples] {}; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + + // accel data is doubled + if (i % 2) { + // coordinate convention (x forward, y right, z down) + 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); + + // coordinate convention (x forward, y right, z down) + 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 an indication 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 0000000000..b7673bf93f --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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); + ~ICM20602() override; + + 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 0000000000..6378a0600f --- /dev/null +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * 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 uint32_t SPI_SPEED = 10 * 1000 * + 1000; // 10MHz SPI serial interface for communicating with all registers +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0x12; + +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_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 0000000000..3a11950c05 --- /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 0000000000..57560c6f1f --- /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 0000000000..bf825881d5 --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp @@ -0,0 +1,409 @@ +/**************************************************************************** + * + * 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 GYRO_RATE{8000}; // 8 kHz gyro +static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel + +static constexpr uint32_t FIFO_INTERVAL{1000}; // 1000 us / 1000 Hz interval + +static constexpr uint32_t FIFO_GYRO_SAMPLES{FIFO_INTERVAL / (1000000 / GYRO_RATE)}; +static constexpr uint32_t FIFO_ACCEL_SAMPLES{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() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != 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); + + // read 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) / 2) * 2; // round down to nearest 2 + + if (samples < 2) { + perf_count(_fifo_empty_perf); + return; + + } else if (samples > 32) { + // not a real overflow, but something went wrong + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // check for FIFO overflow + if (RegisterRead(Register::INT_STATUS) & INT_STATUS_BIT::FIFO_OFLOW_INT) { + perf_count(_fifo_overflow_perf); + ResetFIFO(); + return; + } + + // Transfer data + struct ICM_Report { + uint8_t cmd; + FIFO::DATA f[32]; // max 32 samples + }; + static_assert(sizeof(ICM_Report) == (sizeof(uint8_t) + 32 * sizeof(FIFO::DATA))); // ensure no struct padding + + ICM_Report *report = (ICM_Report *)_dma_data_buffer; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); + memset(report, 0, transfer_size); + report->cmd = static_cast(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); + + static constexpr uint32_t gyro_dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; + // estimate timestamp of first sample in the FIFO from number of samples and fill rate + const hrt_abstime timestamp_sample = _time_data_ready - ((samples - 1) * gyro_dt); + + PX4Accelerometer::FIFOSample accel{}; + accel.timestamp_sample = timestamp_sample; + accel.dt = FIFO_INTERVAL / FIFO_ACCEL_SAMPLES; + + PX4Gyroscope::FIFOSample gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_INTERVAL / FIFO_GYRO_SAMPLES; + + int accel_samples = 0; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + + // accel data is doubled + if (i % 2) { + // coordinate convention (x forward, y right, z down) + 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++; + } + + // coordinate convention (x forward, y right, z down) + 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 + if (hrt_elapsed_time(&_time_last_temperature_update) > 1_s) { + // read 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]); + + static constexpr float RoomTemp_Offset = 25.0f; // Room Temperature Offset 25°C + static constexpr float Temp_Sensitivity = 326.8f; // Sensitivity 326.8 LSB/°C + + 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 0000000000..3fe5a5de6a --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/ICM20608G.hpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * 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); + ~ICM20608G() override; + + 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 0000000000..af14c1e5fa --- /dev/null +++ b/src/drivers/imu/invensense/icm20608-g/InvenSense_ICM20608G_registers.hpp @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * 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 uint32_t SPI_SPEED = 8 * 1000 * 1000; // 8MHz SPI serial interface for communicating with all registers +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0xAF; + +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 0000000000..347e44778e --- /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/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fffbf0d8ec..d7beb8976c 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 @@ private: 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 @@ private: 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 @@ protected: 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;