From 6f25ca97740790fcdc75d40cc0f9a63f099c81b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 22 Oct 2023 08:24:52 +1100 Subject: [PATCH] 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 --- .../AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp index 1baacf9458f4c..e20ce39041095 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp @@ -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;