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 ArduPilot#25025

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;