rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

accgyro_mpu.c.
This commit is contained in:
Dominic Clifton 2015-09-19 16:21:50 +01:00
parent da46d9f1d2
commit 0361d161fb
8 changed files with 122 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

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

85
src/main/drivers/accgyro_mpu.c
View File

@ -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 {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
} mpuConfiguration_t;
static mpuConfiguration_t mpuConfiguration;
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);
if (!ack)
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
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[1] = (int16_t)((buf[2] << 8) | buf[3]);
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
accData[0] = (int16_t)((data[0] << 8) | data[1]);
accData[1] = (int16_t)((data[2] << 8) | data[3]);
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[1] = (int16_t)((buf[2] << 8) | buf[3]);
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
return true;
}

10
src/main/drivers/accgyro_mpu.h
View File

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

12
src/main/drivers/accgyro_mpu3050.c
View File

@ -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);
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
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;
}

16
src/main/drivers/accgyro_mpu6050.c
View File

@ -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 = 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_SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
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 = 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_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.
// 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);
}

105
src/main/drivers/accgyro_spi_mpu6500.c
View File

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

18
src/main/drivers/accgyro_spi_mpu6500.h
View File

@ -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_PWR_MGMT_1 (0x6B)
#define MPU6500_RA_ACCEL_CFG (0x1C)
#define MPU6500_RA_LPF (0x1A)
#define MPU6500_RA_RATE_DIV (0x19)
#define MPU6500_RA_WHOAMI (0x75)
#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);

2
src/main/sensors/initialisation.c
View File

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