icm42: disable AFSR

BossHobby · Nov 1, 2023 · befab65 · befab65
1 parent afd74d4
commit befab65
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 0 deletions.
diff --git 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
@@ -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();