From 0361d161fb3bd60b170678a0da319b00b3e52e09 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 19 Sep 2015 16:21:50 +0100 Subject: [PATCH] Relocate and use some of the common MPU code from MPU6500 into accgyro_mpu.c. --- Makefile | 1 + src/main/drivers/accgyro_mpu.c | 85 +++++++++++++++----- src/main/drivers/accgyro_mpu.h | 10 +++ src/main/drivers/accgyro_mpu3050.c | 12 +-- src/main/drivers/accgyro_mpu6050.c | 16 ++-- src/main/drivers/accgyro_spi_mpu6500.c | 105 +++++-------------------- src/main/drivers/accgyro_spi_mpu6500.h | 18 +++-- src/main/sensors/initialisation.c | 2 +- 8 files changed, 122 insertions(+), 127 deletions(-) diff --git a/Makefile b/Makefile index 40ec0b1c1..45db036b4 100755 --- a/Makefile +++ b/Makefile @@ -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 \ diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 9708ece29..e9745617a 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -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; } diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index b36c78eee..6d946c1b4 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -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, diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index a45f1a751..77061ee5b 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -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; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index b790e6bc9..84881cc9b 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -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); } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 08b14ebbf..95bbcfa57 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -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; -} diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 20f4e06b9..11a3c3e3d 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -15,16 +15,17 @@ * along with Cleanflight. If not, see . */ -#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); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2fdc24d2b..fa43959d8 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -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();