AP_InertialSensor: apply stuck gyro fix to all IxM42xxx sensors

TDK has confirmed this applies to all IxM42xxx sensors

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
@@ -1074,10 +1074,28 @@ 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);
+    }
+
+    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);
+        break;
+    }
+    case Invensensev3_Type::ICM40605:
+    case Invensensev3_Type::ICM40609:
+    case Invensensev3_Type::ICM45686:
+        break;
     }
 
     return true;