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 MPU6000 into 

accgyro_mpu.c.
This commit is contained in:
Dominic Clifton 2015-09-19 17:11:04 +01:00
parent 0361d161fb
commit 678c0413cb
10 changed files with 98 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -433,6 +433,7 @@ CJMCU_SRC = \
CC3D_SRC = \
startup_stm32f10x_md_gcc.S \
drivers/accgyro_mpu.c \
drivers/accgyro_spi_mpu6000.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \

2
src/main/drivers/accgyro_adxl345.c
View File

@ -111,7 +111,7 @@ static bool adxl345Read(int16_t *accelData)
i++;
if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
return false;;
return false;
}
x += (int16_t)(buf[0] + (buf[1] << 8));

15
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_spi_mpu6000.h"
#include "accgyro_spi_mpu6500.h"
#include "accgyro_mpu.h"
@ -139,10 +140,22 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
return found;
#ifdef USE_GYRO_SPI_MPU6000
found = mpu6000SpiDetect();
if (found) {
mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU6000_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
return false;
}
#endif

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

@ -63,6 +63,7 @@ typedef enum {
MPU_NONE,
MPU_3050,
MPU_60x0,
MPU_60x0_SPI,
MPU_65xx_I2C,
MPU_65xx_SPI
} detectedMPUSensor_e;

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

@ -55,7 +55,7 @@ static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
{
if (mpuDetectionResult.sensor != MPU_3050) {
return false;;
return false;
}
gyro->init = mpu3050Init;
gyro->read = mpuGyroRead;

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

@ -164,7 +164,7 @@ bool mpu6050AccDetect(acc_t *acc)
bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
return false;;
return false;
}
gyro->init = mpu6050GyroInit;
gyro->read = mpuGyroRead;

159
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -33,45 +33,20 @@
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_spi.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_spi_mpu6000.h"
static bool mpuSpi6000InitDone = false;
static void mpu6000AccAndGyroInit(void);
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
static bool mpuSpi6000InitDone = false;
// Bits
#define BIT_SLEEP 0x40
@ -125,27 +100,46 @@ static bool mpuSpi6000InitDone = false;
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
return true;
}
static void mpu6000ReadRegister(uint8_t reg, uint8_t *data, int length)
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000;
return true;
}
void mpu6000SpiGyroInit(void)
{
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
int16_t data[3];
mpuGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(void)
@ -157,9 +151,6 @@ bool mpu6000SpiDetect(void)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
if (mpuSpi6000InitDone) {
return true;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
@ -168,7 +159,7 @@ bool mpu6000SpiDetect(void)
do {
delay(150);
mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
mpu6000ReadRegister(MPU6000_WHOAMI, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
@ -178,7 +169,7 @@ bool mpu6000SpiDetect(void)
} while (attemptsRemaining--);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
@ -202,7 +193,7 @@ bool mpu6000SpiDetect(void)
return false;
}
void mpu6000AccAndGyroInit() {
static void mpu6000AccAndGyroInit(void) {
if (mpuSpi6000InitDone) {
return;
@ -241,104 +232,36 @@ void mpu6000AccAndGyroInit() {
mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(acc_t *acc)
{
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
acc->init = mpu6000SpiAccInit;
acc->read = mpu6000SpiAccRead;
acc->read = mpuAccRead;
delay(100);
return true;
}
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
{
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
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;
delay(100);
return true;
}
bool mpu6000SpiGyroRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_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]);
return true;
}
bool mpu6000SpiAccRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_ACCEL_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]);
configureMPULPF(lpf);
return true;
}

37
src/main/drivers/accgyro_spi_mpu6000.h
View File

@ -3,6 +3,38 @@
#define MPU6000_CONFIG 0x1A
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
@ -12,9 +44,10 @@
#define MPU6000_WHO_AM_I_CONST (0x68)
bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

3
src/main/drivers/system.h
View File

@ -49,6 +49,7 @@ typedef enum {
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;

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_GYRO_SPI_MPU6500) || defined(USE_ACC_MPU6050)
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();