From 483d19f44c4693e69d9ee4d8bda5a87dc13a2cce Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 26 Oct 2023 10:10:50 +1100
Subject: [PATCH] AP_InertialSensor: apply stuck gyro fix to all IxM42xxx
 sensors

TDK has confirmed this applies to all IxM42xxx sensors
---
 .../AP_InertialSensor_Invensensev3.cpp        | 36 +++++++++++++++----
 1 file changed, 29 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
index b56a390386..110d3586b6 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Invensensev3.cpp
@@ -127,8 +127,8 @@ 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
+// ICM42xxx specific registers
+#define INV3REG_42XXX_INTF_CONFIG1  0x4d
 
 // WHOAMI values
 #define INV3_ID_ICM40605      0x33
@@ -802,6 +802,28 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
     register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, (accel_aaf_deltsqr & 0xFF)); // ACCEL_AAF_DELTSQR
     register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, ((accel_aaf_bitshift<<4) & 0xF0) | ((accel_aaf_deltsqr>>8) & 0x0F)); // ACCEL_AAF_BITSHIFT | ACCEL_AAF_DELTSQR
 
+    switch (inv3_type) {
+    case Invensensev3_Type::ICM42688:
+    case Invensensev3_Type::ICM42605:
+    case Invensensev3_Type::IIM42652:
+    case Invensensev3_Type::ICM42670: {
+        /*
+          fix for the "stuck gyro" issue, which affects all IxM42xxx
+          sensors. This disables the AFSR feature which changes the
+          noise sensitivity with angular rate. When the switch happens
+          (at around 100 deg/sec) the gyro gets stuck for around 2ms,
+          producing constant output which causes a DC gyro bias
+        */
+        const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1);
+        register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40, true);
+        break;
+    }
+    case Invensensev3_Type::ICM40605:
+    case Invensensev3_Type::ICM40609:
+    case Invensensev3_Type::ICM45686:
+        break;
+    }
+
     // enable gyro and accel in low-noise modes
     register_write(INV3REG_PWR_MGMT0, 0x0F);
     hal.scheduler->delay_microseconds(300);
@@ -826,6 +848,10 @@ void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
     register_write(INV3REG_70_ACCEL_CONFIG0, 0x05);
     // AAF is not available for accels, so LPF at 180Hz
     register_write(INV3REG_70_ACCEL_CONFIG1, 0x01);
+
+    // fix "stuck" gyro issue
+    const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1);
+    register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40);
 }
 
 /*
@@ -984,7 +1010,7 @@ bool AP_InertialSensor_Invensensev3::hardware_init(void)
 {
     WITH_SEMAPHORE(dev->get_semaphore());
 
-    dev->setup_checked_registers(7, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
+    dev->setup_checked_registers(8, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
 
     // initially run the bus at low speed
     dev->set_speed(AP_HAL::Device::SPEED_LOW);
@@ -1074,10 +1100,6 @@ 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;