From 5d443c4a4b6e34927b320d3929f34f919731375d Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Wed, 1 Nov 2023 15:07:31 +0100
Subject: [PATCH] icm42: disable AFSR

---
 src/driver/spi_icm42605.c | 11 +++++++++++
 src/driver/spi_icm42605.h |  3 +++
 2 files changed, 14 insertions(+)

diff --git a/src/driver/spi_icm42605.c b/src/driver/spi_icm42605.c
index cbe2cc973..a777d07e9 100644
--- a/src/driver/spi_icm42605.c
+++ b/src/driver/spi_icm42605.c
@@ -49,6 +49,17 @@ void icm42605_configure() {
   icm42605_write(ICM42605_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
   icm42605_write(ICM42605_INT_CONFIG0, ICM42605_UI_DRDY_INT_CLEAR_ON_SBR);
 
+  {
+    // Disable AFSR to prevent stalls in gyro output
+    // Undocumented setting see
+    // https://github.com/ArduPilot/ardupilot/issues/25025
+    // https://github.com/ArduPilot/ardupilot/pull/25332
+    uint8_t val = icm42605_read(ICM42605_INTF_CONFIG1);
+    val &= ~ICM42605_INTF_CONFIG1_AFSR_MASK;
+    val |= ICM42605_INTF_CONFIG1_AFSR_DISABLE;
+    icm42605_write(ICM42605_INTF_CONFIG1, val);
+  }
+
   // bank 1
   icm42605_write(ICM42605_REG_BANK_SEL, 1);
   icm42605_write(ICM42605_GYRO_CONFIG_STATIC3, AAF_DELT);
diff --git a/src/driver/spi_icm42605.h b/src/driver/spi_icm42605.h
index aa87ef202..35a11d946 100644
--- a/src/driver/spi_icm42605.h
+++ b/src/driver/spi_icm42605.h
@@ -187,6 +187,9 @@
 #define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT)
 #define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT)
 
+#define ICM42605_INTF_CONFIG1_AFSR_MASK 0xC0
+#define ICM42605_INTF_CONFIG1_AFSR_DISABLE 0x40
+
 uint8_t icm42605_detect();
 void icm42605_configure();