Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added support for ICM42670 IMU

Browse Source
This commit is contained in:
Andrew Tridgell 2022-08-02 07:58:59 +10:00 committed by Randy Mackay
parent aedc20e938
commit 5b6fc2f97d
3 changed files with 169 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -121,6 +121,8 @@ public:
DEVTYPE_INS_ICM40605 = 0x36,
DEVTYPE_INS_IIM42652 = 0x37,
DEVTYPE_BMI270 = 0x38,
DEVTYPE_INS_BMI085 = 0x39,
DEVTYPE_INS_ICM42670 = 0x3A,
};
protected:

191
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -21,6 +21,7 @@
ICM-42605
ICM-40605
IIM-42652
ICM-42670
Note that this sensor includes 32kHz internal sampling and an
anti-aliasing filter, which means this driver can be a lot simpler
@ -48,7 +49,6 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
// 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
@ -56,22 +56,42 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
#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
// registers for ICM-42670, multi-bank
#define INV3REG_70_PWR_MGMT0 0x1F
#define INV3REG_70_GYRO_CONFIG0 0x20
#define INV3REG_70_ACCEL_CONFIG0 0x21
#define INV3REG_70_FIFO_COUNTH 0x3D
#define INV3REG_70_FIFO_DATA 0x3F
#define INV3REG_70_INTF_CONFIG0 0x35
#define INV3REG_70_MCLK_RDY 0x00
#define INV3REG_70_SIGNAL_PATH_RESET 0x02
#define INV3REG_70_FIFO_CONFIG1 0x28
#define INV3REG_BLK_SEL_W 0x79
#define INV3REG_BLK_SEL_R 0x7C
#define INV3REG_MADDR_W 0x7A
#define INV3REG_MADDR_R 0x7D
#define INV3REG_M_W 0x7B
#define INV3REG_M_R 0x7E
#define INV3REG_BANK_MREG1 0x00
#define INV3REG_BANK_MREG2 0x28
#define INV3REG_BANK_MREG3 0x50
#define INV3REG_MREG1_FIFO_CONFIG5 0x1
#define INV3REG_MREG1_SENSOR_CONFIG3 0x06
// WHOAMI values
#define INV3_ID_ICM40605 0x33
#define INV3_ID_ICM40609 0x3b
#define INV3_ID_ICM42605 0x42
#define INV3_ID_ICM42688 0x47
#define INV3_ID_IIM42652 0x6f
// run output data at 2kHz
#define INV3_ODR 2000
#define INV3_ID_ICM42670 0x67
/*
really nice that this sensor has an option to request little-endian
@ -128,13 +148,18 @@ AP_InertialSensor_Backend *AP_InertialSensor_Invensensev3::probe(AP_InertialSens
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, fifo_config1);
// 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);
if (inv3_type == Invensensev3_Type::ICM42670) {
// FIFO_FLUSH
register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x04);
} else {
// FIFO_MODE stop-on-full
register_write(INV3REG_FIFO_CONFIG, 0x80);
// FIFO partial disable, enable accel, gyro, temperature
register_write(INV3REG_FIFO_CONFIG1, fifo_config1);
// 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);
@ -170,6 +195,10 @@ void AP_InertialSensor_Invensensev3::start()
fifo_config1 = 0x0F;
temp_sensitivity = 1.0 * 128 / 115.49;
break;
case Invensensev3_Type::ICM42670:
devtype = DEVTYPE_INS_ICM42670;
temp_sensitivity = 1.0 / 2.0;
break;
case Invensensev3_Type::ICM40609:
default:
devtype = DEVTYPE_INS_ICM40609;
@ -181,8 +210,8 @@ void AP_InertialSensor_Invensensev3::start()
// always use FIFO
fifo_reset();
if (!_imu.register_gyro(gyro_instance, INV3_ODR, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, INV3_ODR, dev->get_bus_id_devtype(devtype))) {
if (!_imu.register_gyro(gyro_instance, expected_ODR, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, expected_ODR, dev->get_bus_id_devtype(devtype))) {
return;
}
@ -190,8 +219,8 @@ void AP_InertialSensor_Invensensev3::start()
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);
_set_accel_raw_sample_rate(accel_instance, expected_ODR);
_set_gyro_raw_sample_rate(gyro_instance, expected_ODR);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
@ -211,7 +240,7 @@ void AP_InertialSensor_Invensensev3::start()
}
// start the timer process to read samples
dev->register_periodic_callback(1e6 / INV3_ODR, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
dev->register_periodic_callback(1e6 / expected_ODR, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
}
/*
@ -241,9 +270,17 @@ 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 & 0xF8) != 0x68) {
// no or bad data
return false;
if (inv3_type == Invensensev3_Type::ICM42670) {
if ((d.header & 0xFC) != 0x68) {
// no or bad data
return false;
}
} else {
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])};
@ -272,8 +309,10 @@ void AP_InertialSensor_Invensensev3::read_fifo()
{
bool need_reset = false;
uint16_t n_samples;
const uint8_t reg_counth = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_COUNTH:INV3REG_FIFO_COUNTH;
const uint8_t reg_data = (inv3_type == Invensensev3_Type::ICM42670)?INV3REG_70_FIFO_DATA:INV3REG_FIFO_DATA;
if (!block_read(INV3REG_FIFO_COUNTH, (uint8_t*)&n_samples, 2)) {
if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) {
goto check_registers;
}
@ -284,7 +323,7 @@ void AP_InertialSensor_Invensensev3::read_fifo()
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)) {
if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * INV3_SAMPLE_SIZE)) {
goto check_registers;
}
@ -328,20 +367,77 @@ void AP_InertialSensor_Invensensev3::register_write(uint8_t reg, uint8_t val, bo
dev->write_register(reg, val, checked);
}
/*
read a bank register, only used on startup
*/
uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t reg)
{
register_write(INV3REG_BLK_SEL_R, bank);
register_write(INV3REG_MADDR_R, reg);
hal.scheduler->delay_microseconds(10);
const uint8_t val = register_read(INV3REG_M_R);
hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_R, 0);
return val;
}
/*
write to a bank register. This is only used on startup, so can use
sleeps to wait for success
*/
void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t reg, uint8_t val)
{
// wait for clock ready
for (uint8_t wait_tries=0; wait_tries<10; wait_tries++) {
if (register_read(INV3REG_70_MCLK_RDY) != 0) {
break;
}
hal.scheduler->delay_microseconds(10);
}
if (register_read(INV3REG_70_MCLK_RDY) == 0) {
return;
}
for (uint8_t tries=0; tries<10; tries++) {
register_write(INV3REG_BLK_SEL_W, bank);
register_write(INV3REG_MADDR_W, reg);
register_write(INV3REG_M_W, val);
hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_W, 0);
hal.scheduler->delay_microseconds(10);
const uint8_t val2 = register_read_bank(bank, reg);
if (val == val2) {
break;
}
hal.scheduler->delay_microseconds(100);
}
}
/*
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);
if (inv3_type == Invensensev3_Type::ICM42670) {
// use low-noise mode
register_write(INV3REG_70_PWR_MGMT0, 0x0f);
hal.scheduler->delay_microseconds(300);
// setup gyro for 2kHz
register_write(INV3REG_GYRO_CONFIG0, 0x05);
// setup gyro for 1.6kHz
register_write(INV3REG_70_GYRO_CONFIG0, 0x05);
// setup accel for 2kHz
register_write(INV3REG_ACCEL_CONFIG0, 0x05);
// setup accel for 1.6kHz
register_write(INV3REG_70_ACCEL_CONFIG0, 0x05);
} else {
// 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);
}
}
/*
@ -350,6 +446,8 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
bool AP_InertialSensor_Invensensev3::check_whoami(void)
{
uint8_t whoami = register_read(INV3REG_WHOAMI);
expected_ODR = 2000;
switch (whoami) {
case INV3_ID_ICM40609:
inv3_type = Invensensev3_Type::ICM40609;
@ -371,6 +469,11 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
inv3_type = Invensensev3_Type::IIM42652;
accel_scale = (GRAVITY_MSS / 2048);
return true;
case INV3_ID_ICM42670:
inv3_type = Invensensev3_Type::ICM42670;
accel_scale = (GRAVITY_MSS / 2048);
expected_ODR = 1600;
return true;
}
// not a value WHOAMI result
return false;
@ -399,9 +502,39 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
case Invensensev3_Type::IIM42652:
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40605:
case Invensensev3_Type::ICM42670:
_clip_limit = 15.5f * GRAVITY_MSS;
break;
}
if (inv3_type == Invensensev3_Type::ICM42670) {
// the ICM-42670 needs some more power-up config
for (uint8_t tries=0; tries<50; tries++) {
// initiate a power up sequence
register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x10);
hal.scheduler->delay_microseconds(1000);
register_write(INV3REG_70_PWR_MGMT0, 0x0f, true);
if (register_read(INV3REG_70_MCLK_RDY) != 0) {
break;
}
hal.scheduler->delay(5);
}
if (register_read(INV3REG_70_MCLK_RDY) == 0) {
return false;
}
// disable APEX for larger FIFO
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);
// FIFO stop-on-full, disable bypass
register_write(INV3REG_70_FIFO_CONFIG1, 0x2, true);
// little-endian, fifo count in records
register_write(INV3REG_70_INTF_CONFIG0, 0x40, true);
}
return true;
}

5
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -35,6 +35,7 @@ public:
ICM42605,
ICM40605,
IIM42652,
ICM42670,
};
// acclerometers on Invensense sensors will return values up to 32G
@ -59,6 +60,9 @@ private:
uint8_t register_read(uint8_t reg);
void register_write(uint8_t reg, uint8_t val, bool checked=false);
uint8_t register_read_bank(uint8_t bank, uint8_t reg);
void register_write_bank(uint8_t bank, uint8_t reg, uint8_t val);
bool accumulate_samples(const struct FIFOData *data, uint8_t n_samples);
// instance numbers of accel and gyro data
@ -75,6 +79,7 @@ private:
const enum Rotation rotation;
float accel_scale;
float expected_ODR;
AP_HAL::OwnPtr<AP_HAL::Device> dev;