Changed gyro init and read functions to take a gyro_t* parameter.

Scaled gyro values to degrees per second in gyroUpdate.

src/main/blackbox/blackbox.c

@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -990,7 +991,7 @@ static void loadMainState(uint32_t currentTime)
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {
-        blackboxCurrent->gyroADC[i] = gyroADC[i];
+        blackboxCurrent->gyroADC[i] = lrintf(gyroADCf[i]);
     }
 
     for (i = 0; i < XYZ_AXIS_COUNT; i++) {

src/main/common/filter.h

@@ -15,6 +15,8 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#pragma once
+
 #ifdef STM32F10X
 #define MAX_FIR_DENOISE_WINDOW_SIZE 60
 #else

src/main/drivers/accgyro.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "common/axis.h"
+#include "common/filter.h"
 #include "drivers/sensor.h"
 
 #ifndef MPU_I2C_INSTANCE
@@ -34,11 +36,14 @@
 
 typedef struct gyro_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;
+    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
     float scale;                                            // scalefactor
     uint32_t targetLooptime;
+    uint16_t lpf;
+    volatile bool dataReady;
 } gyro_t;
 
 typedef struct acc_s {

src/main/drivers/accgyro_l3g4200d.c

@@ -54,8 +54,8 @@
 #define L3G4200D_DLPF_78HZ       0x80
 #define L3G4200D_DLPF_93HZ       0xC0
 
-static void l3g4200dInit(uint8_t lpf);
+static void l3g4200dInit(gyro_t *gyro);
-static bool l3g4200dRead(int16_t *gyroADC);
+static bool l3g4200dRead(gyro_t *gyro);
 
 bool l3g4200dDetect(gyro_t *gyro)
 {
@@ -76,13 +76,13 @@ bool l3g4200dDetect(gyro_t *gyro)
     return true;
 }
 
-static void l3g4200dInit(uint8_t lpf)
+static void l3g4200dInit(gyro_t *gyro)
 {
     bool ack;
 
     uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
 
-    switch (lpf) {
+    switch (gyro->lpf) {
         default:
             case 32:
             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(gyro_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_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(gyro_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(gyro_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]);

src/main/drivers/accgyro_mpu.c

@@ -25,7 +25,9 @@
 #include "build/build_config.h"
 #include "build/debug.h"
 
+#include "common/filter.h"
 #include "common/maths.h"
+#include "common/utils.h"
 
 #include "nvic.h"
 
@@ -52,8 +54,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
@@ -221,12 +221,22 @@ static void mpu6050FindRevision(void)
     }
 }
 
-extiCallbackRec_t mpuIntCallbackRec;
+typedef struct mpuIntRec_s {
+   extiCallbackRec_t exti;
+   gyro_t *gyro;
+} mpuIntRec_t;
 
-void mpuIntExtiHandler(extiCallbackRec_t *cb)
+mpuIntRec_t mpuIntRec;
+
+/*
+ * Gyro interrupt service routine
+ */
+#if defined(MPU_INT_EXTI)
+static void mpuIntExtiHandler(extiCallbackRec_t *cb)
 {
-    UNUSED(cb);
+    mpuIntRec_t *rec = container_of(cb, mpuIntRec_t, exti);
-    mpuDataReady = true;
+    gyro_t *gyro = rec->gyro;
+    gyro->dataReady = true;
 
 #ifdef DEBUG_MPU_DATA_READY_INTERRUPT
     static uint32_t lastCalledAt = 0;
@@ -236,17 +246,18 @@ void mpuIntExtiHandler(extiCallbackRec_t *cb)
     lastCalledAt = now;
 #endif
 }
+#endif
 
-void mpuIntExtiInit(void)
+static void mpuIntExtiInit(gyro_t *gyro)
 {
+    mpuIntRec.gyro = gyro;
+#if defined(MPU_INT_EXTI)
     static bool mpuExtiInitDone = false;
 
     if (mpuExtiInitDone || !mpuIntExtiConfig) {
         return;
     }
 
-#if defined(USE_MPU_DATA_READY_SIGNAL) && defined(USE_EXTI)
-
     IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
 
 #ifdef ENSURE_MPU_DATA_READY_IS_LOW
@@ -258,20 +269,20 @@ void mpuIntExtiInit(void)
 
 #if defined (STM32F7)
     IOInit(mpuIntIO, OWNER_MPU_EXTI, 0);
-    EXTIHandlerInit(&mpuIntCallbackRec, mpuIntExtiHandler);
+    EXTIHandlerInit(&mpuIntRec.exti, mpuIntExtiHandler);
-    EXTIConfig(mpuIntIO, &mpuIntCallbackRec, NVIC_PRIO_MPU_INT_EXTI, IO_CONFIG(GPIO_MODE_INPUT,0,GPIO_NOPULL));   // TODO - maybe pullup / pulldown ?
+    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_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;
+#endif
 }
 
 static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
@@ -302,28 +313,33 @@ bool mpuAccRead(int16_t *accData)
     return true;
 }
 
-bool mpuGyroRead(int16_t *gyroADC)
+bool mpuGyroRead(gyro_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(gyro_t *gyro)
+{
+    mpuIntExtiInit(gyro);
+}
+
+bool checkMPUDataReady(gyro_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

@@ -185,8 +185,9 @@ typedef struct mpuDetectionResult_s {
 extern mpuDetectionResult_t mpuDetectionResult;
 
 void configureMPUDataReadyInterruptHandling(void);
-void mpuIntExtiInit(void);
+struct gyro_s;
+void mpuGyroInit(struct gyro_s *gyro);
 bool mpuAccRead(int16_t *accData);
-bool mpuGyroRead(int16_t *gyroADC);
+bool mpuGyroRead(struct gyro_s *gyro);
 mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse);
-bool checkMPUDataReady(void);
+bool checkMPUDataReady(struct gyro_s *gyro);

src/main/drivers/accgyro_mpu3050.c

@@ -46,7 +46,7 @@
 #define MPU3050_USER_RESET      0x01
 #define MPU3050_CLK_SEL_PLL_GX  0x01
 
-static void mpu3050Init(uint8_t lpf);
+static void mpu3050Init(gyro_t *gyro);
 static bool mpu3050ReadTemp(int16_t *tempData);
 
 bool mpu3050Detect(gyro_t *gyro)
@@ -65,7 +65,7 @@ bool mpu3050Detect(gyro_t *gyro)
     return true;
 }
 
-static void mpu3050Init(uint8_t lpf)
+static void mpu3050Init(gyro_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_mpu6050.c

@@ -51,7 +51,7 @@
 #define MPU6050_SMPLRT_DIV      0       // 8000Hz
 
 static void mpu6050AccInit(acc_t *acc);
-static void mpu6050GyroInit(uint8_t lpf);
+static void mpu6050GyroInit(gyro_t *gyro);
 
 bool mpu6050AccDetect(acc_t *acc)
 {
@@ -83,8 +83,6 @@ bool mpu6050GyroDetect(gyro_t *gyro)
 
 static void mpu6050AccInit(acc_t *acc)
 {
-    mpuIntExtiInit();
-
     switch (mpuDetectionResult.resolution) {
         case MPU_HALF_RESOLUTION:
             acc->acc_1G = 256 * 4;
@@ -95,16 +93,16 @@ static void mpu6050AccInit(acc_t *acc)
     }
 }
 
-static void mpu6050GyroInit(uint8_t lpf)
+static void mpu6050GyroInit(gyro_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80);      //PWR_MGMT_1    -- DEVICE_RESET 1
     delay(100);
     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)
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //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
-    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)
+    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)
     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_mpu6500.c

@@ -64,14 +64,12 @@ bool mpu6500GyroDetect(gyro_t *gyro)
 
 void mpu6500AccInit(acc_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 
-void mpu6500GyroInit(uint8_t lpf)
+void mpu6500GyroInit(gyro_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
@@ -85,7 +83,7 @@ void mpu6500GyroInit(uint8_t lpf)
     delay(15);
     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf);
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro_mpu6500.h

@@ -32,4 +32,4 @@ bool mpu6500AccDetect(acc_t *acc);
 bool mpu6500GyroDetect(gyro_t *gyro);
 
 void mpu6500AccInit(acc_t *acc);
-void mpu6500GyroInit(uint8_t lpf);
+void mpu6500GyroInit(gyro_t *gyro);

src/main/drivers/accgyro_spi_icm20689.c

@@ -137,14 +137,12 @@ bool icm20689SpiGyroDetect(gyro_t *gyro)
 
 void icm20689AccInit(acc_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 
-void icm20689GyroInit(uint8_t lpf)
+void icm20689GyroInit(gyro_t *gyro)
 {
-    mpuIntExtiInit();
+    mpuGyroInit(gyro);
 
     spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
 
@@ -160,7 +158,7 @@ void icm20689GyroInit(uint8_t lpf)
     delay(15);
     mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
     delay(15);
-    mpuConfiguration.write(MPU_RA_CONFIG, lpf);
+    mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
     delay(15);
     mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
     delay(100);

src/main/drivers/accgyro_spi_icm20689.h

@@ -23,7 +23,7 @@ bool icm20689AccDetect(acc_t *acc);
 bool icm20689GyroDetect(gyro_t *gyro);
 
 void icm20689AccInit(acc_t *acc);
-void icm20689GyroInit(uint8_t lpf);
+void icm20689GyroInit(gyro_t *gyro);
 
 bool icm20689SpiDetect(void);
 
@@ -31,4 +31,4 @@ bool icm20689SpiAccDetect(acc_t *acc);
 bool icm20689SpiGyroDetect(gyro_t *gyro);
 
 bool icm20689WriteRegister(uint8_t reg, uint8_t data);
-bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
+bool icm20689ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

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(gyro_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)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 4;
 }
 

src/main/drivers/accgyro_spi_mpu6500.c

@@ -99,12 +99,12 @@ void mpu6500SpiAccInit(acc_t *acc)
     mpu6500AccInit(acc);
 }
 
-void mpu6500SpiGyroInit(uint8_t lpf)
+void mpu6500SpiGyroInit(gyro_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);

src/main/drivers/accgyro_spi_mpu6500.h

@@ -20,7 +20,7 @@
 bool mpu6500SpiDetect(void);
 
 void mpu6500SpiAccInit(acc_t *acc);
-void mpu6500SpiGyroInit(uint8_t lpf);
+void mpu6500SpiGyroInit(gyro_t *gyro);
 
 bool mpu6500SpiAccDetect(acc_t *acc);
 bool mpu6500SpiGyroDetect(gyro_t *gyro);

src/main/drivers/accgyro_spi_mpu9250.c

@@ -96,22 +96,19 @@ bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
     return true;
 }
 
-void mpu9250SpiGyroInit(uint8_t lpf)
+void mpu9250SpiGyroInit(gyro_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);
     }
@@ -119,8 +116,6 @@ void mpu9250SpiGyroInit(uint8_t lpf)
 
 void mpu9250SpiAccInit(acc_t *acc)
 {
-    mpuIntExtiInit();
-
     acc->acc_1G = 512 * 8;
 }
 

src/main/drivers/gyro_sync.c

@@ -16,11 +16,11 @@
 
 static uint8_t mpuDividerDrops;
 
-bool gyroSyncCheckUpdate(const gyro_t *gyro)
+bool gyroSyncCheckUpdate(gyro_t *gyro)
 {
     if (!gyro->intStatus)
         return false;
-    return gyro->intStatus();
+    return gyro->intStatus(gyro);
 }
 
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator)

src/main/drivers/gyro_sync.h

@@ -6,6 +6,6 @@
  */
 
 struct gyro_s;
-bool gyroSyncCheckUpdate(const struct gyro_s *gyro);
+bool gyroSyncCheckUpdate(struct gyro_s *gyro);
 uint8_t gyroMPU6xxxGetDividerDrops(void);
 uint32_t gyroSetSampleRate(uint8_t lpf, uint8_t gyroSyncDenominator);

src/main/drivers/sensor.h

@@ -21,5 +21,8 @@ struct acc_s;
 typedef void (*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 (*sensorGyroInitFuncPtr)(uint8_t lpf);         // gyro sensor init prototype
+struct gyro_s;
-typedef bool (*sensorInterruptFuncPtr)(void);               // sensor Interrupt Data Ready
+typedef void (*sensorGyroInitFuncPtr)(struct gyro_s *gyro);
+typedef bool (*sensorGyroReadFuncPtr)(struct gyro_s *gyro);
+typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyro_s *gyro);
+typedef bool (*sensorInterruptFuncPtr)(void);

src/main/fc/fc_msp.c

@@ -612,7 +612,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
                 sbufWriteU16(dst, accSmooth[i] / scale);
             }
             for (int i = 0; i < 3; i++) {
-                sbufWriteU16(dst, gyroADC[i]);
+                sbufWriteU16(dst, lrintf(gyroADCf[i] / gyro.scale));
             }
             for (int i = 0; i < 3; i++) {
                 sbufWriteU16(dst, magADC[i]);

src/main/flight/imu.c

@@ -75,8 +75,6 @@ static float rMat[3][3];
 
 attitudeEulerAngles_t attitude = { { 0, 0, 0 } };     // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
 
-static float gyroScale;
-
 STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
 {
     float q1q1 = sq(q1);
@@ -122,7 +120,6 @@ void imuConfigure(
 void imuInit(void)
 {
     smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
-    gyroScale = gyro.scale * (M_PIf / 180.0f);  // gyro output scaled to rad per second
     accVelScale = 9.80665f / acc.acc_1G / 10000.0f;
 
     imuComputeRotationMatrix();
@@ -399,7 +396,7 @@ static void imuCalculateEstimatedAttitude(uint32_t currentTime)
 #endif
 
     imuMahonyAHRSupdate(deltaT * 1e-6f,
-                        gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
+                        DEGREES_TO_RADIANS(gyroADCf[X]), DEGREES_TO_RADIANS(gyroADCf[Y]), DEGREES_TO_RADIANS(gyroADCf[Z]),
                         useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
                         useMag, magADC[X], magADC[Y], magADC[Z],
                         useYaw, rawYawError);

src/main/flight/pid.c

@@ -244,7 +244,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
             }
         }
 
-        const float PVRate = gyroADCf[axis] / 16.4f; // Process variable from gyro output in deg/sec
+        const float PVRate = gyroADCf[axis]; // Process variable from gyro output in deg/sec
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
         //  ---------- 2-DOF PID controller with optional filter on derivative term. b = 1 and only c can be tuned (amount derivative on measurement or error).  ----------

src/main/io/dashboard.c

@@ -18,6 +18,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -495,7 +496,7 @@ void showSensorsPage(void)
     }
 
     if (sensors(SENSOR_GYRO)) {
-        tfp_sprintf(lineBuffer, format, "GYR", gyroADC[X], gyroADC[Y], gyroADC[Z]);
+        tfp_sprintf(lineBuffer, format, "GYR", lrintf(gyroADCf[X]), lrintf(gyroADCf[Y]), lrintf(gyroADCf[Z]));
         padLineBuffer();
         i2c_OLED_set_line(rowIndex++);
         i2c_OLED_send_string(lineBuffer);

src/main/sensors/gyro.c

@@ -39,7 +39,7 @@
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-int32_t gyroADC[XYZ_AXIS_COUNT];
+static int32_t gyroADC[XYZ_AXIS_COUNT];
 float gyroADCf[XYZ_AXIS_COUNT];
 
 static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
@@ -174,41 +174,39 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
 
 void gyroUpdate(void)
 {
-    int16_t gyroADCRaw[XYZ_AXIS_COUNT];
-
     // range: +/- 8192; +/- 2000 deg/sec
-    if (!gyro.read(gyroADCRaw)) {
+    if (!gyro.read(&gyro)) {
         return;
     }
-
+    gyro.dataReady = false;
-    gyroADC[X] = gyroADCRaw[X];
+    gyroADC[X] = gyro.gyroADCRaw[X];
-    gyroADC[Y] = gyroADCRaw[Y];
+    gyroADC[Y] = gyro.gyroADCRaw[Y];
-    gyroADC[Z] = gyroADCRaw[Z];
+    gyroADC[Z] = gyro.gyroADCRaw[Z];
 
     alignSensors(gyroADC, gyroAlign);
 
-    if (!isGyroCalibrationComplete()) {
+    const bool calibrationComplete = isGyroCalibrationComplete();
+    if (!calibrationComplete) {
         performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
     }
 
-    gyroADC[X] -= gyroZero[X];
-    gyroADC[Y] -= gyroZero[Y];
-    gyroADC[Z] -= gyroZero[Z];
-
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        gyroADC[axis] -= gyroZero[axis];
+        // scale gyro output to degrees per second
+        gyroADCf[axis] = (float)gyroADC[axis] * gyro.scale;
 
-        if (debugMode == DEBUG_GYRO)
+        DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf[axis]));
-            debug[axis] = gyroADC[axis];
 
-        gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
+        gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf[axis]);
 
-        if (debugMode == DEBUG_NOTCH)
+        DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf[axis]));
-            debug[axis] = lrintf(gyroADCf[axis]);
 
         gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
 
         gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
 
-        gyroADC[axis] = lrintf(gyroADCf[axis]);
+        if (!calibrationComplete) {
+            gyroADC[axis] = lrintf(gyroADCf[axis] / gyro.scale);
+        }
     }
 }

src/main/sensors/gyro.h

@@ -37,7 +37,6 @@ typedef enum {
 
 extern gyro_t gyro;
 
-extern int32_t gyroADC[XYZ_AXIS_COUNT];
 extern float gyroADCf[XYZ_AXIS_COUNT];
 
 typedef struct gyroConfig_s {

src/main/sensors/initialisation.c

@@ -102,15 +102,15 @@ const extiConfig_t *selectMPUIntExtiConfig(void)
 
 #ifdef USE_FAKE_GYRO
 int16_t fake_gyro_values[XYZ_AXIS_COUNT] = { 0,0,0 };
-static void fakeGyroInit(uint8_t lpf)
+static void fakeGyroInit(gyro_t *gyro)
 {
-    UNUSED(lpf);
+    UNUSED(gyro);
 }
 
-static bool fakeGyroRead(int16_t *gyroADC)
+static bool fakeGyroRead(gyro_t *gyro)
 {
     for (int i = 0; i < XYZ_AXIS_COUNT; ++i) {
-        gyroADC[i] = fake_gyro_values[i];
+        gyro->gyroADCRaw[X] = fake_gyro_values[i];
     }
 
     return true;
@@ -123,7 +123,9 @@ static bool fakeGyroReadTemp(int16_t *tempData)
 }
 
 
-static bool fakeGyroInitStatus(void) {
+static bool fakeGyroInitStatus(gyro_t *gyro)
+{
+    UNUSED(gyro);
     return true;
 }
 
@@ -662,7 +664,8 @@ bool sensorsAutodetect(const sensorAlignmentConfig_t *sensorAlignmentConfig,
     // Now time to init things
     // this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
     gyro.targetLooptime = gyroSetSampleRate(gyroConfig->gyro_lpf, gyroConfig->gyro_sync_denom);    // Set gyro sample rate before initialisation
-    gyro.init(gyroConfig->gyro_lpf); // driver initialisation
+    gyro.lpf = gyroConfig->gyro_lpf;
+    gyro.init(&gyro); // driver initialisation
     gyroInit(gyroConfig); // sensor initialisation
 
     if (detectAcc(sensorSelectionConfig->acc_hardware)) {

src/main/target/COLIBRI_RACE/i2c_bst.c

@@ -613,7 +613,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
                 for (i = 0; i < 3; i++)
                     bstWrite16(accSmooth[i] / scale);
                 for (i = 0; i < 3; i++)
-                    bstWrite16(gyroADC[i]);
+                    bstWrite16(lrintf(gyroADCf[i]));
                 for (i = 0; i < 3; i++)
                     bstWrite16(magADC[i]);
             }