Relocate and use some of the common MPU code from MPU6500 into

accgyro_mpu.c.

Makefile

@@ -520,6 +520,7 @@ CHEBUZZF3_SRC	 = \
 COLIBRI_RACE_SRC        = \
 		   $(STM32F30x_COMMON_SRC) \
 		   drivers/display_ug2864hsweg01.c \
+		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_spi_mpu6500.c \
 		   drivers/barometer_ms5611.c \
 		   drivers/compass_ak8975.c \

src/main/drivers/accgyro_mpu.c

@@ -37,25 +37,29 @@
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
+#include "accgyro_spi_mpu6500.h"
 #include "accgyro_mpu.h"
 
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 
+static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
+static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
 
-uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+static void mpu6050FindRevision(void);
 
-void mpu6050FindRevision(void);
+#ifdef USE_SPI
+static bool detectSPISensorsAndUpdateDetectionResult(void);
+#endif
 
 mpuDetectionResult_t mpuDetectionResult;
 
-typedef struct mpuConfiguration_s {
+mpuConfiguration_t mpuConfiguration;
-    uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
-} mpuConfiguration_t;
-
-static mpuConfiguration_t mpuConfiguration;
 static const extiConfig_t *mpuIntExtiConfig = NULL;
 
+// FIXME move into mpuConfiguration
+uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+
 
 #define MPU_ADDRESS             0x68
 
@@ -87,14 +91,23 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
     // MPU datasheet specifies 30ms.
     delay(35);
 
-    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
+    ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
-    if (!ack)
+    if (ack) {
+        mpuConfiguration.read = mpuReadRegisterI2C;
+        mpuConfiguration.write = mpuWriteRegisterI2C;
+    } else {
+#ifdef USE_SPI
+        bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult();
+        UNUSED(detectedSpiSensor);
+#endif
+
         return &mpuDetectionResult;
+    }
 
     mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
 
     // If an MPU3050 is connected sig will contain 0.
-    ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
+    ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
     inquiryResult &= MPU_INQUIRY_MASK;
     if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
         mpuDetectionResult.sensor = MPU_3050;
@@ -114,8 +127,26 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
     return &mpuDetectionResult;
 }
 
+#ifdef USE_SPI
+static bool detectSPISensorsAndUpdateDetectionResult(void)
+{
+    bool found = false;
 
-void mpu6050FindRevision(void)
+#ifdef USE_GYRO_SPI_MPU6500
+    found = mpu6500Detect();
+    if (found) {
+        mpuDetectionResult.sensor = MPU_65xx_SPI;
+        mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
+        mpuConfiguration.read = mpu6500ReadRegister;
+        mpuConfiguration.write = mpu6500WriteRegister;
+    }
+#endif
+
+    return found;
+}
+#endif
+
+static void mpu6050FindRevision(void)
 {
     bool ack;
     UNUSED(ack);
@@ -276,34 +307,46 @@ void configureMPULPF(uint16_t lpf)
         mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
 }
 
+static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
+{
+    bool ack = i2cRead(MPU_ADDRESS, reg, length, data);
+    return ack;
+}
+
+static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
+{
+    bool ack = i2cWrite(MPU_ADDRESS, reg, data);
+    return ack;
+}
+
 bool mpuAccRead(int16_t *accData)
 {
-    uint8_t buf[6];
+    uint8_t data[6];
 
-    bool ack = i2cRead(MPU_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
+    bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
     if (!ack) {
         return false;
     }
 
-    accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    accData[0] = (int16_t)((data[0] << 8) | data[1]);
-    accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    accData[1] = (int16_t)((data[2] << 8) | data[3]);
-    accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    accData[2] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }
 
 bool mpuGyroRead(int16_t *gyroADC)
 {
-    uint8_t buf[6];
+    uint8_t data[6];
 
-    bool ack = i2cRead(MPU_ADDRESS, mpuConfiguration.gyroReadXRegister, 6, buf);
+    bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
     if (!ack) {
         return false;
     }
 
-    gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
+    gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
-    gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
+    gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
-    gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
+    gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
 
     return true;
 }

src/main/drivers/accgyro_mpu.h

@@ -17,6 +17,16 @@
 
 #pragma once
 
+typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
+typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
+
+typedef struct mpuConfiguration_s {
+    uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
+    mpuReadRegisterFunc read;
+    mpuWriteRegisterFunc write;
+} mpuConfiguration_t;
+
+extern mpuConfiguration_t mpuConfiguration;
 
 enum lpf_e {
     INV_FILTER_256HZ_NOLPF2 = 0,

src/main/drivers/accgyro_mpu3050.c

@@ -75,20 +75,20 @@ void mpu3050Init(void)
 
     delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
 
-    ack = i2cWrite(MPU3050_ADDRESS, MPU3050_SMPLRT_DIV, 0);
+    ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
     if (!ack)
         failureMode(FAILURE_ACC_INIT);
 
-    i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
+    mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
+    mpuConfiguration.write(MPU3050_INT_CFG, 0);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
+    mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
-    i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
+    mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
 }
 
 static bool mpu3050ReadTemp(int16_t *tempData)
 {
     uint8_t buf[2];
-    if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
+    if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
         return false;
     }
 

src/main/drivers/accgyro_mpu6050.c

@@ -196,23 +196,23 @@ static void mpu6050GyroInit(void)
     mpuIntExtiInit();
 
     bool ack;
-    ack = i2cWrite(MPU6050_ADDRESS, 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 = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+    ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV    -- SMPLRT_DIV = 0  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
-    ack = i2cWrite(MPU6050_ADDRESS, 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_PWR_MGMT_1, 0x03); //PWR_MGMT_1    -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference)
     delay(15); //PLL Settling time when changing CLKSEL is max 10ms.  Use 15ms to be sure 
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //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, mpuLowPassFilter); //CONFIG        -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz  GYRO bandwidth = 256Hz)
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);   //GYRO_CONFIG   -- FS_SEL = 3: Full scale set to 2000 deg/sec
+    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. Moved into gyro init because the reset above would screw up accel config. Oops.
     // Accel scale 8g (4096 LSB/g)
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
+    ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_PIN_CFG,
+    ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
             0 << 7 | 0 << 6 | 0 << 5 | 0 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_PIN_CFG   -- INT_LEVEL_HIGH, INT_OPEN_DIS, LATCH_INT_DIS, INT_RD_CLEAR_DIS, FSYNC_INT_LEVEL_HIGH, FSYNC_INT_DIS, I2C_BYPASS_EN, CLOCK_DIS
 
 #ifdef USE_MPU_DATA_READY_SIGNAL
-    ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
+    ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
 #endif
     UNUSED(ack);
 }

src/main/drivers/accgyro_spi_mpu6500.c

@@ -25,73 +25,45 @@
 #include "common/maths.h"
 
 #include "system.h"
+#include "exti.h"
 #include "gpio.h"
 #include "bus_spi.h"
 
 #include "sensor.h"
 #include "accgyro.h"
+#include "accgyro_mpu.h"
 #include "accgyro_spi_mpu6500.h"
 
-enum lpf_e {
-    INV_FILTER_256HZ_NOLPF2 = 0,
-    INV_FILTER_188HZ,
-    INV_FILTER_98HZ,
-    INV_FILTER_42HZ,
-    INV_FILTER_20HZ,
-    INV_FILTER_10HZ,
-    INV_FILTER_5HZ,
-    INV_FILTER_2100HZ_NOLPF,
-    NUM_FILTER
-};
-
-enum gyro_fsr_e {
-    INV_FSR_250DPS = 0,
-    INV_FSR_500DPS,
-    INV_FSR_1000DPS,
-    INV_FSR_2000DPS,
-    NUM_GYRO_FSR
-};
-
-enum clock_sel_e {
-    INV_CLK_INTERNAL = 0,
-    INV_CLK_PLL,
-    NUM_CLK
-};
-
-enum accel_fsr_e {
-    INV_FSR_2G = 0,
-    INV_FSR_4G,
-    INV_FSR_8G,
-    INV_FSR_16G,
-    NUM_ACCEL_FSR
-};
-
 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
 #define ENABLE_MPU6500        GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
 
-static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
 
 static void mpu6500AccInit(void);
-static bool mpu6500AccRead(int16_t *accData);
 static void mpu6500GyroInit(void);
-static bool mpu6500GyroRead(int16_t *gyroADC);
 
 extern uint16_t acc_1G;
 
-static void mpu6500WriteRegister(uint8_t reg, uint8_t data)
+bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
 {
     ENABLE_MPU6500;
     spiTransferByte(MPU6500_SPI_INSTANCE, reg);
     spiTransferByte(MPU6500_SPI_INSTANCE, data);
     DISABLE_MPU6500;
+
+    return true;
 }
 
-static void mpu6500ReadRegister(uint8_t reg, uint8_t *data, int length)
+bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
 {
     ENABLE_MPU6500;
     spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
     spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
     DISABLE_MPU6500;
+
+    return true;
 }
 
 static void mpu6500SpiInit(void)
@@ -133,13 +105,13 @@ static void mpu6500SpiInit(void)
     hardwareInitialised = true;
 }
 
-static bool mpu6500Detect(void)
+bool mpu6500Detect(void)
 {
     uint8_t tmp;
 
     mpu6500SpiInit();
 
-    mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
+    mpu6500ReadRegister(MPU6500_RA_WHOAMI, 1, &tmp);
 
     if (tmp != MPU6500_WHO_AM_I_CONST)
         return false;
@@ -149,42 +121,29 @@ static bool mpu6500Detect(void)
 
 bool mpu6500SpiAccDetect(acc_t *acc)
 {
-    if (!mpu6500Detect()) {
+    if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
     }
 
     acc->init = mpu6500AccInit;
-    acc->read = mpu6500AccRead;
+    acc->read = mpuAccRead;
 
     return true;
 }
 
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
 {
-    if (!mpu6500Detect()) {
+    if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
         return false;
     }
 
     gyro->init = mpu6500GyroInit;
-    gyro->read = mpu6500GyroRead;
+    gyro->read = mpuGyroRead;
 
     // 16.4 dps/lsb scalefactor
     gyro->scale = 1.0f / 16.4f;
-    //gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
 
-    // default lpf is 42Hz
+    configureMPULPF(lpf);
-    if (lpf >= 188)
-        mpuLowPassFilter = INV_FILTER_188HZ;
-    else if (lpf >= 98)
-        mpuLowPassFilter = INV_FILTER_98HZ;
-    else if (lpf >= 42)
-        mpuLowPassFilter = INV_FILTER_42HZ;
-    else if (lpf >= 20)
-        mpuLowPassFilter = INV_FILTER_20HZ;
-    else if (lpf >= 10)
-        mpuLowPassFilter = INV_FILTER_10HZ;
-    else
-        mpuLowPassFilter = INV_FILTER_5HZ;
 
     return true;
 }
@@ -194,19 +153,6 @@ static void mpu6500AccInit(void)
     acc_1G = 512 * 8;
 }
 
-static bool mpu6500AccRead(int16_t *accData)
-{
-    uint8_t buf[6];
-
-    mpu6500ReadRegister(MPU6500_RA_ACCEL_XOUT_H, buf, 6);
-
-    accData[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    accData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    accData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}
-
 static void mpu6500GyroInit(void)
 {
 #ifdef NAZE
@@ -222,6 +168,8 @@ static void mpu6500GyroInit(void)
 
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
     delay(100);
+    mpu6500WriteRegister(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
+    delay(100);
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
     delay(100);
     mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
@@ -230,16 +178,3 @@ static void mpu6500GyroInit(void)
     mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
     mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
 }
-
-static bool mpu6500GyroRead(int16_t *gyroADC)
-{
-    uint8_t buf[6];
-
-    mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);
-
-    gyroADC[X] = (int16_t)((buf[0] << 8) | buf[1]);
-    gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]);
-    gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]);
-
-    return true;
-}

src/main/drivers/accgyro_spi_mpu6500.h

@@ -15,16 +15,17 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define MPU6500_RA_WHOAMI                   (0x75)
+#define MPU6500_RA_RATE_DIV                 (0x19)
+#define MPU6500_RA_LPF                      (0x1A)
+#define MPU6500_RA_GYRO_CFG                 (0x1B)
+#define MPU6500_RA_ACCEL_CFG                (0x1C)
 #define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
 #define MPU6500_RA_GYRO_XOUT_H              (0x43)
+#define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
+#define MPU6500_RA_PWR_MGMT_1               (0x6B)
 #define MPU6500_RA_BANK_SEL                 (0x6D)
 #define MPU6500_RA_MEM_RW                   (0x6F)
-#define MPU6500_RA_GYRO_CFG                 (0x1B)
+#define MPU6500_RA_WHOAMI                   (0x75)
-#define MPU6500_RA_PWR_MGMT_1               (0x6B)
-#define MPU6500_RA_ACCEL_CFG                (0x1C)
-#define MPU6500_RA_LPF                      (0x1A)
-#define MPU6500_RA_RATE_DIV                 (0x19)
 
 #define MPU6500_WHO_AM_I_CONST              (0x70)
 
@@ -32,5 +33,10 @@
 
 #pragma once
 
+bool mpu6500Detect(void);
+
 bool mpu6500SpiAccDetect(acc_t *acc);
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
+
+bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
+bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

src/main/sensors/initialisation.c

@@ -599,7 +599,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
     memset(&acc, 0, sizeof(acc));
     memset(&gyro, 0, sizeof(gyro));
 
-#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_ACC_MPU6050)
+#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_ACC_MPU6050)
 
     const extiConfig_t *extiConfig = selectMPUIntExtiConfig();