Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fix for ICM42688 stuck gyro issue

Browse Source 
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;