From 79917da85a1a333d9d88dfc2611cf50fd3be3417 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Wed, 15 Oct 2014 23:06:04 +0100 Subject: [PATCH] Use Naze hardware revision when initialising mpu6050. The driver is not not naze specific anymore. --- src/main/drivers/accgyro_mpu6050.c | 89 ++++++++++++++++++------------ src/main/drivers/accgyro_mpu6050.h | 10 +++- src/main/sensors/initialisation.c | 26 ++++++++- 3 files changed, 87 insertions(+), 38 deletions(-) diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 467a0d58b..e3a89383b 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -17,6 +17,7 @@ #include #include +#include #include "platform.h" @@ -128,35 +129,36 @@ #define MPU6050_SMPLRT_DIV 0 // 8000Hz 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 + 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 + 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 + 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 + INV_FSR_2G = 0, + INV_FSR_4G, + INV_FSR_8G, + INV_FSR_16G, + NUM_ACCEL_FSR }; + static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6050AccInit(void); static void mpu6050AccRead(int16_t *accData); @@ -170,6 +172,21 @@ typedef enum { static mpu6050Resolution_e mpuAccelTrim; +static mpu6050Config_t *config = NULL; + +void mpu6050GpioInit(void) { + gpio_config_t gpio; + + if (config->gpioAPB2Peripherals) { + RCC_APB2PeriphClockCmd(config->gpioAPB2Peripherals, ENABLE); + } + + gpio.pin = config->gpioPin; + gpio.speed = Speed_2MHz; + gpio.mode = Mode_IN_FLOATING; + gpioInit(config->gpioPort, &gpio); +} + static bool mpu6050Detect(void) { bool ack; @@ -192,12 +209,14 @@ static bool mpu6050Detect(void) return true; } -bool mpu6050AccDetect(acc_t *acc) +bool mpu6050AccDetect(mpu6050Config_t *configToUse, acc_t *acc) { uint8_t readBuffer[6]; uint8_t revision; uint8_t productId; + config = configToUse; + if (!mpu6050Detect()) { return false; } @@ -236,12 +255,15 @@ bool mpu6050AccDetect(acc_t *acc) return true; } -bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) +bool mpu6050GyroDetect(mpu6050Config_t *configToUse, gyro_t *gyro, uint16_t lpf) { + config = configToUse; + if (!mpu6050Detect()) { return false; } + gyro->init = mpu6050GyroInit; gyro->read = mpu6050GyroRead; @@ -266,6 +288,11 @@ bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) static void mpu6050AccInit(void) { + if (config) { + mpu6050GpioInit(); + config = NULL; // avoid re-initialisation of GPIO; + } + switch (mpuAccelTrim) { case MPU_6050_HALF_RESOLUTION: acc_1G = 256 * 8; @@ -288,16 +315,10 @@ static void mpu6050AccRead(int16_t *accData) static void mpu6050GyroInit(void) { - gpio_config_t gpio; - - // MPU_INT output on rev4/5 hardware (PB13, PC13) - gpio.pin = Pin_13; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - if (hse_value == 8000000) - gpioInit(GPIOB, &gpio); - else if (hse_value == 12000000) - gpioInit(GPIOC, &gpio); + if (config) { + mpu6050GpioInit(); + config = NULL; // avoid re-initialisation of GPIO; + } i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index bf65ab037..5067769ac 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -17,7 +17,13 @@ #pragma once -bool mpu6050AccDetect(acc_t *acc); -bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf); +typedef struct mpu6050Config_s { + uint32_t gpioAPB2Peripherals; + uint16_t gpioPin; + GPIO_TypeDef *gpioPort; +} mpu6050Config_t; + +bool mpu6050AccDetect(mpu6050Config_t *config,acc_t *acc); +bool mpu6050GyroDetect(mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf); void mpu6050DmpLoop(void); void mpu6050DmpResetFifo(void); diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index a02005848..a6fe6004e 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -44,6 +44,7 @@ #include "drivers/barometer_ms5611.h" #include "drivers/compass_hmc5883l.h" #include "drivers/sonar_hcsr04.h" +#include "drivers/gpio.h" #include "drivers/system.h" #include "flight/flight.h" @@ -66,6 +67,27 @@ extern gyro_t gyro; extern baro_t baro; extern acc_t acc; +mpu6050Config_t *generateMPU6050Config(void) +{ +#ifdef NAZE + static mpu6050Config_t nazeMPU6050Config; + + // MPU_INT output on rev4/5 hardware (PB13, PC13) + nazeMPU6050Config.gpioPin = Pin_13; + + + if (hardwareRevision < NAZE32_REV5) { + nazeMPU6050Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB; + nazeMPU6050Config.gpioPort = GPIOB; + } else { + nazeMPU6050Config.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC; + nazeMPU6050Config.gpioPort = GPIOC; + } + return &nazeMPU6050Config; +#endif + return NULL; +} + #ifdef USE_FAKE_GYRO static void fakeGyroInit(void) {} static void fakeGyroRead(int16_t *gyroData) { @@ -105,7 +127,7 @@ bool detectGyro(uint16_t gyroLpf) gyroAlign = ALIGN_DEFAULT; #ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(&gyro, gyroLpf)) { + if (mpu6050GyroDetect(generateMPU6050Config(), &gyro, gyroLpf)) { #ifdef NAZE gyroAlign = CW0_DEG; #endif @@ -211,7 +233,7 @@ retry: #endif #ifdef USE_ACC_MPU6050 case ACC_MPU6050: // MPU6050 - if (mpu6050AccDetect(&acc)) { + if (mpu6050AccDetect(generateMPU6050Config(), &acc)) { accHardware = ACC_MPU6050; #ifdef NAZE accAlign = CW0_DEG;