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

AP_InertialSensor: fix for ICM42688 stuck gyro issue 

these undocumented bits in register 0x4d control the "adaptive full
scale range" mode of the ICM42688. The feature is enabled by default
but has a bug where it gives "stuck" gyro values for short periods
(between 1ms and 2ms):, leading to a significant gyro bias at longer
time scales, enough to in some cases cause a vehicle to crash if it is
unable to switch to an alternative IMU

this fixes https://github.com/ArduPilot/ardupilot/issues/25025
This commit is contained in:
Andrew Tridgell 2023-10-22 08:24:52 +11:00
parent cf6fe205d2
commit 792d8a4cb8
1 changed files with 7 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
View File

@ -125,6 +125,9 @@ extern const AP_HAL::HAL& hal;
#define INV3BANK_456_IPREG_SYS1_ADDR 0xA400
#define INV3BANK_456_IPREG_SYS2_ADDR 0xA500
// ICM42688 specific registers
#define INV3REG_42688_INTF_CONFIG1 0x4d
// WHOAMI values
#define INV3_ID_ICM40605 0x33
#define INV3_ID_ICM40609 0x3b
@ -909,6 +912,10 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
// disable STC
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 & ~0x04);
} else if (inv3_type == Invensensev3_Type::ICM42688) {
// fix for the "stuck gyro" issue
const uint8_t v = register_read(INV3REG_42688_INTF_CONFIG1);
register_write(INV3REG_42688_INTF_CONFIG1, (v & 0x3F) | 0x40);
}
return true;