Added direct support for MPU9250 gyro

src/main/drivers/accgyro_mpu.c

@@ -42,6 +42,7 @@
 #include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6000.h"
 #include "accgyro_spi_mpu6500.h"
+#include "accgyro_spi_mpu9250.h"
 #include "accgyro_mpu.h"
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
@@ -242,6 +243,19 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
     }
 #endif
 
+#ifdef  USE_GYRO_SPI_MPU9250
+    if (mpu9250SpiDetect()) {
+        gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
+        gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
+        gyro->mpuConfiguration.read = mpu9250ReadRegister;
+        gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
+        gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
+        gyro->mpuConfiguration.write = mpu9250WriteRegister;
+        gyro->mpuConfiguration.reset = mpu9250ResetGyro;
+        return true;
+    }
+#endif
+
 #ifdef USE_GYRO_SPI_ICM20608
     if (icm20608SpiDetect()) {
         mpuDetectionResult.sensor = ICM_20608_SPI;

src/main/drivers/accgyro_spi_mpu9250.c

@@ -0,0 +1,235 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+
+/*
+ * Authors:
+ * Dominic Clifton - Cleanflight implementation
+ * John Ihlein - Initial FF32 code
+ * Kalyn Doerr (RS2K) - Robust rewrite
+*/
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "io.h"
+
+#include "system.h"
+#include "exti.h"
+#include "bus_spi.h"
+#include "gyro_sync.h"
+#include "light_led.h"
+
+#include "sensor.h"
+#include "accgyro.h"
+#include "accgyro_mpu.h"
+#include "accgyro_spi_mpu9250.h"
+
+static bool mpuSpi9250InitDone = false;
+
+static IO_t mpuSpi9250CsPin = IO_NONE;
+
+#define DISABLE_MPU9250       IOHi(mpuSpi9250CsPin)
+#define ENABLE_MPU9250        IOLo(mpuSpi9250CsPin)
+
+bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
+{
+    ENABLE_MPU9250;
+    spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
+    spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
+    DISABLE_MPU9250;
+
+    return true;
+}
+
+bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
+{
+    ENABLE_MPU9250;
+    delayMicroseconds(1);
+    spiTransferByte(MPU9250_SPI_INSTANCE, reg | 0x80); // read transaction
+    spiTransfer(MPU9250_SPI_INSTANCE, data, NULL, length);
+    DISABLE_MPU9250;
+    delayMicroseconds(1);
+
+    return true;
+}
+
+bool mpu9250WriteRegister(uint8_t reg, uint8_t data)
+{
+    ENABLE_MPU9250;
+    delayMicroseconds(1);
+    spiTransferByte(MPU9250_SPI_INSTANCE, reg);
+    spiTransferByte(MPU9250_SPI_INSTANCE, data);
+    DISABLE_MPU9250;
+    delayMicroseconds(1);
+
+    return true;
+}
+
+bool verifympu9250WriteRegister(uint8_t reg, uint8_t data)
+{
+    mpu9250WriteRegister(reg, data);
+    delayMicroseconds(100);
+
+    uint8_t attemptsRemaining = 20;
+    do {
+        uint8_t in;
+        mpu9250SlowReadRegister(reg, 1, &in);
+        if (in == data) {
+            return true;
+        } else {
+            debug[3]++;
+            mpu9250WriteRegister(reg, data);
+            delayMicroseconds(100);
+        }
+    } while (attemptsRemaining--);
+    return false;
+}
+
+void mpu9250ResetGyro(void)
+{
+    // Device Reset
+    mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
+    delay(150);
+}
+
+static void mpu9250AccAndGyroInit(gyroDev_t *gyro)
+{
+    if (mpuSpi9250InitDone) {
+        return;
+    }
+
+    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed for writing to slow registers
+
+    mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
+    delay(50);
+
+    verifympu9250WriteRegister(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
+
+    //Fchoice_b defaults to 00 which makes fchoice 11
+    const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
+    verifympu9250WriteRegister(MPU_RA_GYRO_CONFIG, raGyroConfigData);
+
+    if (gyro->lpf == 4) {
+        verifympu9250WriteRegister(MPU_RA_CONFIG, 1); //1KHz, 184DLPF
+    } else if (gyro->lpf < 4) {
+        verifympu9250WriteRegister(MPU_RA_CONFIG, 7); //8KHz, 3600DLPF
+    } else if (gyro->lpf > 4) {
+        verifympu9250WriteRegister(MPU_RA_CONFIG, 0); //8KHz, 250DLPF
+    }
+
+    verifympu9250WriteRegister(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro));
+
+    verifympu9250WriteRegister(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    verifympu9250WriteRegister(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0);  // INT_ANYRD_2CLEAR, BYPASS_EN
+
+#if defined(USE_MPU_DATA_READY_SIGNAL)
+    verifympu9250WriteRegister(MPU_RA_INT_ENABLE, 0x01); //this resets register MPU_RA_PWR_MGMT_1 and won't read back correctly.
+#endif
+
+    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+
+    mpuSpi9250InitDone = true; //init done
+}
+
+bool mpu9250SpiDetect(void)
+{
+    /* not the best place for this - should really have an init method */
+#ifdef MPU9250_CS_PIN
+    mpuSpi9250CsPin = IOGetByTag(IO_TAG(MPU9250_CS_PIN));
+#endif
+    IOInit(mpuSpi9250CsPin, OWNER_MPU, RESOURCE_SPI_CS, 0);
+    IOConfigGPIO(mpuSpi9250CsPin, SPI_IO_CS_CFG);
+
+    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); //low speed
+    mpu9250WriteRegister(MPU_RA_PWR_MGMT_1, MPU9250_BIT_RESET);
+
+    uint8_t attemptsRemaining = 20;
+    do {
+        delay(150);
+        uint8_t in;
+        mpu9250ReadRegister(MPU_RA_WHO_AM_I, 1, &in);
+        if (in == MPU9250_WHO_AM_I_CONST) {
+            break;
+        }
+        if (!attemptsRemaining) {
+            return false;
+        }
+    } while (attemptsRemaining--);
+
+    spiSetDivisor(MPU9250_SPI_INSTANCE, SPI_CLOCK_FAST);
+
+    return true;
+}
+
+static void mpu9250SpiAccInit(accDev_t *acc)
+{
+    acc->acc_1G = 512 * 8;
+}
+
+bool mpu9250SpiAccDetect(accDev_t *acc)
+{
+    if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
+        return false;
+    }
+
+    acc->init = mpu9250SpiAccInit;
+    acc->read = mpuAccRead;
+
+    return true;
+}
+
+static void mpu9250SpiGyroInit(gyroDev_t *gyro)
+{
+    mpuGyroInit(gyro);
+
+    mpu9250AccAndGyroInit(gyro);
+
+    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
+
+    mpuGyroRead(gyro);
+
+    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);
+    }
+}
+
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
+{
+    if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
+        return false;
+    }
+
+    gyro->init = mpu9250SpiGyroInit;
+    gyro->read = mpuGyroRead;
+    gyro->intStatus = mpuCheckDataReady;
+
+    // 16.4 dps/lsb scalefactor
+    gyro->scale = 1.0f / 16.4f;
+
+    return true;
+}

src/main/drivers/accgyro_spi_mpu9250.h

@@ -0,0 +1,36 @@
+
+#pragma once
+
+#define mpu9250_CONFIG                      0x1A
+
+/* We should probably use these. :)
+#define BITS_DLPF_CFG_256HZ         0x00
+#define BITS_DLPF_CFG_188HZ         0x01
+#define BITS_DLPF_CFG_98HZ          0x02
+#define BITS_DLPF_CFG_42HZ          0x03
+#define BITS_DLPF_CFG_20HZ          0x04
+#define BITS_DLPF_CFG_10HZ          0x05
+#define BITS_DLPF_CFG_5HZ           0x06
+#define BITS_DLPF_CFG_2100HZ_NOLPF  0x07
+*/
+
+#define GYRO_SCALE_FACTOR  0.00053292f  // (4/131) * pi/180   (32.75 LSB = 1 DPS)
+
+#define MPU9250_WHO_AM_I_CONST              (0x71)
+
+#define MPU9250_BIT_RESET                   (0x80)
+
+// RF = Register Flag
+#define MPU_RF_DATA_RDY_EN (1 << 0)
+
+void mpu9250ResetGyro(void);
+
+bool mpu9250SpiDetect(void);
+
+bool mpu9250SpiAccDetect(accDev_t *acc);
+bool mpu9250SpiGyroDetect(gyroDev_t *gyro);
+
+bool mpu9250WriteRegister(uint8_t reg, uint8_t data);
+bool verifympu9250WriteRegister(uint8_t reg, uint8_t data);
+bool mpu9250ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
+bool mpu9250SlowReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/fc/cli.c

@@ -163,9 +163,9 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */
 // sync with gyroSensor_e
-static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"};
+static const char * const gyroNames[] = { "NONE", "AUTO", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "FAKE"};
 // sync with accelerationSensor_e
-static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"};
+static const char * const lookupTableAccHardware[] = { "NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "FAKE"};
 #if (FLASH_SIZE > 64)
 // sync with baroSensor_e
 static const char * const lookupTableBaroHardware[] = { "NONE", "AUTO", "BMP085", "MS5611", "BMP280", "FAKE"};

src/main/sensors/acceleration.c

@@ -44,6 +44,7 @@
 #include "drivers/accgyro_lsm303dlhc.h"
 #include "drivers/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro_spi_mpu6500.h"
+#include "drivers/accgyro_spi_mpu9250.h"
 #include "drivers/logging.h"
 #include "drivers/sensor.h"
 
@@ -94,7 +95,6 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     accelerationSensor_e accHardware = ACC_NONE;
 
 #ifdef USE_ACC_ADXL345
-    drv_adxl345_config_t acc_params;
 #endif
 
     dev->accAlign = ALIGN_DEFAULT;
@@ -104,8 +104,9 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
     switch (accHardwareToUse) {
     case ACC_AUTODETECT:
         ; // fallthrough
-    case ACC_ADXL345: // ADXL345
 #ifdef USE_ACC_ADXL345
+    case ACC_ADXL345: {
+        drv_adxl345_config_t acc_params;
         acc_params.useFifo = false;
         acc_params.dataRate = 800; // unused currently
 #ifdef NAZE
@@ -119,14 +120,15 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_ADXL345;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+    }
+#endif
 
-    case ACC_LSM303DLHC:
 #ifdef USE_ACC_LSM303DLHC
+    case ACC_LSM303DLHC:
         if (lsm303dlhcAccDetect(dev)) {
 #ifdef ACC_LSM303DLHC_ALIGN
             dev->accAlign = ACC_LSM303DLHC_ALIGN;
@@ -134,14 +136,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_LSM303DLHC;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
-    case ACC_MPU6050: // MPU6050
 #ifdef USE_ACC_MPU6050
+    case ACC_MPU6050: // MPU6050
         if (mpu6050AccDetect(dev)) {
 #ifdef ACC_MPU6050_ALIGN
             dev->accAlign = ACC_MPU6050_ALIGN;
@@ -149,14 +151,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_MPU6050;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
-    case ACC_MMA8452: // MMA8452
 #ifdef USE_ACC_MMA8452
+    case ACC_MMA8452: // MMA8452
 #ifdef NAZE
         // Not supported with this frequency
         if (hardwareRevision < NAZE32_REV5 && mma8452Detect(dev)) {
@@ -169,14 +171,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_MMA8452;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
-    case ACC_BMA280: // BMA280
 #ifdef USE_ACC_BMA280
+    case ACC_BMA280: // BMA280
         if (bma280Detect(dev)) {
 #ifdef ACC_BMA280_ALIGN
             dev->accAlign = ACC_BMA280_ALIGN;
@@ -184,14 +186,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_BMA280;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
-    case ACC_MPU6000:
 #ifdef USE_ACC_SPI_MPU6000
+    case ACC_MPU6000:
         if (mpu6000SpiAccDetect(dev)) {
 #ifdef ACC_MPU6000_ALIGN
             dev->accAlign = ACC_MPU6000_ALIGN;
@@ -199,14 +201,14 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_MPU6000;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
-    case ACC_MPU6500:
 #if defined(USE_ACC_MPU6500) || defined(USE_ACC_SPI_MPU6500)
+    case ACC_MPU6500:
 #ifdef USE_ACC_SPI_MPU6500
         if (mpu6500AccDetect(dev) || mpu6500SpiAccDetect(dev)) {
 #else
@@ -218,24 +220,40 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
             accHardware = ACC_MPU6500;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
+
+#if defined(USE_ACC_SPI_MPU9250)
+    case ACC_MPU9250:
+        if (mpu9250SpiAccDetect(dev)) {
+#ifdef ACC_MPU9250_ALIGN
+            dev->accAlign = ACC_MPU9250_ALIGN;
+#endif
+            accHardware = ACC_MPU9250;
+            break;
+        }
+        /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+        if (accHardwareToUse != ACC_AUTODETECT) {
+            break;
+        }
+#endif
 
-    case ACC_FAKE:
 #ifdef USE_FAKE_ACC
+    case ACC_FAKE:
         if (fakeAccDetect(dev)) {
             accHardware = ACC_FAKE;
             break;
         }
-#endif
         /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
         if (accHardwareToUse != ACC_AUTODETECT) {
             break;
         }
+#endif
 
+    default:
     case ACC_NONE: // disable ACC
         accHardware = ACC_NONE;
         break;

src/main/sensors/acceleration.h

@@ -33,7 +33,8 @@ typedef enum {
     ACC_LSM303DLHC = 6,
     ACC_MPU6000 = 7,
     ACC_MPU6500 = 8,
-    ACC_FAKE = 9,
+    ACC_MPU9250 = 9,
+    ACC_FAKE = 10,
     ACC_MAX = ACC_FAKE
 } accelerationSensor_e;
 

src/main/sensors/gyro.c

@@ -45,6 +45,7 @@
 #include "drivers/accgyro_lsm303dlhc.h"
 #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/io.h"
 #include "drivers/logging.h"
@@ -106,9 +107,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_soft_notch_cutoff_2 = 1
 );
 
-#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)
+#if defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050) || defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250)
+#define USE_GYRO_MPU
+#endif
+
 static const extiConfig_t *selectMPUIntExtiConfig(void)
 {
+#ifdef USE_GYRO_MPU
 #if defined(MPU_INT_EXTI)
     static const extiConfig_t mpuIntExtiConfig = { .tag = IO_TAG(MPU_INT_EXTI) };
     return &mpuIntExtiConfig;
@@ -116,9 +121,12 @@ static const extiConfig_t *selectMPUIntExtiConfig(void)
     return selectMPUIntExtiConfigByHardwareRevision();
 #else
     return NULL;
+#endif // MPU_INT_EXTI
+    return NULL;
+#else
+    return NULL;
 #endif
 }
-#endif
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
 {
@@ -126,9 +134,10 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 
     switch(gyroHardware) {
     case GYRO_AUTODETECT:
-        ; // fallthrough
+        // fallthrough
-    case GYRO_MPU6050:
+
 #ifdef USE_GYRO_MPU6050
+    case GYRO_MPU6050:
         if (mpu6050GyroDetect(dev)) {
             gyroHardware = GYRO_MPU6050;
 #ifdef GYRO_MPU6050_ALIGN
@@ -136,10 +145,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
+
-    case GYRO_L3G4200D:
 #ifdef USE_GYRO_L3G4200D
+    case GYRO_L3G4200D:
         if (l3g4200dDetect(dev)) {
             gyroHardware = GYRO_L3G4200D;
 #ifdef GYRO_L3G4200D_ALIGN
@@ -147,11 +157,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
 
-    case GYRO_MPU3050:
 #ifdef USE_GYRO_MPU3050
+    case GYRO_MPU3050:
         if (mpu3050Detect(dev)) {
             gyroHardware = GYRO_MPU3050;
 #ifdef GYRO_MPU3050_ALIGN
@@ -159,11 +169,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
 
-    case GYRO_L3GD20:
 #ifdef USE_GYRO_L3GD20
+    case GYRO_L3GD20:
         if (l3gd20Detect(dev)) {
             gyroHardware = GYRO_L3GD20;
 #ifdef GYRO_L3GD20_ALIGN
@@ -171,11 +181,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
 
-    case GYRO_MPU6000:
 #ifdef USE_GYRO_SPI_MPU6000
+    case GYRO_MPU6000:
         if (mpu6000SpiGyroDetect(dev)) {
             gyroHardware = GYRO_MPU6000;
 #ifdef GYRO_MPU6000_ALIGN
@@ -183,11 +193,11 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
 
-    case GYRO_MPU6500:
 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
+    case GYRO_MPU6500:
 #ifdef USE_GYRO_SPI_MPU6500
         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 #else
@@ -199,17 +209,30 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 #endif
             break;
         }
+        // fallthrough
+#endif
+
+#ifdef USE_GYRO_SPI_MPU9250
+    case GYRO_MPU9250:
+        if (mpu9250SpiGyroDetect(dev)) {
+            gyroHardware = GYRO_MPU9250;
+#ifdef GYRO_MPU9250_ALIGN
+            dev->gyroAlign = GYRO_MPU9250_ALIGN;
+#endif
+            break;
+        }
+        // fallthrough
 #endif
-        ; // fallthrough
 
-    case GYRO_FAKE:
 #ifdef USE_FAKE_GYRO
+    case GYRO_FAKE:
         if (fakeGyroDetect(dev)) {
             gyroHardware = GYRO_FAKE;
             break;
         }
+        // fallthrough
 #endif
-        ; // fallthrough
+
     default:
     case GYRO_NONE:
         gyroHardware = GYRO_NONE;
@@ -227,12 +250,10 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard
 bool gyroInit(void)
 {
     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)
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
+#ifdef USE_GYRO_MPU
     mpuDetect(&gyro.dev);
     mpuReset = gyro.dev.mpuConfiguration.reset;
-#else
-    const extiConfig_t *extiConfig = NULL;
 #endif
     gyro.dev.mpuIntExtiConfig =  extiConfig;
     if (gyroDetect(&gyro.dev, GYRO_AUTODETECT) == GYRO_NONE) {

src/main/sensors/gyro.h

@@ -30,6 +30,7 @@ typedef enum {
     GYRO_L3GD20,
     GYRO_MPU6000,
     GYRO_MPU6500,
+    GYRO_MPU9250,
     GYRO_FAKE
 } gyroSensor_e;
 

src/main/target/BLUEJAYF4/target.h

@@ -43,18 +43,16 @@
 #define USE_MPU_DATA_READY_SIGNAL
 //#define EXTI_CALLBACK_HANDLER_COUNT 1 // MPU data ready
 #define MPU_INT_EXTI            PC5
-#define MPU6500_CS_PIN          PC4
+#define MPU9250_CS_PIN          PC4
-#define MPU6500_SPI_INSTANCE    SPI1
+#define MPU9250_SPI_INSTANCE    SPI1
 
 #define ACC
-#define USE_ACC_MPU6500
+#define USE_ACC_SPI_MPU9250
-#define USE_ACC_SPI_MPU6500
+#define ACC_MPU9250_ALIGN       CW0_DEG
-#define ACC_MPU6500_ALIGN       CW0_DEG
 
 #define GYRO
-#define USE_GYRO_MPU6500
+#define USE_GYRO_SPI_MPU9250
-#define USE_GYRO_SPI_MPU6500
+#define GYRO_MPU9250_ALIGN      CW0_DEG
-#define GYRO_MPU6500_ALIGN      CW0_DEG
 
 #define MAG
 #define USE_MAG_AK8963

src/main/target/BLUEJAYF4/target.mk

@@ -2,8 +2,8 @@ F405_TARGETS    += $(TARGET)
 FEATURES        += SDCARD VCP ONBOARDFLASH
 
 TARGET_SRC = \
-            drivers/accgyro_spi_mpu6500.c \
             drivers/accgyro_mpu6500.c \
+            drivers/accgyro_spi_mpu9250.c \
             drivers/barometer_bmp085.c \
             drivers/barometer_bmp280.c \
             drivers/barometer_ms5611.c \

src/test/unit/sensor_gyro_unittest.cc

@@ -69,6 +69,7 @@ TEST(SensorGyro, Init)
 TEST(SensorGyro, Read)
 {
     gyroInit();
+    EXPECT_EQ(GYRO_FAKE, detectedSensors[SENSOR_INDEX_GYRO]);
     fakeGyroSet(5, 6, 7);
     const bool read = gyro.dev.read(&gyro.dev);
     EXPECT_EQ(true, read);