Converted to using gyroDev_t and accDev_t as in betaflight

src/main/blackbox/blackbox.c

@@ -1313,8 +1313,8 @@ static bool blackboxWriteSysinfo()
         BLACKBOX_PRINT_HEADER_LINE("rcRate:%d",                             100); //For compatibility reasons write rc_rate 100
         BLACKBOX_PRINT_HEADER_LINE("minthrottle:%d",                        masterConfig.motorConfig.minthrottle);
         BLACKBOX_PRINT_HEADER_LINE("maxthrottle:%d",                        masterConfig.motorConfig.maxthrottle);
-        BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x",                       castFloatBytesToInt(gyro.scale));
+        BLACKBOX_PRINT_HEADER_LINE("gyro.scale:0x%x",                       castFloatBytesToInt(gyro.dev.scale));
-        BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u",                             acc.acc_1G);
+        BLACKBOX_PRINT_HEADER_LINE("acc_1G:%u",                             acc.dev.acc_1G);
 
         BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
             if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
@@ -1336,7 +1336,7 @@ static bool blackboxWriteSysinfo()
             }
             );
 
-        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                           gyro.targetLooptime);
+        BLACKBOX_PRINT_HEADER_LINE("looptime:%d",                           gyro.dev.targetLooptime);
         BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d",                             masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcExpo8);
         BLACKBOX_PRINT_HEADER_LINE("rcYawExpo:%d",                          masterConfig.controlRateProfiles[masterConfig.current_profile_index].rcYawExpo8);
         BLACKBOX_PRINT_HEADER_LINE("thrMid:%d",                             masterConfig.controlRateProfiles[masterConfig.current_profile_index].thrMid8);

src/main/blackbox/blackbox_io.c

@@ -623,7 +623,7 @@ bool blackboxDeviceOpen(void)
                  *                              = floor((looptime_ns * 3) / 500.0)
                  *                              = (looptime_ns * 3) / 500
                  */
-                blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
+                blackboxMaxHeaderBytesPerIteration = constrain((gyro.dev.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION);
 
                 return blackboxPort != NULL;
             }

src/main/config/config.c

@@ -407,7 +407,7 @@ uint32_t getPidUpdateRate(void) {
 }
 
 uint32_t getGyroUpdateRate(void) {
-    return gyro.targetLooptime;
+    return gyro.dev.targetLooptime;
 }
 
 uint16_t getAccUpdateRate(void) {

src/main/drivers/accgyro.h

@@ -17,24 +17,37 @@
 
 #pragma once
 
-#include "sensor.h"
+#include "common/axis.h"
+#include "drivers/sensor.h"
 
 #ifndef MPU_I2C_INSTANCE
 #define MPU_I2C_INSTANCE I2C_DEVICE
 #endif
 
-typedef struct gyro_s {
+#define GYRO_LPF_256HZ      0
+#define GYRO_LPF_188HZ      1
+#define GYRO_LPF_98HZ       2
+#define GYRO_LPF_42HZ       3
+#define GYRO_LPF_20HZ       4
+#define GYRO_LPF_10HZ       5
+#define GYRO_LPF_5HZ        6
+#define GYRO_LPF_NONE       7
+
+typedef struct gyroDev_s {
     sensorGyroInitFuncPtr init;                             // initialize function
-    sensorReadFuncPtr read;                                 // read 3 axis data function
+    sensorGyroReadFuncPtr read;                                 // read 3 axis data function
     sensorReadFuncPtr temperature;                          // read temperature if available
-    sensorInterruptFuncPtr intStatus;
+    sensorGyroInterruptStatusFuncPtr intStatus;
     float scale;                                            // scalefactor
     uint32_t targetLooptime;
-} gyro_t;
+    volatile bool dataReady;
+    uint16_t lpf;
+    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
+} gyroDev_t;
 
-typedef struct acc_s {
+typedef struct accDev_s {
     sensorAccInitFuncPtr init;                                 // initialize function
     sensorReadFuncPtr read;                                 // read 3 axis data function
     uint16_t acc_1G;
     char revisionCode;                                      // a revision code for the sensor, if known
-} acc_t;
+} accDev_t;

src/main/drivers/accgyro_adxl345.c

@@ -56,12 +56,12 @@
 #define ADXL345_RANGE_16G   0x03
 #define ADXL345_FIFO_STREAM 0x80
 
-static void adxl345Init(acc_t *acc);
+static void adxl345Init(accDev_t *acc);
 static bool adxl345Read(int16_t *accelData);
 
 static bool useFifo = false;
 
-bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
+bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -78,7 +78,7 @@ bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc)
     return true;
 }
 
-static void adxl345Init(acc_t *acc)
+static void adxl345Init(accDev_t *acc)
 {
     if (useFifo) {
         uint8_t fifoDepth = 16;

src/main/drivers/accgyro_adxl345.h

@@ -22,4 +22,4 @@ typedef struct drv_adxl345_config_s {
     uint16_t dataRate;
 } drv_adxl345_config_t;
 
-bool adxl345Detect(drv_adxl345_config_t *init, acc_t *acc);
+bool adxl345Detect(accDev_t *acc, drv_adxl345_config_t *init);

src/main/drivers/accgyro_bma280.c

@@ -32,10 +32,10 @@
 #define BMA280_PMU_BW      0x10
 #define BMA280_PMU_RANGE   0x0F
 
-static void bma280Init(acc_t *acc);
+static void bma280Init(accDev_t *acc);
 static bool bma280Read(int16_t *accelData);
 
-bool bma280Detect(acc_t *acc)
+bool bma280Detect(accDev_t *acc)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -49,7 +49,7 @@ bool bma280Detect(acc_t *acc)
     return true;
 }
 
-static void bma280Init(acc_t *acc)
+static void bma280Init(accDev_t *acc)
 {
     i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_RANGE, 0x08); // +-8g range
     i2cWrite(MPU_I2C_INSTANCE, BMA280_ADDRESS, BMA280_PMU_BW, 0x0E); // 500Hz BW

src/main/drivers/accgyro_bma280.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool bma280Detect(acc_t *acc);
+bool bma280Detect(accDev_t *acc);

src/main/drivers/accgyro_fake.c

@@ -31,9 +31,9 @@
 
 static int16_t fakeGyroADC[XYZ_AXIS_COUNT];
 
-static void fakeGyroInit(uint8_t lpf)
+static void fakeGyroInit(gyroDev_t *gyro)
 {
-    UNUSED(lpf);
+    UNUSED(gyro);
 }
 
 void fakeGyroSet(int16_t x, int16_t y, int16_t z)
@@ -43,11 +43,11 @@ void fakeGyroSet(int16_t x, int16_t y, int16_t z)
     fakeGyroADC[Z] = z;
 }
 
-static bool fakeGyroRead(int16_t *gyroADC)
+static bool fakeGyroRead(gyroDev_t *gyro)
 {
-    gyroADC[X] = fakeGyroADC[X];
+    gyro->gyroADCRaw[X] = fakeGyroADC[X];
-    gyroADC[Y] = fakeGyroADC[Y];
+    gyro->gyroADCRaw[Y] = fakeGyroADC[Y];
-    gyroADC[Z] = fakeGyroADC[Z];
+    gyro->gyroADCRaw[Z] = fakeGyroADC[Z];
     return true;
 }
 
@@ -57,12 +57,13 @@ static bool fakeGyroReadTemp(int16_t *tempData)
     return true;
 }
 
-static bool fakeGyroInitStatus(void)
+static bool fakeGyroInitStatus(gyroDev_t *gyro)
 {
+    UNUSED(gyro);
     return true;
 }
 
-bool fakeGyroDetect(gyro_t *gyro)
+bool fakeGyroDetect(gyroDev_t *gyro)
 {
     gyro->init = fakeGyroInit;
     gyro->intStatus = fakeGyroInitStatus;
@@ -78,7 +79,7 @@ bool fakeGyroDetect(gyro_t *gyro)
 
 static int16_t fakeAccData[XYZ_AXIS_COUNT];
 
-static void fakeAccInit(acc_t *acc)
+static void fakeAccInit(accDev_t *acc)
 {
     UNUSED(acc);
 }
@@ -98,7 +99,7 @@ static bool fakeAccRead(int16_t *accData)
     return true;
 }
 
-bool fakeAccDetect(acc_t *acc)
+bool fakeAccDetect(accDev_t *acc)
 {
     acc->init = fakeAccInit;
     acc->read = fakeAccRead;

src/main/drivers/accgyro_fake.h

@@ -17,10 +17,8 @@
 
 #pragma once
 
-struct acc_s;
+bool fakeAccDetect(accDev_t *acc);
-bool fakeAccDetect(struct acc_s *acc);
 void fakeAccSet(int16_t x, int16_t y, int16_t z);
 
-struct gyro_s;
+bool fakeGyroDetect(gyroDev_t *gyro);
-bool fakeGyroDetect(struct gyro_s *gyro);
 void fakeGyroSet(int16_t x, int16_t y, int16_t z);

src/main/drivers/accgyro_l3g4200d.c

@@ -54,10 +54,10 @@
 #define L3G4200D_DLPF_78HZ       0x80
 #define L3G4200D_DLPF_93HZ       0xC0
 
-static void l3g4200dInit(uint8_t lpf);
+static void l3g4200dInit(gyroDev_t *gyro);
-static bool l3g4200dRead(int16_t *gyroADC);
+static bool l3g4200dRead(gyroDev_t *gyro);
 
-bool l3g4200dDetect(gyro_t *gyro)
+bool l3g4200dDetect(gyroDev_t *gyro)
 {
     uint8_t deviceid;
 
@@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
     return true;
 }
 
-static void l3g4200dInit(uint8_t lpf)
+static void l3g4200dInit(gyroDev_t *gyro)
 {
     bool ack;
 
     uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
 
-    switch (lpf) {
+    switch (gyro->lpf) {
         default:
         case 3:     // BITS_DLPF_CFG_42HZ
             mpuLowPassFilter = L3G4200D_DLPF_32HZ;
@@ -109,7 +109,7 @@ static void l3g4200dInit(uint8_t lpf)
 }
 
 // Read 3 gyro values into user-provided buffer. No overrun checking is done.
-static bool l3g4200dRead(int16_t *gyroADC)
+static bool l3g4200dRead(gyroDev_t *gyro)
 {
     uint8_t buf[6];
 
@@ -117,9 +117,9 @@ static bool l3g4200dRead(int16_t *gyroADC)
         return false;
     }
 
-    gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyro->gyroADCRaw[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyro->gyroADCRaw[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyro->gyroADCRaw[Z] = (int16_t)((buf[4] << 8) | buf[5]);
 
     return true;
 }

src/main/drivers/accgyro_l3g4200d.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3g4200dDetect(gyro_t *gyro);
+bool l3g4200dDetect(gyroDev_t *gyro);

src/main/drivers/accgyro_l3gd20.c

@@ -84,9 +84,9 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
     spiSetDivisor(L3GD20_SPI, SPI_CLOCK_STANDARD);
 }
 
-void l3gd20GyroInit(uint8_t lpf)
+void l3gd20GyroInit(gyroDev_t *gyro)
 {
-    UNUSED(lpf); // FIXME use it!
+    UNUSED(gyro); // FIXME use it!
 
     l3gd20SpiInit(L3GD20_SPI);
 
@@ -120,7 +120,7 @@ void l3gd20GyroInit(uint8_t lpf)
     delay(100);
 }
 
-static bool l3gd20GyroRead(int16_t *gyroADC)
+static bool l3gd20GyroRead(gyroDev_t *gyro)
 {
     uint8_t buf[6];
 
@@ -134,9 +134,9 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
 
     DISABLE_L3GD20;
 
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyro->gyroADCRaw[0] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyro->gyroADCRaw[1] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyro->gyroADCRaw[2] = (int16_t)((buf[4] << 8) | buf[5]);
 
 #if 0
     debug[0] = (int16_t)((buf[1] << 8) | buf[0]);
@@ -150,7 +150,7 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
 // Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
 #define L3GD20_GYRO_SCALE_FACTOR  0.07f
 
-bool l3gd20Detect(gyro_t *gyro)
+bool l3gd20Detect(gyroDev_t *gyro)
 {
     gyro->init = l3gd20GyroInit;
     gyro->read = l3gd20GyroRead;

src/main/drivers/accgyro_l3gd20.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool l3gd20Detect(gyro_t *gyro);
+bool l3gd20Detect(gyroDev_t *gyro);

src/main/drivers/accgyro_lsm303dlhc.c

@@ -115,7 +115,7 @@ int32_t accelSummedSamples100Hz[3];
 
 int32_t accelSummedSamples500Hz[3];
 
-void lsm303dlhcAccInit(acc_t *acc)
+void lsm303dlhcAccInit(accDev_t *acc)
 {
     i2cWrite(MPU_I2C_INSTANCE, LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT);
 
@@ -157,7 +157,7 @@ static bool lsm303dlhcAccRead(int16_t *gyroADC)
     return true;
 }
 
-bool lsm303dlhcAccDetect(acc_t *acc)
+bool lsm303dlhcAccDetect(accDev_t *acc)
 {
     bool ack;
     uint8_t status;

src/main/drivers/accgyro_lsm303dlhc.h

@@ -195,10 +195,10 @@ typedef struct {
 /** @defgroup Acc_Full_Scale_Selection
   * @{
   */
-#define LSM303DLHC_FULLSCALE_2G            ((uint8_t)0x00)  /*!< ±2 g */
+#define LSM303DLHC_FULLSCALE_2G            ((uint8_t)0x00)  /*!< +/-2 g */
-#define LSM303DLHC_FULLSCALE_4G            ((uint8_t)0x10)  /*!< ±4 g */
+#define LSM303DLHC_FULLSCALE_4G            ((uint8_t)0x10)  /*!< +/-4 g */
-#define LSM303DLHC_FULLSCALE_8G            ((uint8_t)0x20)  /*!< ±8 g */
+#define LSM303DLHC_FULLSCALE_8G            ((uint8_t)0x20)  /*!< +/-8 g */
-#define LSM303DLHC_FULLSCALE_16G           ((uint8_t)0x30)  /*!< ±16 g */
+#define LSM303DLHC_FULLSCALE_16G           ((uint8_t)0x30)  /*!< +/-16 g */
 /**
   * @}
   */
@@ -388,13 +388,13 @@ typedef struct {
 /** @defgroup Mag_Full_Scale
   * @{
   */
-#define  LSM303DLHC_FS_1_3_GA               ((uint8_t) 0x20)  /*!< Full scale = ±1.3 Gauss */
+#define  LSM303DLHC_FS_1_3_GA               ((uint8_t) 0x20)  /*!< Full scale = +/-1.3 Gauss */
-#define  LSM303DLHC_FS_1_9_GA               ((uint8_t) 0x40)  /*!< Full scale = ±1.9 Gauss */
+#define  LSM303DLHC_FS_1_9_GA               ((uint8_t) 0x40)  /*!< Full scale = +/-1.9 Gauss */
-#define  LSM303DLHC_FS_2_5_GA               ((uint8_t) 0x60)  /*!< Full scale = ±2.5 Gauss */
+#define  LSM303DLHC_FS_2_5_GA               ((uint8_t) 0x60)  /*!< Full scale = +/-2.5 Gauss */
-#define  LSM303DLHC_FS_4_0_GA               ((uint8_t) 0x80)  /*!< Full scale = ±4.0 Gauss */
+#define  LSM303DLHC_FS_4_0_GA               ((uint8_t) 0x80)  /*!< Full scale = +/-4.0 Gauss */
-#define  LSM303DLHC_FS_4_7_GA               ((uint8_t) 0xA0)  /*!< Full scale = ±4.7 Gauss */
+#define  LSM303DLHC_FS_4_7_GA               ((uint8_t) 0xA0)  /*!< Full scale = +/-4.7 Gauss */
-#define  LSM303DLHC_FS_5_6_GA               ((uint8_t) 0xC0)  /*!< Full scale = ±5.6 Gauss */
+#define  LSM303DLHC_FS_5_6_GA               ((uint8_t) 0xC0)  /*!< Full scale = +/-5.6 Gauss */
-#define  LSM303DLHC_FS_8_1_GA               ((uint8_t) 0xE0)  /*!< Full scale = ±8.1 Gauss */
+#define  LSM303DLHC_FS_8_1_GA               ((uint8_t) 0xE0)  /*!< Full scale = +/-8.1 Gauss */
 /**
   * @}
   */
@@ -438,5 +438,5 @@ typedef struct {
 #define LSM303DLHC_TEMPSENSOR_DISABLE        ((uint8_t) 0x00)   /*!< Temp sensor Disable */
 
 
-bool lsm303dlhcAccDetect(acc_t *acc);
+bool lsm303dlhcAccDetect(accDev_t *acc);
 

src/main/drivers/accgyro_mma845x.c

@@ -81,10 +81,10 @@
 
 static uint8_t device_id;
 
-static void mma8452Init(acc_t *acc);
+static void mma8452Init(accDev_t *acc);
 static bool mma8452Read(int16_t *accelData);
 
-bool mma8452Detect(acc_t *acc)
+bool mma8452Detect(accDev_t *acc)
 {
     bool ack = false;
     uint8_t sig = 0;
@@ -114,7 +114,7 @@ static inline void mma8451ConfigureInterrupt(void)
     i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG5, 0); // DRDY routed to INT2
 }
 
-static void mma8452Init(acc_t *acc)
+static void mma8452Init(accDev_t *acc)
 {
 
     i2cWrite(MPU_I2C_INSTANCE, MMA8452_ADDRESS, MMA8452_CTRL_REG1, 0); // Put device in standby to configure stuff

src/main/drivers/accgyro_mma845x.h

@@ -17,4 +17,4 @@
 
 #pragma once
 
-bool mma8452Detect(acc_t *acc);
+bool mma8452Detect(accDev_t *acc);

src/main/drivers/accgyro_mpu.c

@@ -51,8 +51,6 @@ static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
 
 static void mpu6050FindRevision(void);
 
-static volatile bool mpuDataReady;
-
 #ifdef USE_SPI
 static bool detectSPISensorsAndUpdateDetectionResult(void);
 #endif
@@ -144,6 +142,16 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
     }
 #endif
 
+#ifdef USE_GYRO_SPI_ICM20689
+    if (icm20689SpiDetect()) {
+        mpuDetectionResult.sensor = ICM_20689_SPI;
+        mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
+        mpuConfiguration.read = icm20689ReadRegister;
+        mpuConfiguration.write = icm20689WriteRegister;
+        return true;
+    }
+#endif
+
 #ifdef USE_GYRO_SPI_MPU6000
     if (mpu6000SpiDetect()) {
         mpuDetectionResult.sensor = MPU_60x0_SPI;
@@ -210,12 +218,21 @@ static void mpu6050FindRevision(void)
     }
 }
 
-extiCallbackRec_t mpuIntCallbackRec;
+typedef struct mpuIntRec_s {
+   extiCallbackRec_t exti;
+   gyroDev_t *gyro;
+} mpuIntRec_t;
 
-void mpuIntExtiHandler(extiCallbackRec_t *cb)
+mpuIntRec_t mpuIntRec;
+
+/*
+ * Gyro interrupt service routine
+ */
+static void mpuIntExtiHandler(extiCallbackRec_t *cb)
 {
-    UNUSED(cb);
+    mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
-    mpuDataReady = true;
+    gyroDev_t *gyro = rec->gyro;
+    gyro->dataReady = true;
 
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
     static uint32_t lastCalledAt = 0;
@@ -226,7 +243,7 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
 #endif
 }
 
-void mpuIntExtiInit(void)
+static void mpuIntExtiInit(gyroDev_t *gyro)
 {
     static bool mpuExtiInitDone = false;
 
@@ -234,6 +251,7 @@ void mpuIntExtiInit(void)
         return;
     }
 
+    mpuIntRec.gyro = gyro;
 #if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
 
     IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
@@ -245,12 +263,19 @@ void mpuIntExtiInit(void)
     }
 #endif
 
+#if defined (STM32F7)
+    IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
+    EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
+    EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));   // TODO - maybe pullup / pulldown ?
+#else
+
     IOInit(mpuIntIO, OWNER_MPU, RESOURCE_EXTI, 0);
     IOConfigGPIO(mpuIntIO, IOCFG_IN_FLOATING);   // TODO - maybe pullup / pulldown ?
 
-    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
+    EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
-    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
+    EXTIConfig(mpuIntIO, &mpuIntRec.exti, NVIC_PRIO_MPU_INT_EXTI, EXTI_Trigger_Rising);
     EXTIEnable(mpuIntIO, true);
+#endif
 #endif
 
     mpuExtiInitDone = true;
@@ -284,28 +309,33 @@ bool mpuAccRead(int16_t *accData)
     return true;
 }
 
-bool mpuGyroRead(int16_t *gyroADC)
+bool mpuGyroRead(gyroDev_t *gyro)
 {
     uint8_t data[6];
 
-    bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
+    const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
     if (!ack) {
         return false;
     }
 
-    gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
+    gyro->gyroADCRaw[X] = (int16_t)((data[0] << 8) | data[1]);
-    gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
+    gyro->gyroADCRaw[Y] = (int16_t)((data[2] << 8) | data[3]);
-    gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
+    gyro->gyroADCRaw[Z] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }
 
-bool checkMPUDataReady(void)
+void mpuGyroInit(gyroDev_t *gyro)
+{
+    mpuIntExtiInit(gyro);
+}
+
+bool checkMPUDataReady(gyroDev_t* gyro)
 {
     bool ret;
-    if (mpuDataReady) {
+    if (gyro->dataReady) {
         ret = true;
-        mpuDataReady= false;
+        gyro->dataReady= false;
     } else {
         ret = false;
     }

src/main/drivers/accgyro_mpu.h

@@ -17,7 +17,6 @@
 
 #pragma once
 
-#include "io_types.h"
 #include "exti.h"
 
 // MPU6050
@@ -168,7 +167,8 @@ typedef enum {
     MPU_60x0_SPI,
     MPU_65xx_I2C,
     MPU_65xx_SPI,
-    MPU_9250_SPI
+    MPU_9250_SPI,
+    ICM_20689_SPI
 } detectedMPUSensor_e;
 
 typedef enum {
@@ -184,8 +184,9 @@ typedef struct mpuDetectionResult_s {
 extern mpuDetectionResult_t mpuDetectionResult;
 
 void configureMPUDataReadyInterruptHandling(void);
-void mpuIntExtiInit(void);
+struct gyroDev_s;
+void mpuGyroInit(struct gyroDev_s *gyro);
 bool mpuAccRead(int16_t *accData);
-bool mpuGyroRead(int16_t *gyroADC);
+bool mpuGyroRead(struct gyroDev_s *gyro);
 mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
-bool checkMPUDataReady(void);
+bool checkMPUDataReady(struct gyroDev_s *gyro);

src/main/drivers/accgyro_mpu3050.c

@@ -46,10 +46,10 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-static void mpu3050Init(uint8_t lpf);
+static void mpu3050Init(gyroDev_t *gyro);
 static bool mpu3050ReadTemp(int16_t *tempData);
 
-bool mpu3050Detect(gyro_t *gyro)
+bool mpu3050Detect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_3050) {
         return false;
@@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
     return true;
 }
 
-static void mpu3050Init(uint8_t lpf)
+static void mpu3050Init(gyroDev_t *gyro)
 {
     bool ack;
 
@@ -75,7 +75,7 @@ static void mpu3050Init(uint8_t lpf)
     if (!ack)
         failureMode(FAILURE_ACC_INIT);
 
-    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | lpf);
+    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
     mpuConfiguration.write(MPU3050_INT_CFG, 0);
     mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
     mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);

src/main/drivers/accgyro_mpu3050.h

@@ -26,4 +26,4 @@
 #define MPU3050_USER_CTRL       0x3D
 #define MPU3050_PWR_MGM         0x3E
 
-bool mpu3050Detect(gyro_t *gyro);
+bool mpu3050Detect(gyroDev_t *gyro);

src/main/drivers/accgyro_mpu6050.c

@@ -50,10 +50,10 @@
 
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
-static void mpu6050AccInit(acc_t *acc);
+static void mpu6050AccInit(accDev_t *acc);
-static void mpu6050GyroInit(uint8_t lpf);
+static void mpu6050GyroInit(gyroDev_t *gyro);
 
-bool mpu6050AccDetect(acc_t *acc)
+bool mpu6050AccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
@@ -66,7 +66,7 @@ bool mpu6050AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6050GyroDetect(gyro_t *gyro)
+bool mpu6050GyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0) {
         return false;
@@ -81,13 +81,11 @@ bool mpu6050GyroDetect(gyro_t *gyro)
     return true;
 }
 
-static void mpu6050AccInit(acc_t *acc)
+static void mpu6050AccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     switch (mpuDetectionResult.resolution) {
         case MPU_HALF_RESOLUTION:
-            acc->acc_1G = 256 * 8;
+            acc->acc_1G = 256 * 4;
             break;
         case MPU_FULL_RESOLUTION:
             acc->acc_1G = 512 * 8;
@@ -95,18 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
     }
 }
 
-static void mpu6050GyroInit(uint8_t lpf)
+static void mpu6050GyroInit(gyroDev_t *gyro)
 {
-    bool ack;
+    mpuGyroInit(gyro);
 
-    mpuIntExtiInit();
+    bool ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
-
-    ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
     ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure
-    ack = mpuConfiguration.write(MPU_RA_CONFIG, lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
+    ack = mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
     ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
 
     // ACC Init stuff.

src/main/drivers/accgyro_mpu6050.h

@@ -17,5 +17,5 @@
 
 #pragma once
 
-bool mpu6050AccDetect(acc_t *acc);
+bool mpu6050AccDetect(accDev_t *acc);
-bool mpu6050GyroDetect(gyro_t *gyro);
+bool mpu6050GyroDetect(gyroDev_t *gyro);

src/main/drivers/accgyro_mpu6500.c

@@ -34,7 +34,7 @@
 #include "accgyro_mpu.h"
 #include "accgyro_mpu6500.h"
 
-bool mpu6500AccDetect(acc_t *acc)
+bool mpu6500AccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
         return false;
@@ -46,7 +46,7 @@ bool mpu6500AccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6500GyroDetect(gyro_t *gyro)
+bool mpu6500GyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
         return false;
@@ -62,16 +62,14 @@ bool mpu6500GyroDetect(gyro_t *gyro)
     return true;
 }
 
-void mpu6500AccInit(acc_t *acc)
+void mpu6500AccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
+    acc->acc_1G = 512 * 4;
-
-    acc->acc_1G = 512 * 8;
 }
 
-void mpu6500GyroInit(uint8_t lpf)
+void mpu6500GyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
 #ifdef NAZE
     // FIXME target specific code in driver code.
@@ -98,7 +96,7 @@ void mpu6500GyroInit(uint8_t lpf)
     delay(15);
     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
     delay(15);
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf);
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider
     delay(100);

src/main/drivers/accgyro_mpu6500.h

@@ -20,6 +20,7 @@
 #define MPU6500_WHO_AM_I_CONST              (0x70)
 #define MPU9250_WHO_AM_I_CONST              (0x71)
 #define ICM20608G_WHO_AM_I_CONST            (0xAF)
+#define ICM20602_WHO_AM_I_CONST             (0x12)
 
 #define MPU6500_BIT_RESET                   (0x80)
 #define MPU6500_BIT_INT_ANYRD_2CLEAR        (1 << 4)
@@ -27,8 +28,8 @@
 #define MPU6500_BIT_I2C_IF_DIS              (1 << 4)
 #define MPU6500_BIT_RAW_RDY_EN              (0x01)
 
-bool mpu6500AccDetect(acc_t *acc);
+bool mpu6500AccDetect(accDev_t *acc);
-bool mpu6500GyroDetect(gyro_t *gyro);
+bool mpu6500GyroDetect(gyroDev_t *gyro);
 
-void mpu6500AccInit(acc_t *acc);
+void mpu6500AccInit(accDev_t *acc);
-void mpu6500GyroInit(uint8_t lpf);
+void mpu6500GyroInit(gyroDev_t *gyro);

src/main/drivers/accgyro_spi_mpu6000.c

@@ -124,32 +124,29 @@ bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu6000SpiGyroInit(uint8_t lpf)
+void mpu6000SpiGyroInit(gyroDev_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpu6000AccAndGyroInit();
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
     // Accel and Gyro DLPF Setting
-    mpu6000WriteRegister(MPU6000_CONFIG, lpf);
+    mpu6000WriteRegister(MPU6000_CONFIG, gyro->lpf);
     delayMicroseconds(1);
 
     spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_CLOCK_FAST);  // 18 MHz SPI clock
 
-    int16_t data[3];
+    mpuGyroRead(gyro);
-    mpuGyroRead(data);
 
-    if (((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) {
+    if (((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) {
         failureMode(FAILURE_GYRO_INIT_FAILED);
     }
 }
 
-void mpu6000SpiAccInit(acc_t *acc)
+void mpu6000SpiAccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 8;
 }
 
@@ -180,7 +177,6 @@ bool mpu6000SpiDetect(void)
         }
     } while (attemptsRemaining--);
 
-
     mpu6000ReadRegister(MPU_RA_PRODUCT_ID, 1, &in);
 
     /* look for a product ID we recognise */
@@ -207,7 +203,6 @@ bool mpu6000SpiDetect(void)
 
 static void mpu6000AccAndGyroInit(void)
 {
-
     if (mpuSpi6000InitDone) {
         return;
     }
@@ -245,7 +240,6 @@ static void mpu6000AccAndGyroInit(void)
     mpu6000WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
     delayMicroseconds(15);
 
-
     mpu6000WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR
     delayMicroseconds(15);
 
@@ -260,7 +254,7 @@ static void mpu6000AccAndGyroInit(void)
     mpuSpi6000InitDone = true;
 }
 
-bool mpu6000SpiAccDetect(acc_t *acc)
+bool mpu6000SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;
@@ -272,7 +266,7 @@ bool mpu6000SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu6000SpiGyroDetect(gyro_t *gyro)
+bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu6000.h

@@ -17,8 +17,8 @@
 
 bool mpu6000SpiDetect(void);
 
-bool mpu6000SpiAccDetect(acc_t *acc);
+bool mpu6000SpiAccDetect(accDev_t *acc);
-bool mpu6000SpiGyroDetect(gyro_t *gyro);
+bool mpu6000SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/drivers/accgyro_spi_mpu6500.c

@@ -84,19 +84,22 @@ bool mpu6500SpiDetect(void)
 
     mpu6500ReadRegister(MPU_RA_WHO_AM_I, 1, &tmp);
 
-    if (tmp == MPU6500_WHO_AM_I_CONST || tmp == MPU9250_WHO_AM_I_CONST || tmp == ICM20608G_WHO_AM_I_CONST) {
+    if (tmp == MPU6500_WHO_AM_I_CONST ||
+        tmp == MPU9250_WHO_AM_I_CONST ||
+        tmp == ICM20608G_WHO_AM_I_CONST ||
+        tmp == ICM20602_WHO_AM_I_CONST) {
         return true;
     }
 
     return false;
 }
 
-void mpu6500SpiAccInit(acc_t *acc)
+void mpu6500SpiAccInit(accDev_t *acc)
 {
     mpu6500AccInit(acc);
 }
 
-bool mpu6500SpiAccDetect(acc_t *acc)
+bool mpu6500SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
@@ -108,12 +111,12 @@ bool mpu6500SpiAccDetect(acc_t *acc)
     return true;
 }
 
-void mpu6500SpiGyroInit(uint8_t lpf)
+void mpu6500SpiGyroInit(gyroDev_t *gyro)
 {
     spiSetDivisor(MPU6500_SPI_INSTANCE, SPI_CLOCK_SLOW);
     delayMicroseconds(1);
 
-    mpu6500GyroInit(lpf);
+    mpu6500GyroInit(gyro);
 
     // Disable Primary I2C Interface
     mpu6500WriteRegister(MPU_RA_USER_CTRL, MPU6500_BIT_I2C_IF_DIS);
@@ -123,7 +126,7 @@ void mpu6500SpiGyroInit(uint8_t lpf)
     delayMicroseconds(1);
 }
 
-bool mpu6500SpiGyroDetect(gyro_t *gyro)
+bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu6500.h

@@ -19,11 +19,11 @@
 
 bool mpu6500SpiDetect(void);
 
-bool mpu6500SpiAccDetect(acc_t *acc);
+bool mpu6500SpiAccDetect(accDev_t *acc);
-bool mpu6500SpiGyroDetect(gyro_t *gyro);
+bool mpu6500SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
 bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
 
-void mpu6500SpiGyroInit(uint8_t lpf);
+void mpu6500SpiGyroInit(gyroDev_t *gyro);
-void mpu6500SpiAccInit(acc_t *acc);
+void mpu6500SpiAccInit(accDev_t *acc);

src/main/drivers/accgyro_spi_mpu9250.c

@@ -96,31 +96,26 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu9250SpiGyroInit(uint8_t lpf)
+void mpu9250SpiGyroInit(gyroDev_t *gyro)
 {
-    (void)(lpf);
+    mpuGyroInit(gyro);
 
-    mpuIntExtiInit();
+    mpu9250AccAndGyroInit(gyro->lpf);
-
-    mpu9250AccAndGyroInit(lpf);
 
     spiResetErrorCounter(MPU9250_SPI_INSTANCE);
 
     spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST); //high speed now that we don't need to write to the slow registers
 
-    int16_t data[3];
+    mpuGyroRead(gyro);
-    mpuGyroRead(data);
 
-    if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
+    if ((((int8_t)gyro->gyroADCRaw[1]) == -1 && ((int8_t)gyro->gyroADCRaw[0]) == -1) || spiGetErrorCounter(MPU9250_SPI_INSTANCE) != 0) {
         spiResetErrorCounter(MPU9250_SPI_INSTANCE);
         failureMode(FAILURE_GYRO_INIT_FAILED);
     }
 }
 
-void mpu9250SpiAccInit(acc_t *acc)
+void mpu9250SpiAccInit(accDev_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 8;
 }
 
@@ -214,7 +209,7 @@ bool mpu9250SpiDetect(void)
     return true;
 }
 
-bool mpu9250SpiAccDetect(acc_t *acc)
+bool mpu9250SpiAccDetect(accDev_t *acc)
 {
     if (mpuDetectionResult.sensor != MPU_9250_SPI) {
         return false;
@@ -226,7 +221,7 @@ bool mpu9250SpiAccDetect(acc_t *acc)
     return true;
 }
 
-bool mpu9250SpiGyroDetect(gyro_t *gyro)
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
 {
     if (mpuDetectionResult.sensor != MPU_9250_SPI) {
         return false;

src/main/drivers/accgyro_spi_mpu9250.h

@@ -27,8 +27,8 @@ void mpu9250ResetGyro(void);
 
 bool mpu9250SpiDetect(void);
 
-bool mpu9250SpiAccDetect(acc_t *acc);
+bool mpu9250SpiAccDetect(accDev_t *acc);
-bool mpu9250SpiGyroDetect(gyro_t *gyro);
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
 
 bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
 bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);

src/main/drivers/gyro_sync.c

@@ -14,37 +14,23 @@
  * You should have received a copy of the GNU General Public License
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
+
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
 
 #include "platform.h"
-#include "build/build_config.h"
 
-
+#include "sensor.h"
-#include "common/axis.h"
+#include "accgyro.h"
-#include "common/maths.h"
+#include "gyro_sync.h"
-
-#include "drivers/sensor.h"
-#include "drivers/accgyro.h"
-#include "drivers/gyro_sync.h"
-
-#include "fc/runtime_config.h"
-
-#include "config/config.h"
-
-extern gyro_t gyro;
 
 static uint8_t mpuDividerDrops;
 
-bool getMpuDataStatus(gyro_t *gyro)
+bool gyroSyncCheckUpdate(gyroDev_t *gyro)
 {
-    return gyro->intStatus();
+    if (!gyro->intStatus)
-}
+        return false;
-
+    return gyro->intStatus(gyro);
-bool gyroSyncCheckUpdate(void)
-{
-    return getMpuDataStatus(&gyro);
 }
 
 uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator)

src/main/drivers/gyro_sync.h

@@ -15,8 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define INTERRUPT_WAIT_TIME 10
+struct gyroDev_s;
-
+bool gyroSyncCheckUpdate(struct gyroDev_s *gyro);
-bool gyroSyncCheckUpdate(void);
 uint8_t gyroMPU6xxxCalculateDivider(void);
 uint32_t gyroSetSampleRate(uint32_t looptime, uint8_t lpf, uint8_t gyroSync, uint8_t gyroSyncDenominator);

src/main/drivers/sensor.h

@@ -17,9 +17,12 @@
 
 #pragma once
 
-struct acc_s;
+struct accDev_s;
 typedef bool (*sensorInitFuncPtr)(void);                    // sensor init prototype
 typedef bool (*sensorReadFuncPtr)(int16_t *data);           // sensor read and align prototype
-typedef void (*sensorAccInitFuncPtr)(struct acc_s *acc);    // sensor init prototype
+typedef void (*sensorAccInitFuncPtr)(struct accDev_s *acc);    // sensor init prototype
-typedef void (*sensorGyroInitFuncPtr)(uint8_t lpf);         // gyro sensor init prototype
+struct gyroDev_s;
-typedef bool (*sensorInterruptFuncPtr)(void);               // sensor Interrupt Data Ready
+typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);
+typedef bool (*sensorInterruptFuncPtr)(void);

src/main/fc/fc_msp.c

@@ -537,7 +537,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
     case MSP_RAW_IMU:
         {
             // Hack scale due to choice of units for sensor data in multiwii
-            const uint8_t scale = (acc.acc_1G > 1024) ? 8 : 1;
+            const uint8_t scale = (acc.dev.acc_1G > 1024) ? 8 : 1;
             for (int i = 0; i < 3; i++) {
                 sbufWriteU16(dst, accADC[i] / scale);
             }

src/main/fc/fc_tasks.c

@@ -274,7 +274,7 @@ void fcTasksInit(void)
     }
 
 #else
-    rescheduleTask(TASK_GYROPID, gyro.targetLooptime);
+    rescheduleTask(TASK_GYROPID, gyro.dev.targetLooptime);
     setTaskEnabled(TASK_GYROPID, true);
 #endif
 

src/main/fc/mw.c

@@ -182,8 +182,8 @@ void annexCode(void)
     }
 
     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
-    if (gyro.temperature)
+    if (gyro.dev.temperature)
-        gyro.temperature(&telemTemperature1);
+        gyro.dev.temperature(&telemTemperature1);
 }
 
 void mwDisarm(void)
@@ -485,7 +485,7 @@ void filterRc(bool isRXDataNew)
     #ifdef ASYNC_GYRO_PROCESSING
         biquadFilterInitLPF(&filteredCycleTimeState, 1, getPidUpdateRate());
     #else
-        biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime);
+        biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.dev.targetLooptime);
     #endif
         filterInitialised = true;
     }
@@ -523,9 +523,9 @@ void taskGyro(timeUs_t currentTimeUs) {
     if (masterConfig.gyroConfig.gyroSync) {
         while (true) {
         #ifdef ASYNC_GYRO_PROCESSING
-            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
+            if (gyroSyncCheckUpdate(&gyro.dev) || ((currentDeltaTime + (micros() - currentTimeUs)) >= (getGyroUpdateRate() + GYRO_WATCHDOG_DELAY))) {
         #else
-            if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTimeUs)) >= (gyro.targetLooptime + GYRO_WATCHDOG_DELAY))) {
+            if (gyroSyncCheckUpdate(&gyro.dev) || ((currentDeltaTime + (micros() - currentTimeUs)) >= (gyro.dev.targetLooptime + GYRO_WATCHDOG_DELAY))) {
         #endif
                 break;
             }

src/main/flight/imu.c

@@ -30,7 +30,6 @@
 
 #include "build/build_config.h"
 
-
 #include "common/axis.h"
 #include "common/filter.h"
 
@@ -155,7 +154,7 @@ void imuInit(void)
     int axis;
 
     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
-    gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
+    gyroScale = gyro.dev.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
 
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         imuAccelInBodyFrame.A[axis] = 0;
@@ -417,7 +416,7 @@ static int imuCalculateAccelerometerConfidence(void)
     }
 
     // Magnitude^2 in percent of G^2
-    accMagnitude = accMagnitude * 100 / ((int32_t)acc.acc_1G * acc.acc_1G);
+    accMagnitude = accMagnitude * 100 / ((int32_t)acc.dev.acc_1G * acc.dev.acc_1G);
 
     int32_t nearness = ABS(100 - accMagnitude);
 
@@ -521,7 +520,7 @@ static void imuUpdateMeasuredAcceleration(void)
 #else
     /* Convert acceleration to cm/s/s */
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
+        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G);
         imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
     }
 #endif
@@ -573,7 +572,7 @@ void imuUpdateAccelerometer(void)
 
 #ifdef ASYNC_GYRO_PROCESSING
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuAccumulatedAcc[axis] += accADC[axis] * (GRAVITY_CMSS / acc.acc_1G);
+        imuAccumulatedAcc[axis] += accADC[axis] * (GRAVITY_CMSS / acc.dev.acc_1G);
     }
     imuAccumulatedAccCount++;
 #endif

src/main/flight/pid.c

@@ -519,7 +519,7 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
 
     for (int axis = 0; axis < 3; axis++) {
         // Step 1: Calculate gyro rates
-        pidState[axis].gyroRate = gyroADC[axis] * gyro.scale;
+        pidState[axis].gyroRate = gyroADC[axis] * gyro.dev.scale;
 
         // Step 2: Read target
         float rateTarget;

src/main/flight/servos.c

@@ -446,7 +446,7 @@ void filterServos(void)
         // Initialize servo lowpass filter (servos are calculated at looptime rate)
         if (!servoFilterIsSet) {
             for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-                biquadFilterInitLPF(&servoFitlerState[servoIdx], servoMixerConfig->servo_lowpass_freq, gyro.targetLooptime);
+                biquadFilterInitLPF(&servoFitlerState[servoIdx], servoMixerConfig->servo_lowpass_freq, gyro.dev.targetLooptime);
             }
 
             servoFilterIsSet = true;

src/main/io/serial_cli.c

@@ -2900,8 +2900,8 @@ static void cliStatus(char *cmdline)
 
             cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware);
 
-            if (mask == SENSOR_ACC && acc.revisionCode) {
+            if (mask == SENSOR_ACC && acc.dev.revisionCode) {
-                cliPrintf(".%c", acc.revisionCode);
+                cliPrintf(".%c", acc.dev.revisionCode);
             }
         }
     }

src/main/sensors/acceleration.c

@@ -53,6 +53,8 @@ static biquadFilter_t accFilter[XYZ_AXIS_COUNT];
 
 void accInit(uint32_t targetLooptime)
 {
+    acc.dev.acc_1G = 256; // set default
+    acc.dev.init(&acc.dev);
     accTargetLooptime = targetLooptime;
     if (accLpfCutHz) {
         for (int axis = 0; axis < 3; axis++) {
@@ -159,7 +161,7 @@ void performAcclerationCalibration(void)
             accSample[Y] = accSamples[axis][Y] / CALIBRATING_ACC_CYCLES - accZero->raw[Y];
             accSample[Z] = accSamples[axis][Z] / CALIBRATING_ACC_CYCLES - accZero->raw[Z];
 
-            sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.acc_1G);
+            sensorCalibrationPushSampleForScaleCalculation(&calState, axis / 2, accSample, acc.dev.acc_1G);
         }
 
         sensorCalibrationSolveForScale(&calState, accTmp);
@@ -183,7 +185,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
 
 void updateAccelerationReadings(void)
 {
-    if (!acc.read(accADCRaw)) {
+    if (!acc.dev.read(accADCRaw)) {
         return;
     }
 

src/main/sensors/acceleration.h

@@ -37,6 +37,10 @@ typedef enum {
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 
+typedef struct acc_s {
+    accDev_t dev;
+} acc_t;
+
 extern sensor_align_e accAlign;
 extern acc_t acc;
 

src/main/sensors/gyro.c

@@ -25,6 +25,8 @@
 #include "common/maths.h"
 #include "common/filter.h"
 
+#include "drivers/gyro_sync.h"
+
 #include "io/beeper.h"
 #include "io/statusindicator.h"
 
@@ -48,17 +50,20 @@ static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
 
 void gyroInit(const gyroConfig_t *gyroConfigToUse)
 {
-    /*
-     * After refactoring this function is always called after gyro sampling rate is known, so
-     * no additional condition is required
-     */
     gyroConfig = gyroConfigToUse;
+    // After refactoring this function is always called after gyro sampling rate is known, so
+    // no additional condition is required
+    // Set gyro sample rate before driver initialisation
+    gyro.dev.lpf = gyroConfig->gyro_lpf;
+    gyro.dev.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
+    // driver initialisation
+    gyro.dev.init(&gyro.dev);
     if (gyroConfig->gyro_soft_lpf_hz) {
         for (int axis = 0; axis < 3; axis++) {
         #ifdef ASYNC_GYRO_PROCESSING
             biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, getGyroUpdateRate());
         #else
-            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.targetLooptime);
+            biquadFilterInitLPF(&gyroFilterLPF[axis], gyroConfig->gyro_soft_lpf_hz, gyro.dev.targetLooptime);
         #endif
         }
     }
@@ -125,17 +130,15 @@ static void performAcclerationCalibration(uint8_t gyroMovementCalibrationThresho
 
 void gyroUpdate(void)
 {
-    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-
     // range: +/- 8192; +/- 2000 deg/sec
-    if (!gyro.read(gyroADCRaw)) {
+    if (!gyro.dev.read(&gyro.dev)) {
         return;
     }
 
     // Prepare a copy of int32_t gyroADC for mangling to prevent overflow
-    gyroADC[X] = gyroADCRaw[X];
+    gyroADC[X] = gyro.dev.gyroADCRaw[X];
-    gyroADC[Y] = gyroADCRaw[Y];
+    gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
-    gyroADC[Z] = gyroADCRaw[Z];
+    gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
 
     if (gyroConfig->gyro_soft_lpf_hz) {
         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {

src/main/sensors/gyro.h

@@ -29,10 +29,13 @@ typedef enum {
     GYRO_MPU6000,
     GYRO_MPU6500,
     GYRO_MPU9250,
-    GYRO_FAKE,
+    GYRO_FAKE
-    GYRO_MAX = GYRO_FAKE
 } gyroSensor_e;
 
+typedef struct gyro_s {
+    gyroDev_t dev;
+} gyro_t;
+
 extern gyro_t gyro;
 
 extern int32_t gyroADC[XYZ_AXIS_COUNT];

src/main/sensors/initialisation.c

@@ -49,7 +49,6 @@
 #include "drivers/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro_spi_mpu6500.h"
 #include "drivers/accgyro_spi_mpu9250.h"
-#include "drivers/gyro_sync.h"
 
 #include "drivers/barometer.h"
 #include "drivers/barometer_bmp085.h"
@@ -111,7 +110,7 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
 #endif
 }
 
-bool detectGyro(void)
+static bool detectGyro(gyroDev_t *dev)
 {
     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 
@@ -122,7 +121,7 @@ bool detectGyro(void)
             ; // fallthrough
         case GYRO_MPU6050:
 #ifdef USE_GYRO_MPU6050
-            if (mpu6050GyroDetect(&gyro)) {
+            if (mpu6050GyroDetect(dev)) {
                 gyroHardware = GYRO_MPU6050;
 #ifdef GYRO_MPU6050_ALIGN
                 gyroAlign = GYRO_MPU6050_ALIGN;
@@ -133,7 +132,7 @@ bool detectGyro(void)
             ; // fallthrough
         case GYRO_L3G4200D:
 #ifdef USE_GYRO_L3G4200D
-            if (l3g4200dDetect(&gyro)) {
+            if (l3g4200dDetect(dev)) {
                 gyroHardware = GYRO_L3G4200D;
 #ifdef GYRO_L3G4200D_ALIGN
                 gyroAlign = GYRO_L3G4200D_ALIGN;
@@ -145,7 +144,7 @@ bool detectGyro(void)
 
         case GYRO_MPU3050:
 #ifdef USE_GYRO_MPU3050
-            if (mpu3050Detect(&gyro)) {
+            if (mpu3050Detect(dev)) {
                 gyroHardware = GYRO_MPU3050;
 #ifdef GYRO_MPU3050_ALIGN
                 gyroAlign = GYRO_MPU3050_ALIGN;
@@ -157,7 +156,7 @@ bool detectGyro(void)
 
         case GYRO_L3GD20:
 #ifdef USE_GYRO_L3GD20
-            if (l3gd20Detect(&gyro)) {
+            if (l3gd20Detect(dev)) {
                 gyroHardware = GYRO_L3GD20;
 #ifdef GYRO_L3GD20_ALIGN
                 gyroAlign = GYRO_L3GD20_ALIGN;
@@ -169,7 +168,7 @@ bool detectGyro(void)
 
         case GYRO_MPU6000:
 #ifdef USE_GYRO_SPI_MPU6000
-            if (mpu6000SpiGyroDetect(&gyro)) {
+            if (mpu6000SpiGyroDetect(dev)) {
                 gyroHardware = GYRO_MPU6000;
 #ifdef GYRO_MPU6000_ALIGN
                 gyroAlign = GYRO_MPU6000_ALIGN;
@@ -182,9 +181,9 @@ bool detectGyro(void)
         case GYRO_MPU6500:
 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 #ifdef USE_GYRO_SPI_MPU6500
-            if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro)) {
+            if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 #else
-            if (mpu6500GyroDetect(&gyro)) {
+            if (mpu6500GyroDetect(dev)) {
 #endif
                 gyroHardware = GYRO_MPU6500;
 #ifdef GYRO_MPU6500_ALIGN
@@ -198,7 +197,7 @@ bool detectGyro(void)
 
     case GYRO_MPU9250:
 #ifdef USE_GYRO_SPI_MPU9250
-        if (mpu9250SpiGyroDetect(&gyro))
+        if (mpu9250SpiGyroDetect(dev))
         {
             gyroHardware = GYRO_MPU9250;
 #ifdef GYRO_MPU9250_ALIGN
@@ -211,7 +210,7 @@ bool detectGyro(void)
         ; // fallthrough
         case GYRO_FAKE:
 #ifdef USE_FAKE_GYRO
-            if (fakeGyroDetect(&gyro)) {
+            if (fakeGyroDetect(dev)) {
                 gyroHardware = GYRO_FAKE;
                 break;
             }
@@ -233,7 +232,7 @@ bool detectGyro(void)
     return true;
 }
 
-static bool detectAcc(accelerationSensor_e accHardwareToUse)
+static bool detectAcc(accDev_t *dev, accelerationSensor_e accHardwareToUse)
 {
     accelerationSensor_e accHardware;
 
@@ -252,9 +251,9 @@ retry:
             acc_params.useFifo = false;
             acc_params.dataRate = 800; // unused currently
 #ifdef NAZE
-            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(&acc_params, &acc)) {
+            if (hardwareRevision < NAZE32_REV5 && adxl345Detect(dev, &acc_params)) {
 #else
-            if (adxl345Detect(&acc_params, &acc)) {
+            if (adxl345Detect(dev, &acc_params)) {
 #endif
 #ifdef ACC_ADXL345_ALIGN
                 accAlign = ACC_ADXL345_ALIGN;
@@ -266,7 +265,7 @@ retry:
             ; // fallthrough
         case ACC_LSM303DLHC:
 #ifdef USE_ACC_LSM303DLHC
-            if (lsm303dlhcAccDetect(&acc)) {
+            if (lsm303dlhcAccDetect(dev)) {
 #ifdef ACC_LSM303DLHC_ALIGN
                 accAlign = ACC_LSM303DLHC_ALIGN;
 #endif
@@ -277,7 +276,7 @@ retry:
             ; // fallthrough
         case ACC_MPU6050: // MPU6050
 #ifdef USE_ACC_MPU6050
-            if (mpu6050AccDetect(&acc)) {
+            if (mpu6050AccDetect(dev)) {
 #ifdef ACC_MPU6050_ALIGN
                 accAlign = ACC_MPU6050_ALIGN;
 #endif
@@ -290,9 +289,9 @@ retry:
 #ifdef USE_ACC_MMA8452
 #ifdef NAZE
             // Not supported with this frequency
-            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(&acc)) {
+            if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
 #else
-            if (mma8452Detect(&acc)) {
+            if (mma8452Detect(dev)) {
 #endif
 #ifdef ACC_MMA8452_ALIGN
                 accAlign = ACC_MMA8452_ALIGN;
@@ -304,7 +303,7 @@ retry:
             ; // fallthrough
         case ACC_BMA280: // BMA280
 #ifdef USE_ACC_BMA280
-            if (bma280Detect(&acc)) {
+            if (bma280Detect(dev)) {
 #ifdef ACC_BMA280_ALIGN
                 accAlign = ACC_BMA280_ALIGN;
 #endif
@@ -315,7 +314,7 @@ retry:
             ; // fallthrough
         case ACC_MPU6000:
 #ifdef USE_ACC_SPI_MPU6000
-            if (mpu6000SpiAccDetect(&acc)) {
+            if (mpu6000SpiAccDetect(dev)) {
 #ifdef ACC_MPU6000_ALIGN
                 accAlign = ACC_MPU6000_ALIGN;
 #endif
@@ -327,9 +326,9 @@ retry:
         case ACC_MPU6500:
 #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
 #ifdef USE_ACC_SPI_MPU6500
-            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
+            if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
 #else
-            if (mpu6500AccDetect(&acc)) {
+            if (mpu6500AccDetect(dev)) {
 #endif
 #ifdef ACC_MPU6500_ALIGN
                 accAlign = ACC_MPU6500_ALIGN;
@@ -340,7 +339,7 @@ retry:
 #endif
         case ACC_MPU9250:
 #ifdef USE_ACC_SPI_MPU9250
-            if (mpu9250SpiAccDetect(&acc)) {
+            if (mpu9250SpiAccDetect(dev)) {
 #ifdef ACC_MPU9250_ALIGN
                 accAlign = ACC_MPU9250_ALIGN;
 #endif
@@ -351,7 +350,7 @@ retry:
             ; // fallthrough
         case ACC_FAKE:
 #ifdef USE_FAKE_ACC
-            if (fakeAccDetect(&acc)) {
+            if (fakeAccDetect(dev)) {
                 accHardware = ACC_FAKE;
                 break;
             }
@@ -680,38 +679,26 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
         int16_t magDeclinationFromConfig,
         const gyroConfig_t *gyroConfig)
 {
-    memset(&acc, 0, sizeof(acc));
-    memset(&gyro, 0, sizeof(gyro));
-
 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250)
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
     detectMpu(extiConfig);
 #endif
 
-    if (!detectGyro()) {
+    memset(&gyro, 0, sizeof(gyro));
+    if (!detectGyro(&gyro.dev)) {
         return false;
     }
+    gyroInit(gyroConfig);
 
-    // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
+    memset(&acc, 0, sizeof(acc));
-    // Set gyro sample rate before initialisation
+    if (detectAcc(&acc.dev, sensorSelectionConfig->acc_hardware)) {
-    gyro.targetLooptime = gyroSetSampleRate(gyroConfig->looptime, gyroConfig->gyro_lpf, gyroConfig->gyroSync, gyroConfig->gyroSyncDenominator);
+#ifdef ASYNC_GYRO_PROCESSING
-    gyro.init(gyroConfig->gyro_lpf); // driver initialisation
+         // ACC will be updated at its own rate
-    gyroInit(gyroConfig); // sensor initialisation
-
-    if (detectAcc(sensorSelectionConfig->acc_hardware)) {
-        acc.acc_1G = 256; // set default
-        acc.init(&acc);
-    #ifdef ASYNC_GYRO_PROCESSING
-        /*
-         * ACC will be updated at its own rate
-         */
         accInit(getAccUpdateRate());
-    #else
+#else
-        /*
+        // acc updated at same frequency in taskMainPidLoop in mw.c
-         * acc updated at same frequency in taskMainPidLoop in mw.c
+        accInit(gyro.dev.targetLooptime);
-         */
+#endif
-        accInit(gyro.targetLooptime);
-    #endif
     }
 
 #ifdef BARO

src/main/telemetry/frsky.c

@@ -164,7 +164,7 @@ static void sendAccel(void)
 
     for (i = 0; i < 3; i++) {
         sendDataHead(ID_ACC_X + i);
-        serialize16(accADC[i] * 1000 / acc.acc_1G);
+        serialize16(accADC[i] * 1000 / acc.dev.acc_1G);
     }
 }