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

Add basic MPU6000 auto detection code. De-duplictate repeated code in

Browse source 
MPU6000.
This commit is contained in:
Dominic Clifton 2014-11-01 10:45:44 +00:00
parent 0ba2933611
commit 715c175aad
2 changed files with 51 additions and 61 deletions
Download patch file Download diff file Expand all files Collapse all files

110
src/main/drivers/accgyro_spi_mpu6000.c
View file

@ -125,6 +125,22 @@
void mpu6000SpiGyroRead(int16_t *gyroData); void mpu6000SpiGyroRead(int16_t *gyroData);
void mpu6000SpiAccRead(int16_t *gyroData); void mpu6000SpiAccRead(int16_t *gyroData);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
}
static void mpu6000ReadRegister(uint8_t reg, uint8_t *data, int length)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000;
}
void mpu6000SpiGyroInit(void) void mpu6000SpiGyroInit(void)
{ {
} }
@ -136,20 +152,22 @@ void mpu6000SpiAccInit(void)
bool mpu6000SpiDetect(void) bool mpu6000SpiDetect(void)
{ {
// FIXME this isn't working, not debugged yet. uint8_t in;
return true; // just assume it's there for now
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
if (in != MPU6000_WHO_AM_I_CONST)
return false;
return true;
#if 0 #if 0
// FIXME this isn't working, not debugged yet.
uint8_t product; uint8_t product;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
ENABLE_MPU6000; mpu6000ReadRegister(MPU6000_PRODUCT_ID, &product, 1);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PRODUCT_ID);
spiTransfer(MPU6000_SPI_INSTANCE, &product, NULL, 1);
DISABLE_MPU6000;
/* look for a product ID we recognise */ /* look for a product ID we recognise */
@ -184,59 +202,36 @@ void mpu6000AccAndGyroInit() {
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
ENABLE_MPU6000; // Device Reset
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_1); // Device Reset mpu6000WriteRegister(MPU6000_PWR_MGMT_1, BIT_H_RESET);
spiTransferByte(MPU6000_SPI_INSTANCE, BIT_H_RESET);
DISABLE_MPU6000;
delay(150); delay(150);
ENABLE_MPU6000; mpu6000WriteRegister(MPU6000_SIGNAL_PATH_RESET, BIT_GYRO | BIT_ACC | BIT_TEMP);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_SIGNAL_PATH_RESET); // Device Reset
spiTransferByte(MPU6000_SPI_INSTANCE, BIT_GYRO | BIT_ACC | BIT_TEMP);
DISABLE_MPU6000;
delay(150); delay(150);
ENABLE_MPU6000; // Clock Source PPL with Z axis gyro reference
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_1); // Clock Source PPL with Z axis gyro reference mpu6000WriteRegister(MPU6000_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU_CLK_SEL_PLLGYROZ);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
ENABLE_MPU6000; // Disable Primary I2C Interface
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_USER_CTRL); // Disable Primary I2C Interface mpu6000WriteRegister(MPU6000_USER_CTRL, BIT_I2C_IF_DIS);
spiTransferByte(MPU6000_SPI_INSTANCE, BIT_I2C_IF_DIS);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
ENABLE_MPU6000; mpu6000WriteRegister(MPU6000_PWR_MGMT_2, 0x00);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_PWR_MGMT_2);
spiTransferByte(MPU6000_SPI_INSTANCE, 0x00);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
ENABLE_MPU6000; // Accel Sample Rate 1kHz
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_SMPLRT_DIV); // Accel Sample Rate 1kHz // Gyroscope Output Rate = 1kHz when the DLPF is enabled
spiTransferByte(MPU6000_SPI_INSTANCE, 0x00); // Gyroscope Output Rate = 1kHz when the DLPF is enabled mpu6000WriteRegister(MPU6000_SMPLRT_DIV, 0x00);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
ENABLE_MPU6000; // Accel +/- 8 G Full Scale
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_ACCEL_CONFIG); // Accel +/- 8 G Full Scale mpu6000WriteRegister(MPU6000_ACCEL_CONFIG, BITS_FS_8G);
spiTransferByte(MPU6000_SPI_INSTANCE, BITS_FS_8G);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
ENABLE_MPU6000; // Gyro +/- 1000 DPS Full Scale
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_GYRO_CONFIG); // Gyro +/- 1000 DPS Full Scale mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
spiTransferByte(MPU6000_SPI_INSTANCE, BITS_FS_2000DPS); delayMicroseconds(1);
DISABLE_MPU6000;
initDone = true; initDone = true;
} }
@ -302,11 +297,8 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
ENABLE_MPU6000; // Accel and Gyro DLPF Setting
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_CONFIG); // Accel and Gyro DLPF Setting mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
spiTransferByte(MPU6000_SPI_INSTANCE, mpuLowPassFilter);
DISABLE_MPU6000;
delayMicroseconds(1); delayMicroseconds(1);
mpu6000SpiGyroRead(data); mpu6000SpiGyroRead(data);
@ -327,12 +319,10 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
void mpu6000SpiGyroRead(int16_t *gyroData) void mpu6000SpiGyroRead(int16_t *gyroData)
{ {
uint8_t buf[6]; uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
ENABLE_MPU6000; mpu6000ReadRegister(MPU6000_GYRO_XOUT_H, buf, 6);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_GYRO_XOUT_H | 0x80);
spiTransfer(MPU6000_SPI_INSTANCE, buf, NULL, 6);
DISABLE_MPU6000;
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
@ -342,12 +332,10 @@ void mpu6000SpiGyroRead(int16_t *gyroData)
void mpu6000SpiAccRead(int16_t *gyroData) void mpu6000SpiAccRead(int16_t *gyroData)
{ {
uint8_t buf[6]; uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
ENABLE_MPU6000; mpu6000ReadRegister(MPU6000_ACCEL_XOUT_H, buf, 6);
spiTransferByte(MPU6000_SPI_INSTANCE, MPU6000_ACCEL_XOUT_H | 0x80);
spiTransfer(MPU6000_SPI_INSTANCE, buf, NULL, 6);
DISABLE_MPU6000;
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]); gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);

2
src/main/drivers/accgyro_spi_mpu6000.h
View file

@ -10,6 +10,8 @@
#define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS) #define GYRO_SCALE_FACTOR 0.00053292f // (4/131) * pi/180 (32.75 LSB = 1 DPS)
#define MPU6000_WHO_AM_I_CONST (0x68)
bool mpu6000SpiAccDetect(acc_t *acc); bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf); bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);