From 3b99d9f558c66977fa8bff05ef62e83c266cd1a4 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 24 Oct 2023 10:46:19 -0500 Subject: [PATCH] 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 Co-authored-by: Steve Evans Thanks to all participants here: https://github.com/betaflight/betaflight/issues/12970 --- src/main/drivers/accgyro/accgyro_spi_icm426xx.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 24003f9017..d9efe417b2 100644 --- 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);