Add support for MPU6500 connected via I2C.

Makefile

@@ -295,6 +295,7 @@ NAZE_SRC	 = startup_stm32f10x_md_gcc.S \
 		   drivers/accgyro_mpu.c \
 		   drivers/accgyro_mpu3050.c \
 		   drivers/accgyro_mpu6050.c \
+		   drivers/accgyro_mpu6500.c \
 		   drivers/accgyro_spi_mpu6500.c \
 		   drivers/adc.c \
 		   drivers/adc_stm32f10x.c \

docs/Cli.md

@@ -204,7 +204,7 @@ Re-apply any new defaults as desired.
 | `rx_min_usec`                   | Defines the shortest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 2000   | 985           | Profile      | UINT16   |
 | `rx_max_usec`                   | Defines the longest pulse width value used when ensuring the channel value is valid.  If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`.                                                                                                                                                                                                                                                                                                                                                                                                                     | 100    | 3000   | 2115          | Profile      | UINT16   |
 | `gimbal_mode`                  | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal                                                                                                                                                                                                                                                                                                    | 0      | 1      | 0             | Profile      | UINT8    |
-| `acc_hardware`                  | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation.                                                                                                                                                                            | 0      | 9      | 0             | Master       | UINT8    |
+| `acc_hardware`                  | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for MPU6000, 8 for MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation.                                                                                                                                                                            | 0      | 9      | 0             | Master       | UINT8    |
 | `acc_lpf_factor`                | This setting controls the Low Pass Filter factor for ACC.  Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time.  Zero = no filter                                                                                                                                                                                                                                                                                                                                                                                                                                                                                | 0      | 250    | 4             | Profile      | UINT8    |
 | `accxy_deadband`                |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 40            | Profile      | UINT8    |
 | `accz_deadband`                 |                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        | 0      | 100    | 40            | Profile      | UINT8    |

src/main/drivers/accgyro_mpu.c

@@ -37,6 +37,7 @@
 #include "accgyro.h"
 #include "accgyro_mpu3050.h"
 #include "accgyro_mpu6050.h"
+#include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6000.h"
 #include "accgyro_spi_mpu6500.h"
 #include "accgyro_mpu.h"
@@ -118,12 +119,14 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
 
     sig &= MPU_INQUIRY_MASK;
 
-    if (sig != MPUx0x0_WHO_AM_I_CONST)
+    if (sig == MPUx0x0_WHO_AM_I_CONST) {
-        return &mpuDetectionResult;
 
         mpuDetectionResult.sensor = MPU_60x0;
 
         mpu6050FindRevision();
+    } else if (sig == MPU6500_WHO_AM_I_CONST) {
+        mpuDetectionResult.sensor = MPU_65xx_I2C;
+    }
 
     return &mpuDetectionResult;
 }
@@ -134,7 +137,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
     bool found = false;
 
 #ifdef USE_GYRO_SPI_MPU6500
-    found = mpu6500Detect();
+    found = mpu6500SpiDetect();
     if (found) {
         mpuDetectionResult.sensor = MPU_65xx_SPI;
         mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;

src/main/drivers/accgyro_mpu6050.c

@@ -204,7 +204,7 @@ static void mpu6050GyroInit(void)
     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 = 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.
+    // ACC Init stuff.
     // Accel scale 8g (4096 LSB/g)
     ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
 

src/main/drivers/accgyro_mpu6500.c

@@ -0,0 +1,110 @@
+/*
+ * 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/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include "platform.h"
+
+#include "common/axis.h"
+#include "common/maths.h"
+
+#include "system.h"
+#include "exti.h"
+#include "gpio.h"
+
+#include "sensor.h"
+#include "accgyro.h"
+#include "accgyro_mpu.h"
+#include "accgyro_mpu6500.h"
+
+extern mpuDetectionResult_t mpuDetectionResult;
+extern uint8_t mpuLowPassFilter;
+
+
+extern uint16_t acc_1G;
+
+
+bool mpu6500AccDetect(acc_t *acc)
+{
+    if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
+        return false;
+    }
+
+    acc->init = mpu6500AccInit;
+    acc->read = mpuAccRead;
+
+    return true;
+}
+
+bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf)
+{
+    if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
+        return false;
+    }
+
+    gyro->init = mpu6500GyroInit;
+    gyro->read = mpuGyroRead;
+
+    // 16.4 dps/lsb scalefactor
+    gyro->scale = 1.0f / 16.4f;
+
+    configureMPULPF(lpf);
+
+    return true;
+}
+
+void mpu6500AccInit(void)
+{
+    acc_1G = 512 * 8;
+}
+
+void mpu6500GyroInit(void)
+{
+#ifdef NAZE
+    // FIXME target specific code in driver code.
+
+    gpio_config_t gpio;
+    // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
+    if (hse_value == 12000000) {
+        gpio.pin = Pin_13;
+        gpio.speed = Speed_2MHz;
+        gpio.mode = Mode_IN_FLOATING;
+        gpioInit(GPIOC, &gpio);
+    }
+#endif
+
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, 0);
+    delay(100);
+    mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
+    mpuConfiguration.write(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
+    mpuConfiguration.write(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
+    mpuConfiguration.write(MPU6500_RA_LPF, mpuLowPassFilter);
+    mpuConfiguration.write(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
+
+    // Data ready interrupt configuration
+    mpuConfiguration.write(MPU6500_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
+#ifdef USE_MPU_DATA_READY_SIGNAL
+    mpuConfiguration.write(MPU6500_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
+#endif
+
+}

src/main/drivers/accgyro_mpu6500.h

@@ -0,0 +1,42 @@
+/*
+ * 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/>.
+ */
+
+#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_INT_PIN_CFG              (0x37)
+#define MPU6500_RA_INT_ENABLE               (0x38)
+#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_WHOAMI                   (0x75)
+
+#define MPU6500_WHO_AM_I_CONST              (0x70)
+
+#define MPU6500_BIT_RESET                   (0x80)
+
+#pragma once
+
+bool mpu6500AccDetect(acc_t *acc);
+bool mpu6500GyroDetect(gyro_t *gyro, uint16_t lpf);
+
+void mpu6500AccInit(void);
+void mpu6500GyroInit(void);

src/main/drivers/accgyro_spi_mpu6500.c

@@ -32,6 +32,7 @@
 #include "sensor.h"
 #include "accgyro.h"
 #include "accgyro_mpu.h"
+#include "accgyro_mpu6500.h"
 #include "accgyro_spi_mpu6500.h"
 
 #define DISABLE_MPU6500       GPIO_SetBits(MPU6500_CS_GPIO,   MPU6500_CS_PIN)
@@ -40,10 +41,6 @@
 extern mpuDetectionResult_t mpuDetectionResult;
 extern uint8_t mpuLowPassFilter;
 
-
-static void mpu6500AccInit(void);
-static void mpu6500GyroInit(void);
-
 extern uint16_t acc_1G;
 
 bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
@@ -105,7 +102,7 @@ static void mpu6500SpiInit(void)
     hardwareInitialised = true;
 }
 
-bool mpu6500Detect(void)
+bool mpu6500SpiDetect(void)
 {
     uint8_t tmp;
 
@@ -148,33 +145,3 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
     return true;
 }
 
-static void mpu6500AccInit(void)
-{
-    acc_1G = 512 * 8;
-}
-
-static void mpu6500GyroInit(void)
-{
-#ifdef NAZE
-    gpio_config_t gpio;
-    // MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
-    if (hse_value == 12000000) {
-        gpio.pin = Pin_13;
-        gpio.speed = Speed_2MHz;
-        gpio.mode = Mode_IN_FLOATING;
-        gpioInit(GPIOC, &gpio);
-    }
-#endif
-
-    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);
-    mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
-    mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
-    mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
-    mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
-}

src/main/drivers/accgyro_spi_mpu6500.h

@@ -20,6 +20,8 @@
 #define MPU6500_RA_GYRO_CFG                 (0x1B)
 #define MPU6500_RA_ACCEL_CFG                (0x1C)
 #define MPU6500_RA_ACCEL_XOUT_H             (0x3B)
+#define MPU6500_RA_INT_PIN_CFG              (0x37)
+#define MPU6500_RA_INT_ENABLE               (0x38)
 #define MPU6500_RA_GYRO_XOUT_H              (0x43)
 #define MPU6500_RA_SIGNAL_PATH_RST          (0x68)
 #define MPU6500_RA_PWR_MGMT_1               (0x6B)
@@ -33,7 +35,7 @@
 
 #pragma once
 
-bool mpu6500Detect(void);
+bool mpu6500SpiDetect(void);
 
 bool mpu6500SpiAccDetect(acc_t *acc);
 bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);

src/main/sensors/acceleration.h

@@ -26,8 +26,8 @@ typedef enum {
     ACC_MMA8452 = 4,
     ACC_BMA280 = 5,
     ACC_LSM303DLHC = 6,
-    ACC_SPI_MPU6000 = 7,
+    ACC_MPU6000 = 7,
-    ACC_SPI_MPU6500 = 8,
+    ACC_MPU6500 = 8,
     ACC_FAKE = 9,
 } accelerationSensor_e;
 

src/main/sensors/gyro.h

@@ -24,8 +24,8 @@ typedef enum {
     GYRO_L3G4200D,
     GYRO_MPU3050,
     GYRO_L3GD20,
-    GYRO_SPI_MPU6000,
+    GYRO_MPU6000,
-    GYRO_SPI_MPU6500,
+    GYRO_MPU6500,
     GYRO_FAKE
 } gyroSensor_e;
 

src/main/sensors/initialisation.c

@@ -38,6 +38,7 @@
 #include "drivers/accgyro_mpu.h"
 #include "drivers/accgyro_mpu3050.h"
 #include "drivers/accgyro_mpu6050.h"
+#include "drivers/accgyro_mpu6500.h"
 #include "drivers/accgyro_l3gd20.h"
 #include "drivers/accgyro_lsm303dlhc.h"
 
@@ -218,40 +219,30 @@ bool detectGyro(uint16_t gyroLpf)
 #endif
             ; // fallthrough
 
-        case GYRO_SPI_MPU6000:
+        case GYRO_MPU6000:
 #ifdef USE_GYRO_SPI_MPU6000
             if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6000_ALIGN
+#ifdef GYRO_MPU6000_ALIGN
-                gyroHardware = GYRO_SPI_MPU6000;
+                gyroHardware = GYRO_MPU6000;
-                gyroAlign = GYRO_SPI_MPU6000_ALIGN;
+                gyroAlign = GYRO_MPU6000_ALIGN;
 #endif
                 break;
             }
 #endif
             ; // fallthrough
 
-        case GYRO_SPI_MPU6500:
+        case GYRO_MPU6500:
-#ifdef USE_GYRO_SPI_MPU6500
+#ifdef USE_GYRO_MPU6500
 #ifdef USE_HARDWARE_REVISION_DETECTION
 		  spiBusInit();
 #endif
-#ifdef NAZE
+            if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
-            if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+#ifdef GYRO_MPU6500_ALIGN
-#ifdef GYRO_SPI_MPU6500_ALIGN
+                gyroHardware = GYRO_MPU6500;
-                gyroHardware = GYRO_SPI_MPU6500;
+                gyroAlign = GYRO_MPU6500_ALIGN;
-                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
 #endif
                 break;
             }
-#else
-            if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6500_ALIGN
-                gyroHardware = GYRO_SPI_MPU6500;
-                gyroAlign = GYRO_SPI_MPU6500_ALIGN;
-#endif
-                break;
-            }
-#endif
 #endif
             ; // fallthrough
 
@@ -357,28 +348,24 @@ retry:
             }
 #endif
             ; // fallthrough
-        case ACC_SPI_MPU6000:
+        case ACC_MPU6000:
 #ifdef USE_ACC_SPI_MPU6000
             if (mpu6000SpiAccDetect(&acc)) {
-#ifdef ACC_SPI_MPU6000_ALIGN
+#ifdef ACC_MPU6000_ALIGN
-                accAlign = ACC_SPI_MPU6000_ALIGN;
+                accAlign = ACC_MPU6000_ALIGN;
 #endif
-                accHardware = ACC_SPI_MPU6000;
+                accHardware = ACC_MPU6000;
                 break;
             }
 #endif
             ; // fallthrough
-        case ACC_SPI_MPU6500:
+        case ACC_MPU6500:
-#ifdef USE_ACC_SPI_MPU6500
+#ifdef USE_ACC_MPU6500
-#ifdef NAZE
+            if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
-            if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
+#ifdef ACC_MPU6500_ALIGN
-#else
+                accAlign = ACC_MPU6500_ALIGN;
-            if (mpu6500SpiAccDetect(&acc)) {
 #endif
-#ifdef ACC_SPI_MPU6500_ALIGN
+                accHardware = ACC_MPU6500;
-                accAlign = ACC_SPI_MPU6500_ALIGN;
-#endif
-                accHardware = ACC_SPI_MPU6500;
                 break;
             }
 #endif
@@ -599,7 +586,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_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
+#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();
 

src/main/target/CC3D/target.h

@@ -47,12 +47,12 @@
 #define GYRO
 #define USE_GYRO_SPI_MPU6000
 
-#define GYRO_SPI_MPU6000_ALIGN CW270_DEG
+#define GYRO_MPU6000_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_SPI_MPU6000
 
-#define ACC_SPI_MPU6000_ALIGN CW270_DEG
+#define ACC_MPU6000_ALIGN CW270_DEG
 
 // External I2C BARO
 #define BARO

src/main/target/COLIBRI_RACE/target.h

@@ -57,11 +57,11 @@
 
 #define GYRO
 #define USE_GYRO_SPI_MPU6500
-#define GYRO_SPI_MPU6500_ALIGN CW270_DEG
+#define GYRO_MPU6500_ALIGN CW270_DEG
 
 #define ACC
 #define USE_ACC_SPI_MPU6500
-#define ACC_SPI_MPU6500_ALIGN CW270_DEG
+#define ACC_MPU6500_ALIGN CW270_DEG
 
 #define BARO
 #define USE_BARO_MS5611

src/main/target/NAZE/target.h

@@ -84,25 +84,27 @@
 #define GYRO
 #define USE_GYRO_MPU3050
 #define USE_GYRO_MPU6050
+#define USE_GYRO_MPU6500
 #define USE_GYRO_SPI_MPU6500
 
 
 #define GYRO_MPU3050_ALIGN CW0_DEG
 #define GYRO_MPU6050_ALIGN CW0_DEG
-#define GYRO_SPI_MPU6500_ALIGN CW0_DEG
+#define GYRO_MPU6500_ALIGN CW0_DEG
 
 #define ACC
 #define USE_ACC_ADXL345
 #define USE_ACC_BMA280
 #define USE_ACC_MMA8452
 #define USE_ACC_MPU6050
+#define USE_ACC_MPU6500
 #define USE_ACC_SPI_MPU6500
 
 #define ACC_ADXL345_ALIGN CW270_DEG
 #define ACC_MPU6050_ALIGN CW0_DEG
 #define ACC_MMA8452_ALIGN CW90_DEG
 #define ACC_BMA280_ALIGN CW0_DEG
-#define ACC_SPI_MPU6500_ALIGN CW0_DEG
+#define ACC_MPU6500_ALIGN CW0_DEG
 
 #define BARO
 #define USE_BARO_MS5611