initial ICM426xx support

make/targets.mk

@@ -34,6 +34,8 @@ UNSUPPORTED_TARGETS := \
     CRAZYBEEF3DX \
     CRAZYBEEF3FR \
     CRAZYBEEF3FS \
+    CRAZYFLIE2 \
+    CRAZYFLIE2BQ \
     DOGE EACHIF3 \
     FF_ACROWHOOPSP \
     FF_KOMBINI \

src/main/common/utils.h

@@ -103,6 +103,8 @@ static inline int32_t cmp32(uint32_t a, uint32_t b) {
     return (int32_t)(a - b);
 }
 
+static inline uint32_t llog2(uint32_t n) { return 31 - __builtin_clz(n | 1); }
+
 // using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
 //  than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)
 #if defined(UNIT_TEST) || defined(SIMULATOR_BUILD)

src/main/drivers/accgyro/accgyro.h

@@ -44,6 +44,9 @@
 #define GYRO_32KHZ_HARDWARE_LPF_NORMAL       0
 #define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1
 
+#define GYRO_HARDWARE_LPF_OPTION_1 1
+#define GYRO_HARDWARE_LPF_OPTION_2 2
+
 #define GYRO_LPF_256HZ      0
 #define GYRO_LPF_188HZ      1
 #define GYRO_LPF_98HZ       2
@@ -91,6 +94,8 @@ typedef struct gyroDev_s {
     ioTag_t mpuIntExtiTag;
     uint8_t gyroHasOverflowProtection;
     gyroSensor_e gyroHardware;
+    uint8_t accDataReg;
+    uint8_t gyroDataReg;
 } gyroDev_t;
 
 typedef struct accDev_s {

src/main/drivers/accgyro/accgyro_mpu.c

@@ -55,6 +55,7 @@
 #include "drivers/accgyro/accgyro_spi_bmi160.h"
 #include "drivers/accgyro/accgyro_spi_icm20649.h"
 #include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm426xx.h"
 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
@@ -365,6 +366,32 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) {
         return true;
     }
 #endif
+#ifdef USE_GYRO_SPI_ICM42605
+#ifdef ICM42605_SPI_INSTANCE
+    spiBusSetInstance(&gyro->bus, ICM42605_SPI_INSTANCE);
+#endif
+#ifdef ICM42605_CS_PIN
+    gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM42605_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
+#endif
+    sensor = icm426xxSpiDetect(&gyro->bus);
+    if (sensor != MPU_NONE) {
+        gyro->mpuDetectionResult.sensor = sensor;
+        return true;
+    }
+#endif
+#ifdef USE_GYRO_SPI_ICM42688P
+#ifdef ICM42688P_SPI_INSTANCE
+    spiBusSetInstance(&gyro->bus, ICM42688P_SPI_INSTANCE);
+#endif
+#ifdef ICM42688P_CS_PIN
+    gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM42688P_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
+#endif
+    sensor = icm426xxSpiDetect(&gyro->bus);
+    if (sensor != MPU_NONE) {
+        gyro->mpuDetectionResult.sensor = sensor;
+        return true;
+    }
+#endif
 #ifdef USE_ACCGYRO_BMI160
 #ifndef USE_DUAL_GYRO
     spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE);

src/main/drivers/accgyro/accgyro_mpu.h

@@ -29,7 +29,7 @@
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \
- || defined(USE_GYRO_SPI_ICM20689)
+ || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
 #define GYRO_USES_SPI
 #endif
 
@@ -47,6 +47,8 @@
 #define ICM20608G_WHO_AM_I_CONST            (0xAF)
 #define ICM20649_WHO_AM_I_CONST             (0xE1)
 #define ICM20689_WHO_AM_I_CONST             (0x98)
+#define ICM42605_WHO_AM_I_CONST             (0x42)
+#define ICM42688P_WHO_AM_I_CONST            (0x47)
 
 // RA = Register Address
 
@@ -214,6 +216,8 @@ typedef enum {
     ICM_20608_SPI,
     ICM_20649_SPI,
     ICM_20689_SPI,
+    ICM_42605_SPI,
+    ICM_42688P_SPI,
     BMI_160_SPI,
     IMUF_9001_SPI,
 } mpuSensor_e;