From da0b1402d3bf321d80e199b6710c20e80fa33a43 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Wed, 20 Sep 2023 18:42:17 +0100 Subject: [PATCH] AP_InertialSensor: add high-resolution reads for ICM45686 enable high-resolution support on all ICM4xxxx sensors create INS_HIRES_SAMPLE mask adjust high-resolution sampling for correct byte ordering and depth correct high resolution scaling on 18bit ICM4xxxx IMUs control highres via HAL_INS_HIGHRES_SAMPLE --- .../AP_InertialSensor_Backend.h | 11 +- .../AP_InertialSensor_Invensensev3.cpp | 194 +++++++++++++++--- .../AP_InertialSensor_Invensensev3.h | 49 ++++- 3 files changed, 217 insertions(+), 37 deletions(-) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h index 4e43b1d55c..880571ee4f 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h @@ -29,6 +29,10 @@ #include "AP_InertialSensor.h" +#ifndef HAL_INS_HIGHRES_SAMPLE +#define HAL_INS_HIGHRES_SAMPLE 0 +#endif + class AuxiliaryBus; class AP_Logger; @@ -284,10 +288,15 @@ protected: } // should fast sampling be enabled on this IMU? - bool enable_fast_sampling(uint8_t instance) { + bool enable_fast_sampling(uint8_t instance) const { return (_imu._fast_sampling_mask & (1U<free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + } + } else +#endif if (fifo_buffer != nullptr) { - hal.util->free_type((void*)fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); } } @@ -223,22 +246,20 @@ void AP_InertialSensor_Invensensev3::start() // initially run the bus at low speed dev->set_speed(AP_HAL::Device::SPEED_LOW); - // grab the used instances enum DevTypes devtype; + fifo_config1 = 0x07; + switch (inv3_type) { case Invensensev3_Type::IIM42652: devtype = DEVTYPE_INS_IIM42652; - fifo_config1 = 0x07; temp_sensitivity = 1.0 / 2.07; break; case Invensensev3_Type::ICM42688: devtype = DEVTYPE_INS_ICM42688; - fifo_config1 = 0x07; temp_sensitivity = 1.0 / 2.07; break; case Invensensev3_Type::ICM42605: devtype = DEVTYPE_INS_ICM42605; - fifo_config1 = 0x07; temp_sensitivity = 1.0 / 2.07; break; case Invensensev3_Type::ICM40605: @@ -253,15 +274,47 @@ void AP_InertialSensor_Invensensev3::start() case Invensensev3_Type::ICM45686: devtype = DEVTYPE_INS_ICM45686; temp_sensitivity = 1.0 / 2.0; + gyro_scale = GYRO_SCALE_4000DPS; + accel_scale = ACCEL_SCALE_32G; break; case Invensensev3_Type::ICM40609: default: devtype = DEVTYPE_INS_ICM40609; temp_sensitivity = 1.0 / 2.07; - fifo_config1 = 0x07; + accel_scale = ACCEL_SCALE_32G; break; } +#if HAL_INS_HIGHRES_SAMPLE + // now we know who we are, other things can be checked for + if (enable_highres_sampling(accel_instance)) { + switch (inv3_type) { + case Invensensev3_Type::ICM42688: // HiRes 19bit + case Invensensev3_Type::IIM42652: // HiRes 19bit + case Invensensev3_Type::ICM42670: // HiRes 19bit + case Invensensev3_Type::ICM45686: // HiRes 20bit + highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; + break; + default: + break; + } + } + + // optionally enable high resolution mode + if (highres_sampling) { + fifo_config1 |= (1U<<4); // FIFO_HIRES_EN + gyro_scale = GYRO_SCALE_HIGHRES_2000DPS; + accel_scale = ACCEL_SCALE_HIGHRES_16G; + temp_sensitivity = 1.0 / 132.48; + if (inv3_type == Invensensev3_Type::ICM45686) { + temp_sensitivity = 1.0 / 128.0; + accel_scale = ACCEL_SCALE_HIGHRES_32G; + gyro_scale = GYRO_SCALE_HIGHRES_4000DPS; + } else if (inv3_type == Invensensev3_Type::ICM42670) { + temp_sensitivity = 1.0 / 128.0; + } + } +#endif // always use FIFO fifo_reset(); @@ -298,7 +351,13 @@ void AP_InertialSensor_Invensensev3::start() 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 HAL_INS_HIGHRES_SAMPLE + if (highres_sampling) { + fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE); + } else +#endif + fifo_buffer = 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"); } @@ -310,8 +369,12 @@ void AP_InertialSensor_Invensensev3::start() // get a startup banner to output to the GCS bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) { if (fast_sampling) { - snprintf(banner, banner_len, "IMU%u: fast sampling enabled %.1fkHz", - gyro_instance, backend_rate_hz * 0.001); + snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz", + gyro_instance, +#if HAL_INS_HIGHRES_SAMPLE + highres_sampling ? ", high-resolution" : +#endif + "" , backend_rate_hz * 0.001); return true; } return false; @@ -347,7 +410,9 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui // we have a header to confirm we don't have FIFO corruption! no more mucking // about with the temperature registers - if ((d.header & 0xFC) != 0x68) { + // ICM45686 - TMST_FIELD_EN bit 3 : 1 + // ICM42688 - HEADER_TIMESTAMP_FSYNC bit 2-3 : 10 + if ((d.header & 0xFC) != 0x68) { // ACCEL_EN | GYRO_EN | TMST_FIELD_EN // no or bad data return false; } @@ -376,6 +441,63 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui return true; } +#if HAL_INS_HIGHRES_SAMPLE +// high-resolution packets are always 20-bits, but not always 20-bits of data. +// Scale factors account for the useless bits +static inline float uint20_to_float(uint8_t msb, uint8_t bits, uint8_t lsb) +{ + uint32_t value20bit = uint32_t(msb) << 12U | uint32_t(bits) << 4U | lsb; + int32_t value32bit; + // Check the sign bit (MSB) + if (value20bit & 0x80000) { // MSB is set (negative value) + // Extend the sign bit to the upper 12 bits of the 32-bit integer + value32bit = (int32_t)(value20bit | 0xFFF00000); + } else { // MSB is not set (positive value) + // Zero-fill the upper 12 bits of the 32-bit integer + value32bit = value20bit; + } + + return float(value32bit); +} + + +bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *data, uint8_t n_samples) +{ + for (uint8_t i = 0; i < n_samples; i++) { + const FIFODataHighRes &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 & 0xFC) != 0x78) { // ACCEL_EN | GYRO_EN | HIRES_EN | TMST_FIELD_EN + // no or bad data + return false; + } + + Vector3f accel{uint20_to_float(d.accel[1], d.accel[0], d.ax), + uint20_to_float(d.accel[3], d.accel[2], d.ay), + uint20_to_float(d.accel[5], d.accel[4], d.az)}; + Vector3f gyro{uint20_to_float(d.gyro[1], d.gyro[0], d.gx), + uint20_to_float(d.gyro[3], d.gyro[2], d.gy), + uint20_to_float(d.gyro[5], d.gyro[4], d.gz)}; + + accel *= accel_scale; + gyro *= gyro_scale; + + const float temp = d.temperature * temp_sensitivity + temp_zero; + + // these four calls are about 40us + _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; +} +#endif + /* timer function called at ODR rate */ @@ -402,6 +524,11 @@ void AP_InertialSensor_Invensensev3::read_fifo() break; } +#if HAL_INS_HIGHRES_SAMPLE + const uint8_t fifo_sample_size = highres_sampling ? INV3_HIGHRES_SAMPLE_SIZE : INV3_SAMPLE_SIZE; +#else + const uint8_t fifo_sample_size = INV3_SAMPLE_SIZE; +#endif if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) { goto check_registers; } @@ -417,11 +544,18 @@ void AP_InertialSensor_Invensensev3::read_fifo() while (n_samples > 0) { uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN); - if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) { + if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) { goto check_registers; } - - if (!accumulate_samples(fifo_buffer, n)) { +#if HAL_INS_HIGHRES_SAMPLE + if (highres_sampling) { + if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) { + need_reset = true; + break; + } + } else +#endif + if (!accumulate_samples((FIFOData*)fifo_buffer, n)) { need_reset = true; break; } @@ -716,7 +850,7 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) // Disable FIFO first register_write(INV3REG_456_FIFO_CONFIG3, 0x00); - register_write(INV3REG_456_FIFO_CONFIG0, 0x07); + register_write(INV3REG_456_FIFO_CONFIG0, 0x00); // setup gyro for 1.6-6.4kHz, 4000dps range register_write(INV3REG_456_GYRO_CONFIG0, (0x0 << 4) | odr_config); // GYRO_UI_FS_SEL b4-7, GYRO_ODR b0-3 @@ -732,11 +866,18 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) #endif register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58, reg | 0x01); + uint8_t fifo_config = (1U<<2 | 1U<<1); // FIFO_ACCEL_EN | FIFO_GYRO_EN, FIFO_IF_EN disabled +#if HAL_INS_HIGHRES_SAMPLE + // optionally enable high resolution mode + if (highres_sampling) { + fifo_config |= (1U<<3); // FIFO_HIRES_EN + } +#endif // enable FIFO for each sensor - register_write(INV3REG_456_FIFO_CONFIG3, 0x06, true); + register_write(INV3REG_456_FIFO_CONFIG3, fifo_config, true); - // FIFO stop-on-full, disable bypass and 2K FIFO - register_write(INV3REG_456_FIFO_CONFIG0, 0x87, true); + // FIFO enabled - stop-on-full, disable bypass and 2K FIFO + register_write(INV3REG_456_FIFO_CONFIG0, (2 << 6) | 0x07, true); // enable Interpolator and Anti Aliasing Filter on Gyro reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6); // GYRO_SRC_CTRL b5-6 @@ -746,8 +887,9 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B); // ACCEL_SRC_CTRL b0-1 register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (reg & ~0x3) | 0x2); - // enable FIFO - register_write(INV3REG_456_FIFO_CONFIG3, 0x07, true); + // enable FIFO sensor registers + fifo_config |= (1U<<0); // FIFO_IF_EN + register_write(INV3REG_456_FIFO_CONFIG3, fifo_config, true); } /* @@ -760,8 +902,6 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void) switch (whoami) { case INV3_ID_ICM40609: inv3_type = Invensensev3_Type::ICM40609; - // Accel scale 32g (1024 LSB/g) - accel_scale = (GRAVITY_MSS / 1024); return true; case INV3_ID_ICM42688: inv3_type = Invensensev3_Type::ICM42688; @@ -784,9 +924,6 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void) switch (whoami) { case INV3_ID_ICM45686: inv3_type = Invensensev3_Type::ICM45686; - gyro_scale = GYRO_SCALE_4000DPS; - // Accel scale 32g (1024 LSB/g) - accel_scale = (GRAVITY_MSS / 1024); return true; } // not a value WHOAMI result @@ -884,7 +1021,14 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void) register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_SENSOR_CONFIG3, 0x40); // use 16 bit data, gyro+accel - register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_FIFO_CONFIG5, 0x3); + uint8_t fifo_config = 0x03; +#if HAL_INS_HIGHRES_SAMPLE + // optionally enable high resolution mode + if (highres_sampling) { + fifo_config |= (1U<<3); // FIFO_HIRES_EN + } +#endif + register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_FIFO_CONFIG5, fifo_config); // FIFO stop-on-full, disable bypass register_write(INV3REG_70_FIFO_CONFIG1, 0x2, true); diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h index 410a455f47..aa9971db0d 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h @@ -32,13 +32,13 @@ public: bool get_output_banner(char* banner, uint8_t banner_len) override; enum class Invensensev3_Type : uint8_t { - ICM40609 = 0, - ICM42688, - ICM42605, - ICM40605, - IIM42652, - ICM42670, - ICM45686 + ICM40609 = 0, // No HiRes + ICM42688, // HiRes 19bit + ICM42605, // No HiRes + ICM40605, // No HiRes + IIM42652, // HiRes 19bit + ICM42670, // HiRes 19bit + ICM45686 // HiRes 20bit }; // acclerometers on Invensense sensors will return values up to 32G @@ -72,6 +72,7 @@ private: void register_write_bank_icm456xy(uint16_t bank_addr, uint16_t reg, uint8_t val); bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples); + bool accumulate_highres_samples(const struct FIFODataHighRes *data, uint8_t n_samples); // instance numbers of accel and gyro data uint8_t gyro_instance; @@ -86,20 +87,46 @@ private: const enum Rotation rotation; + static constexpr float SCALE_RANGE_16BIT = 32768; // 2^15; + // HiRes support is either 20bit (19bit accel) or 19bit (18bit accel) + static constexpr float SCALE_RANGE_20BIT = 524288; // 2^19; + static constexpr float SCALE_RANGE_19BIT = 262144; // 2^18; + /* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==0) */ - static constexpr float GYRO_SCALE_2000DPS = (0.0174532f / 16.4f); + static constexpr float GYRO_SCALE_2000DPS = radians(1) / (SCALE_RANGE_16BIT / 2000.0); /* gyro as 8.2 LSB/DPS at scale factor of +/- 4000dps (FS_SEL==0) */ - static constexpr float GYRO_SCALE_4000DPS = (0.0174532f / 8.2f); + static constexpr float GYRO_SCALE_4000DPS = radians(1) / (SCALE_RANGE_16BIT / 4000.0); + /* + highres gyro is always 131 LSB/DPS modified by the data size transmitted + */ + static constexpr float GYRO_SCALE_HIGHRES_2000DPS = radians(1) / (SCALE_RANGE_20BIT / 2000.0); + static constexpr float GYRO_SCALE_HIGHRES_4000DPS = radians(1) / (SCALE_RANGE_20BIT / 4000.0); + /* + Accel scale 16g (2048 LSB/g) + */ + static constexpr float ACCEL_SCALE_16G = (GRAVITY_MSS / (SCALE_RANGE_16BIT / 16)); + /* + Accel scale 32g (1024 LSB/g) + */ + static constexpr float ACCEL_SCALE_32G = (GRAVITY_MSS / (SCALE_RANGE_16BIT / 32)); + /* + highres accel is 16384 LSB/g on 45686 amd 8192 LSB/g on all others + */ + static constexpr float ACCEL_SCALE_HIGHRES_16G = (GRAVITY_MSS / (SCALE_RANGE_20BIT / 16)); + static constexpr float ACCEL_SCALE_HIGHRES_32G = (GRAVITY_MSS / (SCALE_RANGE_20BIT / 32)); - float accel_scale = (GRAVITY_MSS / 2048); + float accel_scale = ACCEL_SCALE_16G; float gyro_scale = GYRO_SCALE_2000DPS; // are we doing more than 1kHz sampling? bool fast_sampling; +#if HAL_INS_HIGHRES_SAMPLE + bool highres_sampling; +#endif // what rate are we generating samples into the backend for gyros and accels? uint16_t backend_rate_hz; @@ -113,7 +140,7 @@ private: enum Invensensev3_Type inv3_type; // buffer for fifo read - struct FIFOData *fifo_buffer; + void* fifo_buffer; float temp_filtered; LowPassFilter2pFloat temp_filter;