Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fast sampling for ICM45686 

Fix accel scale on ICM45686
This commit is contained in:
Andy Piper 2022-11-07 16:23:36 +00:00 committed by Andrew Tridgell
parent 5608dbe0f0
commit 4aafb3ab71
3 changed files with 86 additions and 57 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

@ -288,7 +288,7 @@ protected:
} }
// if fast sampling is enabled, the rate to use in kHz // if fast sampling is enabled, the rate to use in kHz
uint8_t get_fast_sampling_rate() { uint8_t get_fast_sampling_rate() const {
return (1 << uint8_t(_imu._fast_sampling_rate)); return (1 << uint8_t(_imu._fast_sampling_rate));
} }

128
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -37,16 +37,6 @@
extern const AP_HAL::HAL& hal; 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_2000DPS = (0.0174532f / 16.4f);
/*
gyro as 8.2 LSB/DPS at scale factor of +/- 4000dps (FS_SEL==0)
*/
static const float GYRO_SCALE_4000DPS = (0.0174532f / 8.2f);
// set bit 0x80 in register ID for read on SPI // set bit 0x80 in register ID for read on SPI
#define BIT_READ_FLAG 0x80 #define BIT_READ_FLAG 0x80
@ -355,11 +345,8 @@ bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, ui
Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])}; Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])};
accel *= accel_scale; accel *= accel_scale;
if (inv3_type == Invensensev3_Type::ICM45686) { gyro *= gyro_scale;
gyro *= GYRO_SCALE_4000DPS;
} else {
gyro *= GYRO_SCALE_2000DPS;
}
const float temp = d.temperature * temp_sensitivity + temp_zero; const float temp = d.temperature * temp_sensitivity + temp_zero;
// these four calls are about 40us // these four calls are about 40us
@ -501,6 +488,24 @@ void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t r
} }
} }
// calculate the fast sampling backend rate
uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const
{
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > base_odr) {
loop_limit = 2;
}
if (get_loop_rate_hz() > base_odr * 2) {
loop_limit = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
// calculate rate we will be giving samples to the backend
return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr);
}
/* /*
set the filter frequencies and scaling set the filter frequencies and scaling
@ -562,38 +567,26 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI; fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (fast_sampling) { if (fast_sampling) {
// constrain the gyro rate to be at least the loop rate backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, 8 * backend_rate_hz);
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > 1000) {
loop_limit = 2;
}
if (get_loop_rate_hz() > 2000) {
loop_limit = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
// calculate rate we will be giving samples to the backend
backend_rate_hz *= fast_sampling_rate;
// limited filtering on ICM-42605 // limited filtering on ICM-42605
if (inv3_type == Invensensev3_Type::ICM42605) { if (inv3_type == Invensensev3_Type::ICM42605) {
switch (fast_sampling_rate) { switch (backend_rate_hz) {
case 2: // 2KHz case 2000: // 2KHz
odr_config = 0x05; odr_config = 0x05;
// 507Hz AAF // 507Hz AAF
aaf_delt = 47; aaf_delt = 47;
aaf_deltsqr = 2208; aaf_deltsqr = 2208;
aaf_bitshift = 4; aaf_bitshift = 4;
break; break;
case 4: // 4KHz case 4000: // 4KHz
// 995Hz AAF // 995Hz AAF
aaf_delt = 63; aaf_delt = 63;
aaf_deltsqr = 3968; aaf_deltsqr = 3968;
aaf_bitshift = 3; aaf_bitshift = 3;
odr_config = 0x04; odr_config = 0x04;
break; break;
case 8: // 8Khz case 8000: // 8Khz
// 995Hz AAF // 995Hz AAF
aaf_delt = 63; aaf_delt = 63;
aaf_deltsqr = 3968; aaf_deltsqr = 3968;
@ -605,22 +598,22 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
} }
} else { } else {
// ICM-42688 / ICM-40609 / IIM-426525 // ICM-42688 / ICM-40609 / IIM-426525
switch (fast_sampling_rate) { switch (backend_rate_hz) {
case 2: // 2KHz case 2000: // 2KHz
odr_config = 0x05; odr_config = 0x05;
// 536Hz AAF // 536Hz AAF
aaf_delt = 12; aaf_delt = 12;
aaf_deltsqr = 144; aaf_deltsqr = 144;
aaf_bitshift = 8; aaf_bitshift = 8;
break; break;
case 4: // 4KHz case 4000: // 4KHz
odr_config = 0x04; odr_config = 0x04;
// 997Hz AAF // 997Hz AAF
aaf_delt = 21; aaf_delt = 21;
aaf_deltsqr = 440; aaf_deltsqr = 440;
aaf_bitshift = 6; aaf_bitshift = 6;
break; break;
case 8: // 8Khz case 8000: // 8Khz
odr_config = 0x03; odr_config = 0x03;
// 997Hz AAF // 997Hz AAF
aaf_delt = 21; aaf_delt = 21;
@ -683,18 +676,37 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
*/ */
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void) void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
{ {
uint8_t odr_config = 5;
backend_rate_hz = 1600; backend_rate_hz = 1600;
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (fast_sampling) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
}
}
switch (backend_rate_hz) {
case 3200: // 3.2Khz
odr_config = 4;
break;
case 6400: // 6.4Khz
odr_config = 3;
break;
default:
break;
}
// Disable FIFO first // Disable FIFO first
register_write(INV3REG_456_FIFO_CONFIG3, 0x00); register_write(INV3REG_456_FIFO_CONFIG3, 0x00);
register_write(INV3REG_456_FIFO_CONFIG0, 0x07); register_write(INV3REG_456_FIFO_CONFIG0, 0x07);
// setup gyro for 1.6kHz, 4000dps range // setup gyro for 1.6kHz, 4000dps range
register_write(INV3REG_456_GYRO_CONFIG0, 0x05); register_write(INV3REG_456_GYRO_CONFIG0, odr_config | 0x0);
// setup accel for 1.6kHz, 32g range // setup accel for 1.6kHz, 32g range
register_write(INV3REG_456_ACCEL_CONFIG0, 0x05); register_write(INV3REG_456_ACCEL_CONFIG0, odr_config | 0x0);
// enable timestamps on FIFO data // enable timestamps on FIFO data
// SMC_CONTROL_0 // SMC_CONTROL_0
@ -707,6 +719,22 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
// FIFO stop-on-full, disable bypass and 2K FIFO // FIFO stop-on-full, disable bypass and 2K FIFO
register_write(INV3REG_456_FIFO_CONFIG0, 0x87, true); register_write(INV3REG_456_FIFO_CONFIG0, 0x87, 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
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6, (reg & ~(0x3 << 5)) | (0x2 << 5));
// enable Interpolator and Anti Aliasing Filter on accel
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 gyro LPF
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xAC); // GYRO_UI_LPFBW_SEL b0-1
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xAC, (reg & ~0x7) | 0x0); // bypass
// enable accel LPF
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x83); // ACCEL_UI_LPFBW_SEL b0-1
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (reg & ~0x3) | 0x0); // bypass
// enable FIFO // enable FIFO
register_write(INV3REG_456_FIFO_CONFIG3, 0x07, true); register_write(INV3REG_456_FIFO_CONFIG3, 0x07, true);
} }
@ -725,23 +753,18 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
return true; return true;
case INV3_ID_ICM42688: case INV3_ID_ICM42688:
inv3_type = Invensensev3_Type::ICM42688; inv3_type = Invensensev3_Type::ICM42688;
accel_scale = (GRAVITY_MSS / 2048);
return true; return true;
case INV3_ID_ICM42605: case INV3_ID_ICM42605:
inv3_type = Invensensev3_Type::ICM42605; inv3_type = Invensensev3_Type::ICM42605;
accel_scale = (GRAVITY_MSS / 2048);
return true; return true;
case INV3_ID_ICM40605: case INV3_ID_ICM40605:
inv3_type = Invensensev3_Type::ICM40605; inv3_type = Invensensev3_Type::ICM40605;
accel_scale = (GRAVITY_MSS / 2048);
return true; return true;
case INV3_ID_IIM42652: case INV3_ID_IIM42652:
inv3_type = Invensensev3_Type::IIM42652; inv3_type = Invensensev3_Type::IIM42652;
accel_scale = (GRAVITY_MSS / 2048);
return true; return true;
case INV3_ID_ICM42670: case INV3_ID_ICM42670:
inv3_type = Invensensev3_Type::ICM42670; inv3_type = Invensensev3_Type::ICM42670;
accel_scale = (GRAVITY_MSS / 2048);
return true; return true;
} }
// check 456 who am i // check 456 who am i
@ -749,7 +772,7 @@ bool AP_InertialSensor_Invensensev3::check_whoami(void)
switch (whoami) { switch (whoami) {
case INV3_ID_ICM45686: case INV3_ID_ICM45686:
inv3_type = Invensensev3_Type::ICM45686; inv3_type = Invensensev3_Type::ICM45686;
accel_scale = (GRAVITY_MSS / 1024); gyro_scale = GYRO_SCALE_4000DPS;
return true; return true;
} }
// not a value WHOAMI result // not a value WHOAMI result
@ -856,9 +879,11 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
register_write(INV3REG_70_INTF_CONFIG0, 0x40, true); register_write(INV3REG_70_INTF_CONFIG0, 0x40, true);
} else if (inv3_type == Invensensev3_Type::ICM45686) { } else if (inv3_type == Invensensev3_Type::ICM45686) {
hal.scheduler->delay_microseconds(1000); hal.scheduler->delay_microseconds(1000);
// gyro and accel in low-noise mode
register_write(INV3REG_456_PWR_MGMT0, 0x0f); register_write(INV3REG_456_PWR_MGMT0, 0x0f);
hal.scheduler->delay_microseconds(300); hal.scheduler->delay_microseconds(300);
/*************************CLKIN setting*************************/ /*************************CLKIN setting*************************/
// Likely specific to CubeOrangePlus, will need changing for boards without external clocking
// override INT2 pad as CLKIN // override INT2 pad as CLKIN
register_write(INV3REG_456_IOC_PAD_SCENARIO_OVRD, 0x06, true); register_write(INV3REG_456_IOC_PAD_SCENARIO_OVRD, 0x06, true);
@ -869,16 +894,9 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
register_write(INV3REG_456_RTC_CONFIG, 0x23, true); register_write(INV3REG_456_RTC_CONFIG, 0x23, true);
// disable STC // disable STC
uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68); uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68); // I3C_STC_MODE b2
register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x02); register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x04);
/*************************CLKIN setting*************************/
// enable Interpolator and Anti Aliasing Filter on Gyro
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6);
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6, (reg & ~0x60) | 0x40);
// enable Interpolator and Anti Aliasing Filter on Gyro
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B);
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (reg & ~0x3) | 0x2);
} }
return true; return true;

13
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.h
View File

@ -57,6 +57,7 @@ private:
void set_filter_and_scaling_icm42670(void); void set_filter_and_scaling_icm42670(void);
void set_filter_and_scaling_icm456xy(void); void set_filter_and_scaling_icm456xy(void);
void fifo_reset(); void fifo_reset();
uint16_t calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const;
/* Read samples from FIFO */ /* Read samples from FIFO */
void read_fifo(); void read_fifo();
@ -85,7 +86,17 @@ private:
const enum Rotation rotation; const enum Rotation rotation;
float accel_scale; /*
gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==0)
*/
static constexpr float GYRO_SCALE_2000DPS = (0.0174532f / 16.4f);
/*
gyro as 8.2 LSB/DPS at scale factor of +/- 4000dps (FS_SEL==0)
*/
static constexpr float GYRO_SCALE_4000DPS = (0.0174532f / 8.2f);
float accel_scale = (GRAVITY_MSS / 2048);
float gyro_scale = GYRO_SCALE_2000DPS;
// are we doing more than 1kHz sampling? // are we doing more than 1kHz sampling?
bool fast_sampling; bool fast_sampling;