From 4f17fe0aacf5b8a7813f93c41784fb0b6c57f120 Mon Sep 17 00:00:00 2001 From: Steve Evans Date: Mon, 23 Oct 2023 08:08:07 +0100 Subject: [PATCH] Disable ICM426XX AFSR feature to prevent stalls (#13132) --- src/main/drivers/accgyro/accgyro_spi_icm426xx.c | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index a93027a2ad3..ec6ee5f7014 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -55,6 +55,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) @@ -274,6 +279,13 @@ void icm426xxGyroInit(gyroDev_t *gyro) spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value); + // Disable AFSR to prevent stalls in gyro output + uint8_t intfConfig1Value = spiReadRegMsk(dev, ICM426XX_INTF_CONFIG1); + intfConfig1Value &= ~ICM426XX_INTF_CONFIG1_AFSR_MASK; + intfConfig1Value |= ICM426XX_INTF_CONFIG1_AFSR_DISABLE; + spiWriteReg(dev, ICM426XX_INTF_CONFIG1, intfConfig1Value); + + // Turn on gyro and acc on again so ODR and FSR can be configured turnGyroAccOn(dev);