From 0d9ab2984def4b2385e63e95f20af337002d3aee Mon Sep 17 00:00:00 2001
From: Mark Haslinghuis <mark@numloq.nl>
Date: Mon, 23 Oct 2023 16:57:08 +0200
Subject: [PATCH] Disable ICM4268xx AFSR feature to prevent stalls (#13137)

---
 src/main/drivers/accgyro/accgyro_spi_icm426xx.c | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c
index a93027a2ad..33e889affc 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,14 @@ void icm426xxGyroInit(gyroDev_t *gyro)
 
     spiWriteReg(dev, ICM426XX_RA_INT_CONFIG1, intConfig1Value);
 
+    // Disable AFSR to prevent stalls in gyro output
+    // ICM426XX_INTF_CONFIG1 located in user bank 0
+    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);