aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: enable AAF at 488Hz for gyro+accel on ICM42688

Browse Source 
this should improve vibration handling
This commit is contained in:
Andrew Tridgell 2022-07-20 13:13:29 +10:00
parent 5bde9b0e7c
commit 5b579aebce
1 changed files with 41 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -27,10 +27,6 @@
anti-aliasing filter, which means this driver can be a lot simpler anti-aliasing filter, which means this driver can be a lot simpler
than the Invensense and Invensensev2 drivers which need to handle than the Invensense and Invensensev2 drivers which need to handle
8kHz sample rates to achieve decent aliasing protection 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 <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
@ -62,6 +58,17 @@ static const float GYRO_SCALE = (0.0174532f / 16.4f);
#define INV3REG_FIFO_DATA 0x30 #define INV3REG_FIFO_DATA 0x30
#define INV3REG_BANK_SEL 0x76 #define INV3REG_BANK_SEL 0x76
// ICM42688 bank1
#define INV3REG_GYRO_CONFIG_STATIC2 0x0B
#define INV3REG_GYRO_CONFIG_STATIC3 0x0C
#define INV3REG_GYRO_CONFIG_STATIC4 0x0D
#define INV3REG_GYRO_CONFIG_STATIC5 0x0E
// ICM42688 bank2
#define INV3REG_ACCEL_CONFIG_STATIC2 0x03
#define INV3REG_ACCEL_CONFIG_STATIC3 0x04
#define INV3REG_ACCEL_CONFIG_STATIC4 0x05
// registers for ICM-42670, multi-bank // registers for ICM-42670, multi-bank
#define INV3REG_70_PWR_MGMT0 0x1F #define INV3REG_70_PWR_MGMT0 0x1F
#define INV3REG_70_GYRO_CONFIG0 0x20 #define INV3REG_70_GYRO_CONFIG0 0x20
@ -372,12 +379,19 @@ void AP_InertialSensor_Invensensev3::register_write(uint8_t reg, uint8_t val, bo
*/ */
uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t reg) uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t reg)
{ {
register_write(INV3REG_BLK_SEL_R, bank); if (inv3_type == Invensensev3_Type::ICM42670) {
register_write(INV3REG_MADDR_R, reg); // the ICM42670 has a complex bank setup
hal.scheduler->delay_microseconds(10); register_write(INV3REG_BLK_SEL_R, bank);
const uint8_t val = register_read(INV3REG_M_R); register_write(INV3REG_MADDR_R, reg);
hal.scheduler->delay_microseconds(10); hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_R, 0); const uint8_t val = register_read(INV3REG_M_R);
hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_R, 0);
return val;
}
register_write(INV3REG_BANK_SEL, bank);
const uint8_t val = register_read(reg);
register_write(INV3REG_BANK_SEL, 0);
return val; return val;
} }
@ -387,28 +401,18 @@ uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t
*/ */
void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t reg, uint8_t val) void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t reg, uint8_t val)
{ {
// wait for clock ready if (inv3_type == Invensensev3_Type::ICM42670) {
for (uint8_t wait_tries=0; wait_tries<10; wait_tries++) { // the ICM42670 has a complex bank setup
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_BLK_SEL_W, bank);
register_write(INV3REG_MADDR_W, reg); register_write(INV3REG_MADDR_W, reg);
register_write(INV3REG_M_W, val); register_write(INV3REG_M_W, val);
hal.scheduler->delay_microseconds(10); hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_W, 0); register_write(INV3REG_BLK_SEL_W, 0);
hal.scheduler->delay_microseconds(10); hal.scheduler->delay_microseconds(10);
const uint8_t val2 = register_read_bank(bank, reg); } else {
if (val == val2) { register_write(INV3REG_BANK_SEL, bank);
break; register_write(reg, val);
} register_write(INV3REG_BANK_SEL, 0);
hal.scheduler->delay_microseconds(100);
} }
} }
@ -535,6 +539,17 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
// little-endian, fifo count in records // little-endian, fifo count in records
register_write(INV3REG_70_INTF_CONFIG0, 0x40, true); register_write(INV3REG_70_INTF_CONFIG0, 0x40, true);
} }
if (inv3_type == Invensensev3_Type::ICM42688) {
// setup anti-alias filters for 488Hz on gyro and accel, notch disabled
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC2, 0xA1);
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC3, 11); // GYRO_AAF_DELT
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC4, 122); // GYRO_AAF_DELTSQR
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC5, 0x80); // GYRO_AAF_BITSHIFT&GYRO_AAF_DELTSQR
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC2, 11U<<1);
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, 122);
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, 0x80);
}
return true; return true;
} }