ICM426XX - Disable AFSR

Betaflight: Disable ICM426XX AFSR feature to prevent stalls Ardupilot: 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 Co-authored-by: Andrew Tridgell <[email protected]> Co-authored-by: Steve Evans <[email protected]> Thanks to all participants here: betaflight/betaflight#12970
emuflight · Oct 24, 2023 · 3b99d9f · 3b99d9f
1 parent c55b0d7
commit 3b99d9f
Showing 1 changed file with 13 additions and 1 deletion.
diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
@@ -48,6 +48,11 @@
 #define ICM426XX_BANK_SELECT3                       0x03
 #define ICM426XX_BANK_SELECT4                       0x04
 
+// Fix for stalls in gyro output. See https://github.com/ArduPilot/ardupilot/pull/25332
+#define ICM426XX_INTF_CONFIG1                       0x4D
+#define ICM426XX_INTF_CONFIG1_AFSR_MASK             0xC0
+#define ICM426XX_INTF_CONFIG1_AFSR_DISABLE          0x40
+
 #define ICM426XX_RA_PWR_MGMT0                       0x4E  // User Bank 0
 #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN            (3 << 0)
 #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN             (3 << 2)
@@ -284,13 +289,20 @@ void icm426xxGyroInit(gyroDev_t *gyro)
 
     spiBusWriteRegister(&gyro->bus, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED);
 
-    uint8_t intConfig1Value = spiReadRegMsk(&gyro->bus, ICM426XX_RA_INT_CONFIG1);
+    uint8_t intConfig1Value = spiBusReadRegister(&gyro->bus, ICM426XX_RA_INT_CONFIG1);
     // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation"
     intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT);
     intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED);
 
     spiBusWriteRegister(&gyro->bus, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
 
+    // Disable AFSR to prevent stalls in gyro output
+    // ICM426XX_INTF_CONFIG1 location in user bank 0
+    uint8_t intfConfig1Value = spiBusReadRegister(&gyro->bus, ICM426XX_INTF_CONFIG1);
+    intfConfig1Value &= ~ICM426XX_INTF_CONFIG1_AFSR_MASK;
+    intfConfig1Value |= ICM426XX_INTF_CONFIG1_AFSR_DISABLE;
+    spiBusWriteRegister(&gyro->bus, ICM426XX_INTF_CONFIG1, intfConfig1Value);
+
     // Turn on gyro and acc on again so ODR and FSR can be configured
     turnGyroAccOn(gyro);