Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 22:05:17 +03:00
Code Issues Wiki Activity

Add support for MPU6500 connected via I2C.

Browse source
This commit is contained in:
Dominic Clifton 2015-09-19 18:30:34 +01:00
parent 678c0413cb
commit 85ba1eb0bd
14 changed files with 202 additions and 88 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -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 \

2
docs/Cli.md
View file

@ -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 |

9
src/main/drivers/accgyro_mpu.c
View file

@ -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;
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;

2
src/main/drivers/accgyro_mpu6050.c
View file

@ -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);

110
src/main/drivers/accgyro_mpu6500.c Normal file
View file

@ -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
}

42
src/main/drivers/accgyro_mpu6500.h Normal file
View file

@ -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);

37
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -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
}

4
src/main/drivers/accgyro_spi_mpu6500.h
View file

@ -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);

4
src/main/sensors/acceleration.h
View file

@ -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;

4
src/main/sensors/gyro.h
View file

@ -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;

57
src/main/sensors/initialisation.c
View file

@ -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();

4
src/main/target/CC3D/target.h
View file

@ -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

4
src/main/target/COLIBRI_RACE/target.h
View file

@ -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

6
src/main/target/NAZE/target.h
View file

@ -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