diff --git a/Makefile b/Makefile
index a5edba6e17..a718014863 100755
--- a/Makefile
+++ b/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 \
diff --git a/docs/Cli.md b/docs/Cli.md
index 5cdb98de48..3070c6d90a 100644
--- a/docs/Cli.md
+++ b/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 |
diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c
index af66683eba..f5b77c2a05 100644
--- a/src/main/drivers/accgyro_mpu.c
+++ b/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)
- return &mpuDetectionResult;
+ if (sig == MPUx0x0_WHO_AM_I_CONST) {
- mpuDetectionResult.sensor = MPU_60x0;
+ mpuDetectionResult.sensor = MPU_60x0;
- mpu6050FindRevision();
+ 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;
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index 06e91540da..c69e9f1059 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/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);
diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c
new file mode 100644
index 0000000000..a0d114a9e3
--- /dev/null
+++ b/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 .
+ */
+
+#include
+#include
+#include
+
+#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
+
+}
diff --git a/src/main/drivers/accgyro_mpu6500.h b/src/main/drivers/accgyro_mpu6500.h
new file mode 100644
index 0000000000..3a8a389c8e
--- /dev/null
+++ b/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 .
+ */
+
+#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);
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
index 95bbcfa570..6eb7864b56 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.c
+++ b/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
-}
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
index 11a3c3e3d7..7c1c6e88f1 100755
--- a/src/main/drivers/accgyro_spi_mpu6500.h
+++ b/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);
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index cd13a01a4d..5a66f581c9 100644
--- a/src/main/sensors/acceleration.h
+++ b/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_SPI_MPU6500 = 8,
+ ACC_MPU6000 = 7,
+ ACC_MPU6500 = 8,
ACC_FAKE = 9,
} accelerationSensor_e;
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index f09e2af5e2..d9bc6e210b 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -24,8 +24,8 @@ typedef enum {
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
- GYRO_SPI_MPU6000,
- GYRO_SPI_MPU6500,
+ GYRO_MPU6000,
+ GYRO_MPU6500,
GYRO_FAKE
} gyroSensor_e;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index c3f7f668f9..a45d94750f 100755
--- a/src/main/sensors/initialisation.c
+++ b/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
- gyroHardware = GYRO_SPI_MPU6000;
- gyroAlign = GYRO_SPI_MPU6000_ALIGN;
+#ifdef GYRO_MPU6000_ALIGN
+ gyroHardware = GYRO_MPU6000;
+ gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
- case GYRO_SPI_MPU6500:
-#ifdef USE_GYRO_SPI_MPU6500
+ case GYRO_MPU6500:
+#ifdef USE_GYRO_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
-#ifdef NAZE
- if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
-#ifdef GYRO_SPI_MPU6500_ALIGN
- gyroHardware = GYRO_SPI_MPU6500;
- gyroAlign = GYRO_SPI_MPU6500_ALIGN;
+ if (mpu6500GyroDetect(&gyro, gyroLpf) || mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+#ifdef GYRO_MPU6500_ALIGN
+ gyroHardware = GYRO_MPU6500;
+ gyroAlign = GYRO_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
- accAlign = ACC_SPI_MPU6000_ALIGN;
+#ifdef ACC_MPU6000_ALIGN
+ accAlign = ACC_MPU6000_ALIGN;
#endif
- accHardware = ACC_SPI_MPU6000;
+ accHardware = ACC_MPU6000;
break;
}
#endif
; // fallthrough
- case ACC_SPI_MPU6500:
-#ifdef USE_ACC_SPI_MPU6500
-#ifdef NAZE
- if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
-#else
- if (mpu6500SpiAccDetect(&acc)) {
+ case ACC_MPU6500:
+#ifdef USE_ACC_MPU6500
+ if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc)) {
+#ifdef ACC_MPU6500_ALIGN
+ accAlign = ACC_MPU6500_ALIGN;
#endif
-#ifdef ACC_SPI_MPU6500_ALIGN
- accAlign = ACC_SPI_MPU6500_ALIGN;
-#endif
- accHardware = ACC_SPI_MPU6500;
+ accHardware = ACC_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();
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index a79dab592a..7f0043d7e9 100644
--- a/src/main/target/CC3D/target.h
+++ b/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
diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h
index 3aae6f7483..cb35d10746 100755
--- a/src/main/target/COLIBRI_RACE/target.h
+++ b/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
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index 8d2c665754..66a5db3b31 100644
--- a/src/main/target/NAZE/target.h
+++ b/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