From 957842f7ebf16268fad9fad3c7cd1e1af8de6cdf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 7 Jan 2021 14:34:38 +1100 Subject: [PATCH] AP_InertialSensor: added support for v3 Invensense sensors starting with ICM-40609, ICM-42688 and ICM-42605 --- .../AP_InertialSensor/AP_InertialSensor.cpp | 1 + .../AP_InertialSensor_Backend.h | 3 + .../AP_InertialSensor_Invensensev3.cpp | 373 ++++++++++++++++++ .../AP_InertialSensor_Invensensev3.h | 84 ++++ 4 files changed, 461 insertions(+) create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp create mode 100644 libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp index 1fecb71a11..3000669fa5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp @@ -26,6 +26,7 @@ #include "AP_InertialSensor_Invensensev2.h" #include "AP_InertialSensor_ADIS1647x.h" #include "AP_InertialSensor_ExternalAHRS.h" +#include "AP_InertialSensor_Invensensev3.h" /* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop. * Output is on the debug console. */ diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index bbb4dc506a..ec283b5429 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -118,6 +118,9 @@ public: DEVTYPE_INS_ICM20601 = 0x30, DEVTYPE_INS_ADIS1647X = 0x31, DEVTYPE_SERIAL = 0x32, + DEVTYPE_INS_ICM40609 = 0x33, + DEVTYPE_INS_ICM42688 = 0x34, + DEVTYPE_INS_ICM42605 = 0x35, }; protected: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp new file mode 100644 index 0000000000..7840466939 --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -0,0 +1,373 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + driver for Invensensev3 IMUs + + Supported: + ICM-40609 + ICM-42688 + ICM-42605 + + Note that this sensor includes 32kHz internal sampling and an + anti-aliasing filter, which means this driver can be a lot simpler + than the Invensense and Invensensev2 drivers which need to handle + 8kHz sample rates to achieve decent aliasing protection + + The sensor is a multi-bank design (4 banks) but as this driver only + needs access to the first bank and the default bank is the first + bank we can treat it as a single bank design + */ + +#include +#include "AP_InertialSensor_Invensensev3.h" +#include + +extern const AP_HAL::HAL& hal; + +/* + gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==0) + */ +static const float GYRO_SCALE = (0.0174532f / 16.4f); + +// set bit 0x80 in register ID for read on SPI +#define BIT_READ_FLAG 0x80 + +// registers we use +#define INV3REG_WHOAMI 0x75 +#define INV3REG_INT_CONFIG 0x14 +#define INV3REG_FIFO_CONFIG 0x16 +#define INV3REG_PWR_MGMT0 0x4e +#define INV3REG_GYRO_CONFIG0 0x4f +#define INV3REG_ACCEL_CONFIG0 0x50 +#define INV3REG_FIFO_CONFIG1 0x5f +#define INV3REG_FIFO_CONFIG2 0x60 +#define INV3REG_FIFO_CONFIG3 0x61 +#define INV3REG_INT_SOURCE0 0x65 +#define INV3REG_SIGNAL_PATH_RESET 0x4b +#define INV3REG_INTF_CONFIG0 0x4c +#define INV3REG_FIFO_COUNTH 0x2e +#define INV3REG_FIFO_DATA 0x30 +#define INV3REG_BANK_SEL 0x76 + +// WHOAMI values +#define INV3_ID_ICM40609 0x3b +#define INV3_ID_ICM42605 0x42 +#define INV3_ID_ICM42688 0x47 + +// run output data at 2kHz +#define INV3_ODR 2000 + +/* + really nice that this sensor has an option to request little-endian + data + */ +struct PACKED FIFOData { + uint8_t header; + int16_t accel[3]; + int16_t gyro[3]; + int8_t temperature; + uint16_t timestamp; +}; + +#define INV3_SAMPLE_SIZE sizeof(FIFOData) +#define INV3_FIFO_BUFFER_LEN 8 + +AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, + AP_HAL::OwnPtr _dev, + enum Rotation _rotation) + : AP_InertialSensor_Backend(imu) + , rotation(_rotation) + , dev(std::move(_dev)) +{ +} + +AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3() +{ + if (fifo_buffer != nullptr) { + hal.util->free_type((void*)fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + } +} + +AP_InertialSensor_Backend *AP_InertialSensor_Invensensev3::probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr _dev, + enum Rotation _rotation) +{ + if (!_dev) { + return nullptr; + } + + if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) { + _dev->set_read_flag(BIT_READ_FLAG); + } + + AP_InertialSensor_Invensensev3 *sensor = + new AP_InertialSensor_Invensensev3(imu, std::move(_dev), _rotation); + if (!sensor || !sensor->hardware_init()) { + delete sensor; + return nullptr; + } + + return sensor; +} + +void AP_InertialSensor_Invensensev3::fifo_reset() +{ + // FIFO_MODE stop-on-full + register_write(INV3REG_FIFO_CONFIG, 0x80); + // FIFO partial disable, enable accel, gyro, temperature + register_write(INV3REG_FIFO_CONFIG1, 0x07); + // little-endian, fifo count in records, last data hold for ODR mismatch + register_write(INV3REG_INTF_CONFIG0, 0xC0); + register_write(INV3REG_SIGNAL_PATH_RESET, 2); + + notify_accel_fifo_reset(accel_instance); + notify_gyro_fifo_reset(gyro_instance); +} + +void AP_InertialSensor_Invensensev3::start() +{ + WITH_SEMAPHORE(dev->get_semaphore()); + + // initially run the bus at low speed + dev->set_speed(AP_HAL::Device::SPEED_LOW); + + // always use FIFO + fifo_reset(); + + // grab the used instances + enum DevTypes devtype; + switch (inv3_type) { + case Invensensev3_Type::ICM42688: + devtype = DEVTYPE_INS_ICM42688; + break; + case Invensensev3_Type::ICM42605: + devtype = DEVTYPE_INS_ICM42605; + break; + case Invensensev3_Type::ICM40609: + default: + devtype = DEVTYPE_INS_ICM40609; + break; + } + + gyro_instance = _imu.register_gyro(INV3_ODR, dev->get_bus_id_devtype(devtype)); + accel_instance = _imu.register_accel(INV3_ODR, dev->get_bus_id_devtype(devtype)); + + // setup on-sensor filtering and scaling + set_filter_and_scaling(); + + // update backend sample rate + _set_accel_raw_sample_rate(accel_instance, INV3_ODR); + _set_gyro_raw_sample_rate(gyro_instance, INV3_ODR); + + // indicate what multiplier is appropriate for the sensors' + // readings to fit them into an int16_t: + _set_raw_sample_accel_multiplier(accel_instance, multiplier_accel); + + // now that we have initialised, we set the bus speed to high + dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + // setup sensor rotations from probe() + set_gyro_orientation(gyro_instance, rotation); + set_accel_orientation(accel_instance, rotation); + + // allocate fifo buffer + fifo_buffer = (FIFOData *)hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + if (fifo_buffer == nullptr) { + AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer"); + } + + // start the timer process to read samples + dev->register_periodic_callback(1e6 / INV3_ODR, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void)); +} + +/* + publish any pending data + */ +bool AP_InertialSensor_Invensensev3::update() +{ + update_accel(accel_instance); + update_gyro(gyro_instance); + _publish_temperature(accel_instance, temp_filtered); + + return true; +} + +/* + accumulate new samples + */ +void AP_InertialSensor_Invensensev3::accumulate() +{ + // nothing to do +} + +bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples) +{ + for (uint8_t i = 0; i < n_samples; i++) { + const FIFOData &d = data[i]; + + // we have a header to confirm we don't have FIFO corruption! no more mucking + // about with the temperature registers + if ((d.header & 0xF8) != 0x68) { + // no or bad data + return false; + } + + Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])}; + Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])}; + + accel *= accel_scale; + gyro *= GYRO_SCALE; + + const float temp = d.temperature * temp_sensitivity + temp_zero; + + _rotate_and_correct_accel(accel_instance, accel); + _rotate_and_correct_gyro(gyro_instance, gyro); + + _notify_new_accel_raw_sample(accel_instance, accel, 0); + _notify_new_gyro_raw_sample(gyro_instance, gyro); + + temp_filtered = temp_filter.apply(temp); + } + return true; +} + +/* + timer function called at ODR rate + */ +void AP_InertialSensor_Invensensev3::read_fifo() +{ + bool need_reset = false; + uint16_t n_samples; + + if (!block_read(INV3REG_FIFO_COUNTH, (uint8_t*)&n_samples, 2)) { + goto check_registers; + } + + if (n_samples == 0) { + /* Not enough data in FIFO */ + goto check_registers; + } + + while (n_samples > 0) { + uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); + if (!block_read(INV3REG_FIFO_DATA, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) { + goto check_registers; + } + + if (!accumulate_samples(fifo_buffer, n)) { + need_reset = true; + break; + } + n_samples -= n; + } + + if (need_reset) { + fifo_reset(); + } + +check_registers: + // check next register value for correctness + dev->set_speed(AP_HAL::Device::SPEED_LOW); + if (!dev->check_next_register()) { + _inc_gyro_error_count(gyro_instance); + _inc_accel_error_count(accel_instance); + } + dev->set_speed(AP_HAL::Device::SPEED_HIGH); +} + +bool AP_InertialSensor_Invensensev3::block_read(uint8_t reg, uint8_t *buf, uint32_t size) +{ + return dev->read_registers(reg, buf, size); +} + +uint8_t AP_InertialSensor_Invensensev3::register_read(uint8_t reg) +{ + uint8_t val = 0; + dev->read_registers(reg, &val, 1); + return val; +} + +void AP_InertialSensor_Invensensev3::register_write(uint8_t reg, uint8_t val, bool checked) +{ + dev->write_register(reg, val, checked); +} + +/* + set the filter frequencies and scaling + */ +void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void) +{ + // enable gyro and accel in low-noise modes + register_write(INV3REG_PWR_MGMT0, 0x0F); + hal.scheduler->delay_microseconds(300); + + // setup gyro for 2kHz + register_write(INV3REG_GYRO_CONFIG0, 0x05); + + // setup accel for 2kHz + register_write(INV3REG_ACCEL_CONFIG0, 0x05); +} + +/* + check whoami for sensor type + */ +bool AP_InertialSensor_Invensensev3::check_whoami(void) +{ + uint8_t whoami = register_read(INV3REG_WHOAMI); + switch (whoami) { + case INV3_ID_ICM40609: + inv3_type = Invensensev3_Type::ICM40609; + accel_scale = (GRAVITY_MSS / 1024); + return true; + case INV3_ID_ICM42688: + inv3_type = Invensensev3_Type::ICM42688; + accel_scale = (GRAVITY_MSS / 2048); + return true; + case INV3_ID_ICM42605: + inv3_type = Invensensev3_Type::ICM42605; + accel_scale = (GRAVITY_MSS / 2048); + return true; + } + // not a value WHOAMI result + return false; +} + +bool AP_InertialSensor_Invensensev3::hardware_init(void) +{ + WITH_SEMAPHORE(dev->get_semaphore()); + + dev->setup_checked_registers(7, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20); + + // initially run the bus at low speed + dev->set_speed(AP_HAL::Device::SPEED_LOW); + + if (!check_whoami()) { + return false; + } + + dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + switch (inv3_type) { + case Invensensev3_Type::ICM40609: + _clip_limit = 29.5f * GRAVITY_MSS; + break; + case Invensensev3_Type::ICM42688: + case Invensensev3_Type::ICM42605: + _clip_limit = 15.5f * GRAVITY_MSS; + break; + } + + return true; +} diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h new file mode 100644 index 0000000000..5b5a0698bf --- /dev/null +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -0,0 +1,84 @@ +#pragma once +/* + driver for the invensensev3 range of IMUs + These are the ICM-4 series of IMUs + */ + +#include + +#include +#include +#include +#include + +#include "AP_InertialSensor.h" +#include "AP_InertialSensor_Backend.h" + +class AP_InertialSensor_Invensensev3 : public AP_InertialSensor_Backend +{ +public: + virtual ~AP_InertialSensor_Invensensev3(); + + static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev, + enum Rotation rotation); + + /* update accel and gyro state */ + bool update() override; + void accumulate() override; + + void start() override; + + enum class Invensensev3_Type : uint8_t { + ICM40609 = 0, + ICM42688, + ICM42605, + }; + + // acclerometers on Invensense sensors will return values up to 32G + const uint16_t multiplier_accel = INT16_MAX/(32*GRAVITY_MSS); + +private: + AP_InertialSensor_Invensensev3(AP_InertialSensor &imu, + AP_HAL::OwnPtr dev, + enum Rotation rotation); + + /* Initialize sensor*/ + bool hardware_init(); + bool check_whoami(); + + void set_filter_and_scaling(void); + void fifo_reset(); + + /* Read samples from FIFO */ + void read_fifo(); + + bool block_read(uint8_t reg, uint8_t *buf, uint32_t size); + uint8_t register_read(uint8_t reg); + void register_write(uint8_t reg, uint8_t val, bool checked=false); + + bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples); + + // instance numbers of accel and gyro data + uint8_t gyro_instance; + uint8_t accel_instance; + + // temp scaling for FIFO temperature + const float temp_sensitivity = 1.0f/2.07; + const float temp_zero = 25; // degC + + const enum Rotation rotation; + + float accel_scale; + + AP_HAL::OwnPtr dev; + + // which sensor type this is + enum Invensensev3_Type inv3_type; + + // buffer for fifo read + struct FIFOData *fifo_buffer; + + float temp_filtered; + LowPassFilter2pFloat temp_filter; +};