Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: add high-resolution reads for ICM45686

Browse Source 
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
This commit is contained in:
Andy Piper 2023-09-20 18:42:17 +01:00 committed by Andrew Tridgell
parent fee43af729
commit da0b1402d3
3 changed files with 217 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -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<<instance)) != 0;
}
// should highres sampling be enabled on this IMU?
bool enable_highres_sampling(uint8_t instance) const {
return (HAL_INS_HIGHRES_SAMPLE & (1U<<instance)) != 0;
}
// if fast sampling is enabled, the rate to use in kHz
uint8_t get_fast_sampling_rate() const {
return (1 << uint8_t(_imu._fast_sampling_rate));

194
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -22,6 +22,7 @@
ICM-40605 - EOL
IIM-42652
ICM-42670
ICM-45686
Note that this sensor includes 32kHz internal sampling and an
anti-aliasing filter, which means this driver can be a lot simpler
@ -118,6 +119,7 @@ extern const AP_HAL::HAL& hal;
#define INV3REG_456_IREG_ADDRL 0x7D
#define INV3REG_456_IREG_DATA 0x7E
#define INV3REG_456_REG_MISC2 0x7F
#define INV3REG_456_SREG_CTRL 0x63
#define INV3BANK_456_IMEM_SRAM_ADDR 0x0000
#define INV3BANK_456_IPREG_BAR_ADDR 0xA000
@ -152,7 +154,21 @@ struct PACKED FIFOData {
uint16_t timestamp;
};
struct PACKED FIFODataHighRes {
uint8_t header;
uint8_t accel[6];
uint8_t gyro[6];
int16_t temperature;
uint16_t timestamp;
uint8_t gx : 4, ax : 4, gy : 4, ay : 4, gz : 4, az : 4;
};
#define INV3_SAMPLE_SIZE sizeof(FIFOData)
#define INV3_HIGHRES_SAMPLE_SIZE sizeof(FIFODataHighRes)
static_assert(sizeof(FIFOData) == 16);
static_assert(sizeof(FIFODataHighRes) == 20);
#define INV3_FIFO_BUFFER_LEN 8
AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
@ -166,8 +182,15 @@ AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor
AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3()
{
#if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) {
if (fifo_buffer != nullptr) {
hal.util->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);

49
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -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;