Merge pull request #9409 from iNavFlight/dzikuvx-icm426xx-disable-afsr

Disable ASFR function on ICM426xx driver
iNavFlight · Oct 26, 2023 · b67165e · b67165e
2 parents 183e1e5 + f4d5c3b
commit b67165e
Showing 1 changed file with 12 additions and 1 deletion.
diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c
@@ -78,11 +78,13 @@
 #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_RA_INT_SOURCE0                     0x65
 #define ICM42605_UI_DRDY_INT1_EN_DISABLED           (0 << 3)
 #define ICM42605_UI_DRDY_INT1_EN_ENABLED            (1 << 3)
 
+#define ICM42605_INTF_CONFIG1                       0x4D
+#define ICM42605_INTF_CONFIG1_AFSR_MASK             0xC0
+#define ICM42605_INTF_CONFIG1_AFSR_DISABLE          0x40
 
 static void icm42605AccInit(accDev_t *acc)
 {
@@ -190,6 +192,15 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro)
     busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value);
     delay(15);
 
+    //Disable AFSR as in BF and Ardupilot
+    uint8_t intfConfig1Value;
+    busRead(dev, ICM42605_INTF_CONFIG1, &intfConfig1Value);
+    intfConfig1Value &= ~ICM42605_INTF_CONFIG1_AFSR_MASK;
+    intfConfig1Value |= ICM42605_INTF_CONFIG1_AFSR_DISABLE;
+    busWrite(dev, ICM42605_INTF_CONFIG1, intfConfig1Value);
+
+    delay(15);
+
     busSetSpeed(dev, BUS_SPEED_FAST);
 }