From 46e53645809c9e7c95a086ca8d80db66c76ec649 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 20 Mar 2020 13:01:02 -0400 Subject: [PATCH] InvenSense ICM-42688-P IMU driver This is a new InvenSense sensor with max output data rate of 32 kHz (both accel & gyro), on board anti-aliasing filter, optional higher resolution output (18 bit accel, 19 bit gyro), and clock sync. --- boards/modalai/fc-v1/default.cmake | 3 +- boards/modalai/fc-v1/init/rc.board_sensors | 2 +- boards/modalai/fc-v1/src/board_config.h | 11 +- boards/modalai/fc-v1/src/spi.cpp | 3 +- src/drivers/drv_sensor.h | 1 + .../imu/invensense/icm42688p/CMakeLists.txt | 46 ++ .../imu/invensense/icm42688p/ICM42688P.cpp | 670 ++++++++++++++++++ .../imu/invensense/icm42688p/ICM42688P.hpp | 174 +++++ .../InvenSense_ICM42688P_registers.hpp | 248 +++++++ .../invensense/icm42688p/icm42688p_main.cpp | 151 ++++ 10 files changed, 1299 insertions(+), 10 deletions(-) create mode 100644 src/drivers/imu/invensense/icm42688p/CMakeLists.txt create mode 100644 src/drivers/imu/invensense/icm42688p/ICM42688P.cpp create mode 100644 src/drivers/imu/invensense/icm42688p/ICM42688P.hpp create mode 100644 src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp create mode 100644 src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp diff --git a/boards/modalai/fc-v1/default.cmake b/boards/modalai/fc-v1/default.cmake index c0401fe5cb..7200eab389 100644 --- a/boards/modalai/fc-v1/default.cmake +++ b/boards/modalai/fc-v1/default.cmake @@ -25,7 +25,8 @@ px4_add_board( dshot gps imu/bmi088 -# TODO imu/icm42688 + imu/invensense/icm20602 + imu/invensense/icm42688p imu/mpu6000 irlock lights/blinkm diff --git a/boards/modalai/fc-v1/init/rc.board_sensors b/boards/modalai/fc-v1/init/rc.board_sensors index b2a59a1680..e419e7dcea 100644 --- a/boards/modalai/fc-v1/init/rc.board_sensors +++ b/boards/modalai/fc-v1/init/rc.board_sensors @@ -13,7 +13,7 @@ voxlpm -X -b 3 -T P5VDC start mpu6000 -R 6 -s -T 20602 start # Internal SPI bus ICM-42688 -# TODO +icm42688p -R 12 start # Internal SPI bus BMI088 accel bmi088 -A -R 4 start diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h index 0b53270f99..9329b85cdc 100644 --- a/boards/modalai/fc-v1/src/board_config.h +++ b/boards/modalai/fc-v1/src/board_config.h @@ -135,8 +135,8 @@ #define GPIO_SPI2_nCS1_ICM_42688 /* PH5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN5) /* Define the SPI2 Data Ready interrupts */ - -#define GPIO_SPI2_DRDY1_ICM_42688 /* PH12 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTH|GPIO_PIN12) +#define GPIO_SPI2_DRDY1_ICM_42688 /* PH12 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTH|GPIO_PIN12) // IMU_2_INT -> PH12 +#define GPIO_SPI2_DRDY2_ICM_42688 /* PA0 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTA|GPIO_PIN0) // IMU_2_FSYNC <- PA0/TIM5_CH1 /* SPI2 off */ @@ -144,8 +144,6 @@ #define GPIO_SPI2_MISO_OFF _PIN_OFF(GPIO_SPI2_MISO) #define GPIO_SPI2_MOSI_OFF _PIN_OFF(GPIO_SPI2_MOSI) -#define GPIO_DRDY_OFF_SPI2_DRDY1_ICM_42688 _PIN_OFF(GPIO_SPI2_DRDY1_ICM_42688) - /* SPI 5 CS */ #define GPIO_SPI5_nCS1_FRAM /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN7) @@ -162,7 +160,8 @@ /* Define the SPI6 Data Ready interrupts */ -#define GPIO_DRDY_OFF_SPI6_DRDY1_BMI088 _PIN_OFF(GPIO_SPI6_DRDY1_BMI088_INT1_ACCEL)#define GPIO_DRDY_OFF_SPI6_DRDY2_BMI088 _PIN_OFF(GPIO_SPI6_DRDY2_BMI088_INT3_GYRO) +#define GPIO_DRDY_OFF_SPI6_DRDY1_BMI088 _PIN_OFF(GPIO_SPI6_DRDY1_BMI088_INT1_ACCEL) +#define GPIO_DRDY_OFF_SPI6_DRDY2_BMI088 _PIN_OFF(GPIO_SPI6_DRDY2_BMI088_INT3_GYRO) /* SPI6 off */ @@ -186,7 +185,7 @@ #define PX4_SPIDEV_ICM_20602 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM20602) #define PX4_SENSORS1_BUS_CS_GPIO {GPIO_SPI1_nCS1_ICM20602} -#define PX4_SPIDEV_ICM_42688 PX4_MK_SPI_SEL(0,DRV_DEVTYPE_UNUSED) +#define PX4_SPIDEV_ICM_42688 PX4_MK_SPI_SEL(0,DRV_IMU_DEVTYPE_ICM42688P) #define PX4_SENSORS2_BUS_CS_GPIO {GPIO_SPI2_nCS1_ICM_42688} #define PX4_SPIDEV_MEMORY SPIDEV_FLASH(0) diff --git a/boards/modalai/fc-v1/src/spi.cpp b/boards/modalai/fc-v1/src/spi.cpp index dddc8e9665..33009a981a 100644 --- a/boards/modalai/fc-v1/src/spi.cpp +++ b/boards/modalai/fc-v1/src/spi.cpp @@ -40,8 +40,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }), initSPIBus(SPI::Bus::SPI2, { - // ICM-42688 - initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}), }), initSPIBus(SPI::Bus::SPI5, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 97c0f40c4a..8ceae70ccb 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -76,6 +76,7 @@ #define DRV_GYR_DEVTYPE_GYROSIM 0x23 #define DRV_IMU_DEVTYPE_MPU9250 0x24 #define DRV_GYR_DEVTYPE_BMI160 0x25 +#define DRV_IMU_DEVTYPE_ICM42688P 0x26 #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 diff --git a/src/drivers/imu/invensense/icm42688p/CMakeLists.txt b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt new file mode 100644 index 0000000000..42264f98d6 --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2020 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__icm42688p + MAIN icm42688p + COMPILE_FLAGS + SRCS + icm42688p_main.cpp + ICM42688P.cpp + ICM42688P.hpp + InvenSense_ICM42688P_registers.hpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + ) diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp new file mode 100644 index 0000000000..b137c537ad --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -0,0 +1,670 @@ +/**************************************************************************** + * + * 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 "ICM42688P.hpp" + +#include + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM42688P::ICM42688P(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_IMU_DEVTYPE_ICM42688P); + + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_ICM42688P); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ICM42688P); + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +ICM42688P::~ICM42688P() +{ + Stop(); + + if (_dma_data_buffer != nullptr) { + board_dma_free(_dma_data_buffer, FIFO::SIZE); + } + + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_interval_perf); +} + +bool ICM42688P::Init() +{ + if (SPI::init() != PX4_OK) { + PX4_ERR("SPI::init 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; + } + + return Reset(); +} + +void ICM42688P::Stop() +{ + // wait until stopped + while (_state.load() != STATE::STOPPED) { + _state.store(STATE::REQUEST_STOP); + ScheduleNow(); + px4_usleep(10); + } +} + +bool ICM42688P::Reset() +{ + _state.store(STATE::RESET); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ICM42688P::PrintInfo() +{ + PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, + static_cast(1000000 / _fifo_empty_interval_us)); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_accel.print_status(); + _px4_gyro.print_status(); +} + +int ICM42688P::probe() +{ + const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami != WHOAMI) { + PX4_WARN("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void ICM42688P::Run() +{ + switch (_state.load()) { + case STATE::RESET: + // DEVICE_CONFIG: Software reset + RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG); + _reset_timestamp = hrt_absolute_time(); + _state.store(STATE::WAIT_FOR_RESET); + ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { + // if reset succeeded then configure + _state.store(STATE::CONFIGURE); + ScheduleNow(); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { + PX4_ERR("Reset failed, retrying"); + _state.store(STATE::RESET); + ScheduleDelayed(10_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading from FIFO + _state.store(STATE::FIFO_READ); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + PX4_DEBUG("Configure failed, retrying"); + // try again in 10 ms + ScheduleDelayed(10_ms); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = 0; + uint8_t samples = 0; + + if (_data_ready_interrupt_enabled) { + // re-schedule as watchdog timeout + ScheduleDelayed(10_ms); + + // timestamp set in data ready interrupt + samples = _fifo_read_samples.load(); + timestamp_sample = _fifo_watermark_interrupt_timestamp; + } + + bool failure = false; + + // manually check FIFO count if no samples from DRDY or timestamp looks bogus + if (!_data_ready_interrupt_enabled || (samples == 0) + || (hrt_elapsed_time(×tamp_sample) > (_fifo_empty_interval_us / 2))) { + + // use the time now roughly corresponding with the last sample we'll pull from the FIFO + timestamp_sample = hrt_absolute_time(); + const uint16_t fifo_count = FIFOReadCount(); + + if (fifo_count == 0) { + failure = true; + perf_count(_fifo_empty_perf); + } + + samples = fifo_count / sizeof(FIFO::DATA); + } + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + perf_count(_fifo_overflow_perf); + failure = true; + FIFOReset(); + + } else if (samples >= 1) { + if (!FIFORead(timestamp_sample, samples)) { + failure = true; + _px4_accel.increase_error_count(); + _px4_gyro.increase_error_count(); + } + } + + if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) { + // check BANK_0 registers incrementally + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)) { + + _last_config_check_timestamp = timestamp_sample; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + + } else { + // register check failed, force reconfigure + PX4_DEBUG("Health check failed, reconfiguring"); + _state.store(STATE::CONFIGURE); + ScheduleNow(); + } + + } else { + // periodically update temperature (1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = timestamp_sample; + } + } + } + + break; + + case STATE::REQUEST_STOP: + DataReadyInterruptDisable(); + ScheduleClear(); + _state.store(STATE::STOPPED); + break; + + case STATE::STOPPED: + // DO NOTHING + break; + } +} + +void ICM42688P::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_0::ACCEL_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 ACCEL_FS_SEL + + switch (ACCEL_FS_SEL) { + case ACCEL_FS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _px4_accel.set_range(2 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_4G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192); + _px4_accel.set_range(4 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _px4_accel.set_range(8 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_16G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16 * CONSTANTS_ONE_G); + break; + } +} + +void ICM42688P::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_0::GYRO_CONFIG0) & (Bit7 | Bit6 | Bit5); // 7:5 GYRO_FS_SEL + + switch (GYRO_FS_SEL) { + case GYRO_FS_SEL_125_DPS: + _px4_gyro.set_scale(math::radians(1.0f / 262.f)); + _px4_gyro.set_range(math::radians(125.f)); + break; + + case GYRO_FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.f / 131.f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case GYRO_FS_SEL_500_DPS: + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case GYRO_FS_SEL_1000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.f)); + break; + + case GYRO_FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.0f)); + break; + } +} + +void ICM42688P::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 2000; // default to 2 kHz + } + + static constexpr float sample_min_interval = 1e6f / GYRO_RATE; + + _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / sample_min_interval) * sample_min_interval, + sample_min_interval); // round down to nearest sample_min_interval + + _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1000000 / GYRO_RATE); + + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); + + _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); + + // FIFO watermark threshold in number of bytes + const uint16_t fifo_watermark_threshold = _fifo_gyro_samples * sizeof(FIFO::DATA); + + for (auto &r : _register_bank0_cfg) { + if (r.reg == Register::BANK_0::FIFO_CONFIG2) { + // FIFO_WM[7:0] FIFO_CONFIG2 + r.set_bits = fifo_watermark_threshold & 0xFF; + + } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { + // FIFO_WM[11:8] FIFO_CONFIG3 + r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; + } + } +} + +bool ICM42688P::Configure() +{ + bool success = true; + + for (const auto ® : _register_bank0_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + ConfigureAccel(); + ConfigureGyro(); + + return success; +} + +int ICM42688P::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ICM42688P::DataReady() +{ + _fifo_watermark_interrupt_timestamp = hrt_absolute_time(); + _fifo_read_samples.store(_fifo_gyro_samples); + ScheduleNow(); + perf_count(_drdy_interval_perf); +} + +bool ICM42688P::DataReadyInterruptConfigure() +{ + int ret_setevent = -1; + + // Setup data ready on falling edge + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_SPI2_DRDY1_ICM_42688) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ICM_42688, false, true, true, + &ICM42688P::DataReadyInterruptCallback, this); +#endif + + return (ret_setevent == 0); +} + +bool ICM42688P::DataReadyInterruptDisable() +{ + int ret_setevent = -1; + + // Disable data ready callback + // TODO: cleanup horrible DRDY define mess +#if defined(GPIO_SPI2_DRDY1_ICM_42688) + ret_setevent = px4_arch_gpiosetevent(GPIO_SPI2_DRDY1_ICM_42688, false, false, false, nullptr, nullptr); +#endif + + return (ret_setevent == 0); +} + +bool ICM42688P::RegisterCheck(const register_bank0_config_t ®_cfg, bool notify) +{ + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + bool success = true; + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + if (!success) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + + if (notify) { + perf_count(_bad_register_perf); + _px4_accel.increase_error_count(); + _px4_gyro.increase_error_count(); + } + } + + return success; +} + +uint8_t ICM42688P::RegisterRead(Register::BANK_0 reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +void ICM42688P::RegisterWrite(Register::BANK_0 reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +void ICM42688P::RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = orig_val; + + if (setbits) { + val |= setbits; + } + + if (clearbits) { + val &= ~clearbits; + } + + RegisterWrite(reg, val); +} + +void ICM42688P::RegisterSetBits(Register::BANK_0 reg, uint8_t setbits) +{ + RegisterSetAndClearBits(reg, setbits, 0); +} + +void ICM42688P::RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits) +{ + RegisterSetAndClearBits(reg, 0, clearbits); +} + +uint16_t ICM42688P::FIFOReadCount() +{ + // read FIFO count + uint8_t fifo_count_buf[3] {}; + fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; + + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool ICM42688P::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) +{ + TransferBuffer *report = (TransferBuffer *)_dma_data_buffer; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); + memset(report, 0, transfer_size); + report->cmd = static_cast(Register::BANK_0::INT_STATUS) | DIR_READ; + + perf_begin(_transfer_perf); + + if (transfer(_dma_data_buffer, _dma_data_buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); + return false; + } + + perf_end(_transfer_perf); + + if (report->INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + } + + const uint16_t fifo_count_bytes = combine(report->FIFO_COUNTH, report->FIFO_COUNTL); + const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); + + if (fifo_count_samples == 0) { + perf_count(_fifo_empty_perf); + return false; + } + + // check FIFO header in every sample + int valid_samples = 0; + + for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + bool valid = true; + + // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx + const uint8_t FIFO_HEADER = report->f[i].FIFO_Header; + + if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) { + // FIFO sample empty if HEADER_MSG set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) { + // accel bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { + // gyro bit not set + valid = false; + } + + if (valid) { + valid_samples++; + + } else { + perf_count(_bad_transfer_perf); + break; + } + } + + if (valid_samples > 0) { + ProcessGyro(timestamp_sample, report, valid_samples); + ProcessAccel(timestamp_sample, report, valid_samples); + return true; + } + + return false; +} + +void ICM42688P::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // SIGNAL_PATH_RESET: FIFO flush + RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + + // reset while FIFO is disabled + _data_ready_count.store(0); + _fifo_watermark_interrupt_timestamp = 0; + _fifo_read_samples.store(0); +} + +void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples) +{ + PX4Accelerometer::FIFOSample accel; + accel.timestamp_sample = timestamp_sample; + accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + + int accel_samples = 0; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + int16_t accel_x = combine(fifo_sample.ACCEL_DATA_X1, fifo_sample.ACCEL_DATA_X0); + int16_t accel_y = combine(fifo_sample.ACCEL_DATA_Y1, fifo_sample.ACCEL_DATA_Y0); + int16_t accel_z = combine(fifo_sample.ACCEL_DATA_Z1, fifo_sample.ACCEL_DATA_Z0); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[accel_samples] = accel_x; + accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel_samples++; + } + + accel.samples = accel_samples; + + _px4_accel.updateFIFO(accel); +} + +void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const report, uint8_t samples) +{ + PX4Gyroscope::FIFOSample gyro; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; + + for (int i = 0; i < samples; i++) { + const FIFO::DATA &fifo_sample = report->f[i]; + + const int16_t gyro_x = combine(fifo_sample.GYRO_DATA_X1, fifo_sample.GYRO_DATA_X0); + const int16_t gyro_y = combine(fifo_sample.GYRO_DATA_Y1, fifo_sample.GYRO_DATA_Y0); + const int16_t gyro_z = combine(fifo_sample.GYRO_DATA_Z1, fifo_sample.GYRO_DATA_Z0); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + } + + _px4_gyro.updateFIFO(gyro); +} + +void ICM42688P::UpdateTemperature() +{ + // read current temperature + uint8_t temperature_buf[3] {}; + temperature_buf[0] = static_cast(Register::BANK_0::TEMP_DATA1) | DIR_READ; + + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_DATA = combine(temperature_buf[1], temperature_buf[2]); + + // Temperature in Degrees Centigrade = (TEMP_DATA / 132.48) + 25 + const float TEMP_degC = (TEMP_DATA / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + } +} diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp new file mode 100644 index 0000000000..5837cbe65e --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -0,0 +1,174 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 ICM42688P.hpp + * + * Driver for the Invensense ICM42688P connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM42688P_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_ICM42688P; + +class ICM42688P : public device::SPI, public px4::ScheduledWorkItem +{ +public: + ICM42688P(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE); + ~ICM42688P() override; + + bool Init(); + void Start(); + void Stop(); + bool Reset(); + void PrintInfo(); + +private: + + static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro + static constexpr uint32_t ACCEL_RATE{8000}; // 8 kHz accel + static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + + // Transfer data + struct TransferBuffer { + uint8_t cmd; + uint8_t INT_STATUS; + uint8_t FIFO_COUNTH; + uint8_t FIFO_COUNTL; + FIFO::DATA f[FIFO_MAX_SAMPLES]; + }; + // ensure no struct padding + static_assert(sizeof(TransferBuffer) == (4 * sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + void Run() override; + + bool Configure(); + void ConfigureAccel(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_bank0_config_t ®_cfg, bool notify = false); + uint8_t RegisterRead(Register::BANK_0 reg); + void RegisterWrite(Register::BANK_0 reg, uint8_t value); + void RegisterSetAndClearBits(Register::BANK_0 reg, uint8_t setbits, uint8_t clearbits); + void RegisterSetBits(Register::BANK_0 reg, uint8_t setbits); + void RegisterClearBits(Register::BANK_0 reg, uint8_t clearbits); + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); + void FIFOReset(); + + void ProcessAccel(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const TransferBuffer *const buffer, uint8_t samples); + void UpdateTemperature(); + + uint8_t *_dma_data_buffer{nullptr}; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad 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_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _fifo_watermark_interrupt_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + + px4::atomic _data_ready_count{0}; + px4::atomic _fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + REQUEST_STOP, + STOPPED, + }; + + px4::atomic _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{500}; // default 500 us / 2000 Hz transfer interval + uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{11}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, + { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, 0 }, + { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime + { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime + { Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, + { Register::BANK_0::INT_CONFIG1, INT_CONFIG1_BIT::INT_TPULSE_DURATION, 0 }, + { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + }; + +}; diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp new file mode 100644 index 0000000000..307af53ddb --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -0,0 +1,248 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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_ICM42688P_registers.hpp + * + * Invensense ICM-42688-P registers. + * + */ + +#pragma once + +#include + +// 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_ICM42688P +{ +static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0x47; + +static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C +static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C + +namespace Register +{ + +enum class BANK_0 : uint8_t { + DEVICE_CONFIG = 0x11, + + INT_CONFIG = 0x14, + + FIFO_CONFIG = 0x16, + + TEMP_DATA1 = 0x1D, + TEMP_DATA0 = 0x1E, + + INT_STATUS = 0x2D, + FIFO_COUNTH = 0x2E, + FIFO_COUNTL = 0x2F, + FIFO_DATA = 0x30, + + SIGNAL_PATH_RESET = 0x4B, + + PWR_MGMT0 = 0x4E, + GYRO_CONFIG0 = 0x4F, + ACCEL_CONFIG0 = 0x50, + + FIFO_CONFIG1 = 0x5F, + FIFO_CONFIG2 = 0x60, + FIFO_CONFIG3 = 0x61, + + INT_CONFIG0 = 0x63, + INT_CONFIG1 = 0x64, + INT_SOURCE0 = 0x65, + + WHO_AM_I = 0x75, + REG_BANK_SEL = 0x76, +}; + +}; + +//---------------- BANK0 Register bits + +// DEVICE_CONFIG +enum DEVICE_CONFIG_BIT : uint8_t { + SOFT_RESET_CONFIG = Bit0, // +}; + +// INT_CONFIG +enum INT_CONFIG_BIT : uint8_t { + INT1_MODE = Bit2, + INT1_DRIVE_CIRCUIT = Bit1, + INT1_POLARITY = Bit0, +}; + +// FIFO_CONFIG +enum FIFO_CONFIG_BIT : uint8_t { + // 7:6 FIFO_MODE + FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode +}; + +// INT_STATUS +enum INT_STATUS_BIT : uint8_t { + RESET_DONE_INT = Bit4, + DATA_RDY_INT = Bit3, + FIFO_THS_INT = Bit2, + FIFO_FULL_INT = Bit1, +}; + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + ABORT_AND_RESET = Bit3, + FIFO_FLUSH = Bit1, +}; + +// PWR_MGMT0 +enum PWR_MGMT0_BIT : uint8_t { + GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode + ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode +}; + +// GYRO_CONFIG0 +enum GYRO_CONFIG0_BIT : uint8_t { + // 7:5 GYRO_FS_SEL + GYRO_FS_SEL_2000_DPS = 0, // 0b000 = ±2000 dps + GYRO_FS_SEL_1000_DPS = Bit5, // 0b001 = ±1000 dps + GYRO_FS_SEL_500_DPS = Bit6, // 0b010 = ±500 dps + GYRO_FS_SEL_250_DPS = Bit6 | Bit5, // 0b011 = ±250 dps + GYRO_FS_SEL_125_DPS = Bit7, // 0b100 = ±125 dps + + // 3:0 GYRO_ODR + GYRO_ODR_32kHz = Bit0, // 0001: 32kHz + GYRO_ODR_16kHz = Bit1, // 0010: 16kHz + GYRO_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz + GYRO_ODR_4kHz = Bit2, // 0100: 4kHz + GYRO_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz + GYRO_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) +}; + +// ACCEL_CONFIG0 +enum ACCEL_CONFIG0_BIT : uint8_t { + // 7:5 ACCEL_FS_SEL + ACCEL_FS_SEL_16G = 0, // 000: ±16g (default) + ACCEL_FS_SEL_8G = Bit5, // 001: ±8g + ACCEL_FS_SEL_4G = Bit6, // 010: ±4g + ACCEL_FS_SEL_2G = Bit6 | Bit5, // 011: ±2g + + // 3:0 ACCEL_ODR + ACCEL_ODR_32kHz = Bit0, // 0001: 32kHz + ACCEL_ODR_16kHz = Bit1, // 0010: 16kHz + ACCEL_ODR_8kHz = Bit1 | Bit0, // 0011: 8kHz + ACCEL_ODR_4kHz = Bit2, // 0100: 4kHz + ACCEL_ODR_2kHz = Bit2 | Bit0, // 0101: 2kHz + ACCEL_ODR_1kHz = Bit2 | Bit1, // 0110: 1kHz (default) +}; + +// FIFO_CONFIG1 +enum FIFO_CONFIG1_BIT : uint8_t { + FIFO_RESUME_PARTIAL_RD = Bit6, + FIFO_WM_GT_TH = Bit5, + + FIFO_TEMP_EN = Bit2, + FIFO_GYRO_EN = Bit1, + FIFO_ACCEL_EN = Bit0, +}; + +// INT_CONFIG0 +enum INT_CONFIG0_BIT : uint8_t { + // 3:2 FIFO_THS_INT_CLEAR + CLEAR_ON_FIFO_READ = Bit3, +}; + +// INT_CONFIG1 +enum INT_CONFIG1_BIT : uint8_t { + INT_TPULSE_DURATION = Bit6, // 0: Interrupt pulse duration is 100μs +}; + +// INT_SOURCE0 +enum INT_SOURCE0_BIT : uint8_t { + UI_FSYNC_INT1_EN = Bit6, + PLL_RDY_INT1_EN = Bit5, + RESET_DONE_INT1_EN = Bit4, + UI_DRDY_INT1_EN = Bit3, + FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 + FIFO_FULL_INT1_EN = Bit1, + UI_AGC_RDY_INT1_EN = Bit0, +}; + +namespace FIFO +{ +static constexpr size_t SIZE = 2048; + +// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set + +// Packet 3 +struct DATA { + uint8_t FIFO_Header; + uint8_t ACCEL_DATA_X1; + uint8_t ACCEL_DATA_X0; + uint8_t ACCEL_DATA_Y1; + uint8_t ACCEL_DATA_Y0; + uint8_t ACCEL_DATA_Z1; + uint8_t ACCEL_DATA_Z0; + uint8_t GYRO_DATA_X1; + uint8_t GYRO_DATA_X0; + uint8_t GYRO_DATA_Y1; + uint8_t GYRO_DATA_Y0; + uint8_t GYRO_DATA_Z1; + uint8_t GYRO_DATA_Z0; + uint8_t temperature; // Temperature[7:0] + uint8_t timestamp_l; + uint8_t timestamp_h; +}; + +// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx +enum FIFO_HEADER_BIT : uint8_t { + HEADER_MSG = Bit7, // 1: FIFO is empty + HEADER_ACCEL = Bit6, + HEADER_GYRO = Bit5, + HEADER_20 = Bit4, + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, + HEADER_ODR_ACCEL = Bit1, + HEADER_ODR_GYRO = Bit0, +}; + +} +} // namespace InvenSense_ICM42688P diff --git a/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp new file mode 100644 index 0000000000..9dd15e881b --- /dev/null +++ b/src/drivers/imu/invensense/icm42688p/icm42688p_main.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "ICM42688P.hpp" + +#include + +namespace icm42688p +{ +ICM42688P *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_SENSORS2) && defined(PX4_SPIDEV_ICM_42688) + g_dev = new ICM42688P(PX4_SPI_BUS_SENSORS2, PX4_SPIDEV_ICM_42688, 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 icm42688p + +extern "C" int icm42688p_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 icm42688p::usage(); + } + } + + const char *verb = argv[myoptind]; + + if (!strcmp(verb, "start")) { + return icm42688p::start(rotation); + + } else if (!strcmp(verb, "stop")) { + return icm42688p::stop(); + + } else if (!strcmp(verb, "status")) { + return icm42688p::status(); + + } else if (!strcmp(verb, "reset")) { + return icm42688p::reset(); + } + + return icm42688p::usage(); +}