diff --git a/Makefile b/Makefile
index 5939ae528..2c5570fdf 100644
--- a/Makefile
+++ b/Makefile
@@ -220,6 +220,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
drivers/accgyro_mma845x.c \
drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \
+ drivers/accgyro_spi_mpu6500.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
drivers/barometer_bmp085.c \
@@ -247,11 +248,18 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
$(COMMON_SRC)
EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
- drivers/accgyro_mpu6050.c \
+ drivers/accgyro_adxl345.c \
+ drivers/accgyro_bma280.c \
drivers/accgyro_l3g4200d.c \
+ drivers/accgyro_mma845x.c \
+ drivers/accgyro_mpu3050.c \
+ drivers/accgyro_mpu6050.c \
+ drivers/accgyro_spi_mpu6000.c \
+ drivers/accgyro_spi_mpu6500.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
drivers/barometer_bmp085.c \
+ drivers/barometer_ms5611.c \
drivers/bus_i2c_stm32f10x.c \
drivers/bus_spi.c \
drivers/compass_hmc5883l.c \
diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c
index c9db1e13e..467a0d58b 100644
--- a/src/main/drivers/accgyro_mpu6050.c
+++ b/src/main/drivers/accgyro_mpu6050.c
@@ -127,16 +127,37 @@
#define MPU6050_SMPLRT_DIV 0 // 8000Hz
-#define MPU6050_LPF_256HZ 0
-#define MPU6050_LPF_188HZ 1
-#define MPU6050_LPF_98HZ 2
-#define MPU6050_LPF_42HZ 3
-#define MPU6050_LPF_20HZ 4
-#define MPU6050_LPF_10HZ 5
-#define MPU6050_LPF_5HZ 6
-
-static uint8_t mpuLowPassFilter = MPU6050_LPF_42HZ;
-
+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
+};
+static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
static void mpu6050AccInit(void);
static void mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void);
@@ -161,8 +182,8 @@ static bool mpu6050Detect(void)
return false;
// So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device.
- // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0’s 7-bit I2C address.
- // The least significant bit of the MPU-60X0’s I2C address is determined by the value of the AD0 pin. (we know that already).
+ // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address.
+ // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already).
// But here's the best part: The value of the AD0 pin is not reflected in this register.
if (sig != (MPU6050_ADDRESS & 0x7e))
@@ -227,31 +248,18 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
- // default lpf is 42Hz
- switch (lpf) {
- case 256:
- mpuLowPassFilter = MPU6050_LPF_256HZ;
- break;
- case 188:
- mpuLowPassFilter = MPU6050_LPF_188HZ;
- break;
- case 98:
- mpuLowPassFilter = MPU6050_LPF_98HZ;
- break;
- default:
- case 42:
- mpuLowPassFilter = MPU6050_LPF_42HZ;
- break;
- case 20:
- mpuLowPassFilter = MPU6050_LPF_20HZ;
- break;
- case 10:
- mpuLowPassFilter = MPU6050_LPF_10HZ;
- break;
- case 5:
- mpuLowPassFilter = MPU6050_LPF_5HZ;
- break;
- }
+ 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;
}
@@ -292,17 +300,17 @@ static void mpu6050GyroInit(void)
gpioInit(GPIOC, &gpio);
i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
- delay(5);
+ delay(100);
i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
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)
i2cWrite(MPU6050_ADDRESS, 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
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)
- i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, 0x18); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
+ i2cWrite(MPU6050_ADDRESS, 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)
- i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, 2 << 3);
+ i2cWrite(MPU6050_ADDRESS, MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
}
static void mpu6050GyroRead(int16_t *gyroData)
diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c
index aa25acabc..88dfdfde6 100644
--- a/src/main/drivers/accgyro_spi_mpu6000.c
+++ b/src/main/drivers/accgyro_spi_mpu6000.c
@@ -119,14 +119,6 @@
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
-#ifdef CC3D
-
-#define MPU6000_CS_GPIO GPIOA
-#define MPU6000_CS_PIN GPIO_Pin_4
-
-#define MPU6000_SPI_INSTANCE SPI1
-#endif
-
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c
new file mode 100644
index 000000000..9d7a7ebd8
--- /dev/null
+++ b/src/main/drivers/accgyro_spi_mpu6500.c
@@ -0,0 +1,215 @@
+/*
+ * 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 "gpio.h"
+#include "bus_spi.h"
+
+#include "accgyro.h"
+#include "accgyro_spi_mpu6500.h"
+
+// MPU6500 (WHO_AM_I 0x70) on SPI bus
+
+#define MPU6500_RA_WHOAMI (0x75)
+#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
+#define MPU6500_RA_GYRO_XOUT_H (0x43)
+#define MPU6500_RA_BANK_SEL (0x6D)
+#define MPU6500_RA_MEM_RW (0x6F)
+#define MPU6500_RA_GYRO_CFG (0x1B)
+#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)
+#define BIT_RESET (0x80)
+
+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;
+
+static void mpu6500AccInit(void);
+static void mpu6500AccRead(int16_t *accData);
+static void mpu6500GyroInit(void);
+static void mpu6500GyroRead(int16_t *gyroData);
+
+extern uint16_t acc_1G;
+
+static void mpu6500WriteRegister(uint8_t reg, uint8_t data)
+{
+ ENABLE_MPU6500;
+ spiTransferByte(MPU6500_SPI_INSTANCE, reg);
+ spiTransferByte(MPU6500_SPI_INSTANCE, data);
+ DISABLE_MPU6500;
+}
+
+static void mpu6500ReadRegister(uint8_t reg, uint8_t *data, int length)
+{
+ ENABLE_MPU6500;
+ spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
+ spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
+ DISABLE_MPU6500;
+}
+
+static bool mpu6500Detect(void)
+{
+ uint8_t tmp;
+
+ mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
+ if (tmp != MPU6500_WHO_AM_I_CONST)
+ return false;
+
+ return true;
+}
+
+bool mpu6500SpiAccDetect(acc_t *acc)
+{
+ if (!mpu6500Detect()) {
+ return false;
+ }
+
+ acc->init = mpu6500AccInit;
+ acc->read = mpu6500AccRead;
+
+ return true;
+}
+
+bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
+{
+ if (!mpu6500Detect()) {
+ return false;
+ }
+
+ gyro->init = mpu6500GyroInit;
+ gyro->read = mpu6500GyroRead;
+
+ // 16.4 dps/lsb scalefactor
+ gyro->scale = 1.0f / 16.4f;
+ //gyro->scale = (4.0f / 16.4f) * (M_PI / 180.0f) * 0.000001f;
+
+ // default lpf is 42Hz
+ 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;
+}
+
+static void mpu6500AccInit(void)
+{
+ acc_1G = 512 * 8;
+}
+
+static void 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]);
+}
+
+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, BIT_RESET);
+ 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
+}
+
+static void mpu6500GyroRead(int16_t *gyroData)
+{
+ uint8_t buf[6];
+
+ mpu6500ReadRegister(MPU6500_RA_GYRO_XOUT_H, buf, 6);
+
+ gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
+ gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
+ gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
+}
diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h
new file mode 100644
index 000000000..d22ad0c9c
--- /dev/null
+++ b/src/main/drivers/accgyro_spi_mpu6500.h
@@ -0,0 +1,22 @@
+/*
+ * 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 .
+ */
+
+
+#pragma once
+
+bool mpu6500SpiAccDetect(acc_t *acc);
+bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
diff --git a/src/main/drivers/bus_i2c_stm32f10x.c b/src/main/drivers/bus_i2c_stm32f10x.c
index e0540d305..55949cafb 100755
--- a/src/main/drivers/bus_i2c_stm32f10x.c
+++ b/src/main/drivers/bus_i2c_stm32f10x.c
@@ -17,6 +17,7 @@
#include
#include
+#include
#include
@@ -56,7 +57,7 @@ static const i2cDevice_t i2cHardwareMap[] = {
};
// Copy of peripheral address for IRQ routines
-static I2C_TypeDef *I2Cx;
+static I2C_TypeDef *I2Cx = NULL;
// Copy of device index for reinit, etc purposes
static I2CDevice I2Cx_index;
@@ -116,6 +117,9 @@ bool i2cWriteBuffer(uint8_t addr_, uint8_t reg_, uint8_t len_, uint8_t *data)
busy = 1;
error = false;
+ if (!I2Cx)
+ return false;
+
if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending
@@ -155,6 +159,9 @@ bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf)
busy = 1;
error = false;
+ if (!I2Cx)
+ return false;
+
if (!(I2Cx->CR2 & I2C_IT_EVT)) { // if we are restarting the driver
if (!(I2Cx->CR1 & 0x0100)) { // ensure sending a start
while (I2Cx->CR1 & 0x0200 && --timeout > 0) { ; } // wait for any stop to finish sending
diff --git a/src/main/drivers/system.c b/src/main/drivers/system.c
index 7c032314b..3d27df2d1 100755
--- a/src/main/drivers/system.c
+++ b/src/main/drivers/system.c
@@ -131,7 +131,7 @@ void systemInit(bool overclock)
// SysTick
SysTick_Config(SystemCoreClock / 1000);
-#ifdef CC3D
+#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#endif
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index 9edc1a35e..75088c014 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -129,7 +129,7 @@ static const char * const sensorNames[] = {
};
static const char * const accNames[] = {
- "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "FAKE", "None", NULL
+ "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", "None", NULL
};
typedef struct {
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 5c0e3f975..3342511df 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -88,7 +88,7 @@ extern int16_t debug[4]; // FIXME dependency on mw.c
#define MSP_MODE_RANGES 34 //out message Returns all mode ranges
#define MSP_SET_MODE_RANGE 35 //in message Sets a single mode range
-// Multiwii MSP commands
+//
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU 102 //out message 9 DOF
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index 40e3d0565..2b7e8ce94 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -26,8 +26,9 @@ typedef enum AccelSensors {
ACC_BMA280 = 4,
ACC_LSM303DLHC = 5,
ACC_SPI_MPU6000 = 6,
- ACC_FAKE = 7,
- ACC_NONE = 8
+ ACC_SPI_MPU6500 = 7,
+ ACC_FAKE = 8,
+ ACC_NONE = 9
} AccelSensors;
extern uint8_t accHardware;
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index fb771fe47..4e3f54611 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -32,13 +32,12 @@
#include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h"
-#ifdef STM32F3DISCOVERY
+
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
-#endif
-#ifdef CC3D
+
#include "drivers/accgyro_spi_mpu6000.h"
-#endif
+#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
@@ -57,133 +56,6 @@
#include "sensors/compass.h"
#include "sensors/sonar.h"
-
-// Use these to help with porting to new boards
-//#define USE_FAKE_GYRO
-#ifdef USE_I2C
-#define USE_GYRO_L3G4200D
-#define USE_GYRO_L3GD20
-#define USE_GYRO_MPU6050
-#define USE_GYRO_MPU3050
-#endif
-#define USE_GYRO_SPI_MPU6000
-
-//#define USE_FAKE_ACC
-#ifdef USE_I2C
-#define USE_ACC_ADXL345
-#define USE_ACC_BMA280
-#define USE_ACC_MMA8452
-#define USE_ACC_LSM303DLHC
-#define USE_ACC_MPU6050
-#endif
-#define USE_ACC_SPI_MPU6000
-
-#ifdef USE_I2C
-#define USE_BARO_MS5611
-#define USE_BARO_BMP085
-#endif
-
-#ifdef MASSIVEF3
-#define USE_FAKE_GYRO
-#define USE_FAKE_ACC
-#undef USE_GYRO_MPU6050
-#undef USE_ACC_MPU6050
-#undef USE_ACC_ADXL345
-#undef USE_ACC_BMA280
-#undef USE_ACC_MMA8452
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_SPI_MPU6000
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_MPU3050
-#undef USE_GYRO_SPI_MPU6000
-#undef USE_GYRO_L3GD20
-#undef USE_BARO_BMP085
-#endif
-
-#ifdef NAZE
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_SPI_MPU6000
-#undef USE_GYRO_SPI_MPU6000
-#undef USE_GYRO_L3GD20
-#endif
-
-#ifdef NAZE32PRO
-#define USE_FAKE_ACC
-#define USE_FAKE_GYRO
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_ADXL345
-#undef USE_ACC_BMA280
-#undef USE_ACC_MMA8452
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_MPU6050
-#undef USE_ACC_SPI_MPU6000
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_L3GD20
-#undef USE_GYRO_MPU3050
-#undef USE_GYRO_MPU6050
-#undef USE_GYRO_SPI_MPU6000
-#endif
-
-#if defined(OLIMEXINO) || defined(EUSTM32F103RC)
-#undef USE_GYRO_L3GD20
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_MPU3050
-#undef USE_GYRO_SPI_MPU6000
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_BMA280
-#undef USE_ACC_MMA8452
-#undef USE_ACC_ADXL345
-#undef USE_ACC_SPI_MPU6000
-#undef USE_BARO_MS5611
-#endif
-
-#ifdef EUSTM32F103RC
-#define USE_FAKE_GYRO
-#define USE_FAKE_ACC
-#define USE_GYRO_L3G4200D
-#endif
-
-#ifdef STM32F3DISCOVERY
-#undef USE_ACC_SPI_MPU6000
-#undef USE_GYRO_SPI_MPU6000
-#endif
-
-#ifdef CHEBUZZF3
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_MPU6050
-#undef USE_GYRO_MPU3050
-#undef USE_GYRO_SPI_MPU6000
-#undef USE_ACC_ADXL345
-#undef USE_ACC_BMA280
-#undef USE_ACC_MPU6050
-#undef USE_ACC_MMA8452
-#undef USE_ACC_SPI_MPU6000
-#endif
-
-#ifdef CC3D
-#undef USE_GYRO_L3GD20
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_MPU6050
-#undef USE_GYRO_MPU3050
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_ADXL345
-#undef USE_ACC_BMA280
-#undef USE_ACC_MPU6050
-#undef USE_ACC_MMA8452
-#endif
-
-#ifdef CJMCU
-#undef USE_GYRO_SPI_MPU6000
-#undef USE_GYRO_L3GD20
-#undef USE_GYRO_L3G4200D
-#undef USE_GYRO_MPU3050
-#undef USE_ACC_LSM303DLHC
-#undef USE_ACC_SPI_MPU6000
-#undef USE_ACC_ADXL345
-#undef USE_ACC_BMA280
-#undef USE_ACC_MMA8452
-#endif
-
extern float magneticDeclination;
extern gyro_t gyro;
@@ -270,6 +142,21 @@ bool detectGyro(uint16_t gyroLpf)
}
#endif
+#ifdef USE_GYRO_SPI_MPU6500
+#ifdef NAZE
+ // TODO only enable if NAZE32_SP is detected
+ if (false && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+ gyroAlign = CW0_DEG;
+ return true;
+ }
+#else
+ if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
+ gyroAlign = CW0_DEG;
+ return true;
+ }
+#endif
+#endif
+
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
return true;
@@ -359,7 +246,7 @@ retry:
}
; // fallthrough
#endif
-#ifdef USE_GYRO_SPI_MPU6000
+#ifdef USE_ACC_SPI_MPU6000
case ACC_SPI_MPU6000:
if (mpu6000SpiAccDetect(&acc)) {
accHardware = ACC_SPI_MPU6000;
@@ -370,6 +257,23 @@ retry:
break;
}
; // fallthrough
+#endif
+#ifdef USE_ACC_SPI_MPU6500
+ case ACC_SPI_MPU6500:
+#ifdef NAZE
+ // TODO only enable if NAZE32_SP is detected
+ if (false && mpu6500SpiAccDetect(&acc)) {
+#else
+ if (mpu6500SpiAccDetect(&acc)) {
+#endif
+ accHardware = ACC_SPI_MPU6500;
+#ifdef NAZE
+ accAlign = CW0_DEG;
+#endif
+ if (accHardwareToUse == ACC_SPI_MPU6500)
+ break;
+ }
+ ; // fallthrough
#endif
; // prevent compiler error
}
diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h
index 663b57670..51aaf4b30 100644
--- a/src/main/target/CC3D/target.h
+++ b/src/main/target/CC3D/target.h
@@ -29,8 +29,16 @@
#define BEEP_PIN Pin_15 // PA15 (Beeper)
#define BEEP_PERIPHERAL RCC_APB2Periph_GPIOA
+#define MPU6000_CS_GPIO GPIOA
+#define MPU6000_CS_PIN GPIO_Pin_4
+#define MPU6000_SPI_INSTANCE SPI1
+
#define ACC
+#define USE_ACC_SPI_MPU6000
+
#define GYRO
+#define USE_GYRO_SPI_MPU6000
+
#define INVERTER
#define BEEPER
@@ -45,6 +53,8 @@
#define SERIAL_PORT_COUNT 2
+#define USE_SPI
+
#define SENSORS_SET (SENSOR_ACC)
#define GPS
diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h
index 47a41511f..43f410b4b 100644
--- a/src/main/target/CHEBUZZF3/target.h
+++ b/src/main/target/CHEBUZZF3/target.h
@@ -35,7 +35,11 @@
#define BARO_PIN Pin_13
#define GYRO
+#define USE_GYRO_L3GD20
+
#define ACC
+#define USE_ACC_LSM303DLHC
+
#define BEEPER
#define LED0
#define LED1
diff --git a/src/main/target/CJMCU/target.h b/src/main/target/CJMCU/target.h
index b075e12fb..fb43ea589 100644
--- a/src/main/target/CJMCU/target.h
+++ b/src/main/target/CJMCU/target.h
@@ -33,8 +33,13 @@
#define LED2
#define LED2_PERIPHERAL RCC_APB2Periph_GPIOC
+
#define ACC
+#define USE_ACC_MPU6050
+
#define GYRO
+#define USE_GYRO_MPU6050
+
#define MAG
#define BRUSHED_MOTORS
diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h
index 67e2a520a..798442aa6 100644
--- a/src/main/target/EUSTM32F103RC/target.h
+++ b/src/main/target/EUSTM32F103RC/target.h
@@ -20,9 +20,37 @@
#define BARO_GPIO GPIOC
#define BARO_PIN Pin_13
+#define MPU6000_CS_GPIO GPIOB
+#define MPU6000_CS_PIN GPIO_Pin_12
+#define MPU6000_SPI_INSTANCE SPI2
+
+#define MPU6500_CS_GPIO GPIOB
+#define MPU6500_CS_PIN GPIO_Pin_12
+#define MPU6500_SPI_INSTANCE SPI2
+
#define ACC
-#define BARO
+#define USE_FAKE_ACC
+#define USE_ACC_ADXL345
+#define USE_ACC_BMA280
+#define USE_ACC_MMA8452
+#define USE_ACC_MPU3050
+#define USE_ACC_MPU6050
+#define USE_ACC_SPI_MPU6000
+#define USE_ACC_SPI_MPU6500
+
#define GYRO
+#define USE_FAKE_GYRO
+#define USE_GYRO_L3G4200D
+//#define USE_GYRO_L3GD20
+//#define USE_GYRO_MPU3050
+#define USE_GYRO_MPU6050
+#define USE_GYRO_SPI_MPU6000
+#define USE_GYRO_SPI_MPU6500
+
+#define BARO
+#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+
#define MAG
#define SONAR
#define DISPLAY
diff --git a/src/main/target/MASSIVEF3/target.h b/src/main/target/MASSIVEF3/target.h
index 759ba0951..fa47293e5 100644
--- a/src/main/target/MASSIVEF3/target.h
+++ b/src/main/target/MASSIVEF3/target.h
@@ -38,6 +38,11 @@
#define GYRO
#define ACC
+
+#define BARO
+#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+
#define BEEPER
#define LED0
#define LED1
diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h
index e49a99f7d..75f184a64 100644
--- a/src/main/target/NAZE/target.h
+++ b/src/main/target/NAZE/target.h
@@ -37,10 +37,30 @@
#define INVERTER_PERIPHERAL RCC_APB2Periph_GPIOB
#define INVERTER_USART USART2
+// SPI2
+// PB15 28 SPI2_MOSI
+// PB14 27 SPI2_MISO
+// PB13 26 SPI2_SCK
+// PB12 25 SPI2_NSS
+
+#define MPU6500_CS_GPIO GPIOB
+#define MPU6500_CS_PIN GPIO_Pin_12
+#define MPU6500_SPI_INSTANCE SPI2
+
#define GYRO
+#define USE_GYRO_MPU6050
+
#define ACC
-#define MAG
+#define USE_ACC_ADXL345
+#define USE_ACC_BMA280
+#define USE_ACC_MMA8452
+#define USE_ACC_MPU6050
+
#define BARO
+#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+
+#define MAG
#define SONAR
#define BEEPER
#define LED0
@@ -56,6 +76,12 @@
#define USE_I2C
#define I2C_DEVICE (I2CDEV_2)
+#define USE_SPI
+
+
+
+
+
// #define SOFT_I2C // enable to test software i2c
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)
// #define SOFT_I2C_PB67
diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h
index 25826a831..7d2e9a39d 100644
--- a/src/main/target/OLIMEXINO/target.h
+++ b/src/main/target/OLIMEXINO/target.h
@@ -35,9 +35,31 @@
#define LED1
#endif
+
#define ACC
-#define BARO
+#define USE_FAKE_ACC
+//#define USE_ACC_ADXL345
+//#define USE_ACC_BMA280
+//#define USE_ACC_MMA8452
+//#define USE_ACC_LSM303DLHC
+#define USE_ACC_MPU3050
+#define USE_ACC_MPU6050
+//#define USE_ACC_SPI_MPU6000
+//#define USE_ACC_SPI_MPU6500
+
#define GYRO
+#define USE_FAKE_GYRO
+//#define USE_GYRO_L3G4200D
+//#define USE_GYRO_L3GD20
+//#define USE_GYRO_MPU3050
+#define USE_GYRO_MPU6050
+//#define USE_GYRO_SPI_MPU6000
+//#define USE_GYRO_SPI_MPU6500
+
+#define BARO
+//#define USE_BARO_MS5611
+#define USE_BARO_BMP085
+
#define MAG
#define SONAR
diff --git a/src/main/target/STM32F3DISCOVERY/target.h b/src/main/target/STM32F3DISCOVERY/target.h
index 759ba0951..e54b6e932 100644
--- a/src/main/target/STM32F3DISCOVERY/target.h
+++ b/src/main/target/STM32F3DISCOVERY/target.h
@@ -37,7 +37,11 @@
#define BARO_PIN Pin_13
#define GYRO
+#define USE_GYRO_L3GD20
+
#define ACC
+#define USE_ACC_LSM303DLHC
+
#define BEEPER
#define LED0
#define LED1