diff --git a/Makefile b/Makefile index 00de825b5..40ec0b1c1 100755 --- a/Makefile +++ b/Makefile @@ -292,6 +292,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/accgyro_bma280.c \ drivers/accgyro_l3g4200d.c \ drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_spi_mpu6500.c \ @@ -332,6 +333,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \ drivers/accgyro_bma280.c \ drivers/accgyro_l3g4200d.c \ drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_spi_mpu6000.c \ @@ -369,6 +371,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \ PORT103R_SRC = $(EUSTM32F103RC_SRC) OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/adc.c \ drivers/adc_stm32f10x.c \ @@ -408,6 +411,7 @@ CJMCU_SRC = \ startup_stm32f10x_md_gcc.S \ drivers/adc.c \ drivers/adc_stm32f10x.c \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/bus_i2c_stm32f10x.c \ drivers/compass_hmc5883l.c \ @@ -499,6 +503,7 @@ STM32F3DISCOVERY_SRC = \ drivers/accgyro_adxl345.c \ drivers/accgyro_bma280.c \ drivers/accgyro_mma845x.c \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_l3g4200d.c \ @@ -527,6 +532,7 @@ COLIBRI_RACE_SRC = \ SPARKY_SRC = \ $(STM32F30x_COMMON_SRC) \ drivers/display_ug2864hsweg01.c \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8975.c \ @@ -539,6 +545,7 @@ ALIENWIIF3_SRC = $(SPARKY_SRC) SPRACINGF3_SRC = \ $(STM32F30x_COMMON_SRC) \ + drivers/accgyro_mpu.c \ drivers/accgyro_mpu6050.c \ drivers/barometer_ms5611.c \ drivers/compass_ak8975.c \ diff --git a/src/main/drivers/accgyro.h b/src/main/drivers/accgyro.h index 90ddf510f..8ebbe3414 100644 --- a/src/main/drivers/accgyro.h +++ b/src/main/drivers/accgyro.h @@ -17,7 +17,7 @@ #pragma once -extern uint16_t acc_1G; +extern uint16_t acc_1G; // FIXME move into acc_t typedef struct gyro_s { sensorInitFuncPtr init; // initialize function diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c new file mode 100644 index 000000000..064ba5d54 --- /dev/null +++ b/src/main/drivers/accgyro_mpu.c @@ -0,0 +1,279 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" +#include "build_config.h" +#include "debug.h" + +#include "common/maths.h" + +#include "nvic.h" + +#include "system.h" +#include "gpio.h" +#include "exti.h" +#include "bus_i2c.h" + +#include "sensor.h" +#include "accgyro.h" +#include "accgyro_mpu6050.h" +#include "accgyro_mpu.h" + +//#define DEBUG_MPU_DATA_READY_INTERRUPT + +// MPU6050, Standard address 0x68 +// MPU_INT on PB13 on rev4 Naze32 hardware +#define MPU6050_ADDRESS 0x68 + + +uint8_t mpuLowPassFilter = INV_FILTER_42HZ; + +void mpu6050AccInit(void); +bool mpu6050AccRead(int16_t *accData); +void mpu6050GyroInit(void); +bool mpu6050GyroRead(int16_t *gyroADC); + +mpuDetectionResult_t mpuDetectionResult; + +static const extiConfig_t *mpuIntExtiConfig = NULL; + +// MPU6050 +#define MPU_RA_WHO_AM_I 0x75 +#define MPU_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS +#define MPU_RA_PRODUCT_ID 0x0C // Product ID Register +#define MPU_RA_ACCEL_XOUT_H 0x3B +#define MPU_RA_GYRO_XOUT_H 0x43 + +mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse) +{ + memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult)); + + mpuIntExtiConfig = configToUse; + + bool ack; + uint8_t sig; + uint8_t readBuffer[6]; + uint8_t revision; + uint8_t productId; + + delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe + + ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig); + if (!ack) + return &mpuDetectionResult; + + // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device. + // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address. + // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already). + // But here's the best part: The value of the AD0 pin is not reflected in this register. + + if (sig != (MPU6050_ADDRESS & 0x7e)) + + return &mpuDetectionResult; + + + // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code + // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c + + // determine product ID and accel revision + i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer); + revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); + if (revision) { + /* Congrats, these parts are better. */ + if (revision == 1) { + mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; + } else if (revision == 2) { + mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; + } else { + failureMode(FAILURE_ACC_INCOMPATIBLE); + } + } else { + i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId); + revision = productId & 0x0F; + if (!revision) { + failureMode(FAILURE_ACC_INCOMPATIBLE); + } else if (revision == 4) { + mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; + } else { + mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; + } + } + + mpuDetectionResult.sensor = MPU_60x0; + return &mpuDetectionResult; +} + +void MPU_DATA_READY_EXTI_Handler(void) +{ + if (EXTI_GetITStatus(mpuIntExtiConfig->exti_line) == RESET) { + return; + } + + EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line); + +#ifdef DEBUG_MPU_DATA_READY_INTERRUPT + // Measure the delta in micro seconds between calls to the interrupt handler + static uint32_t lastCalledAt = 0; + static int32_t callDelta = 0; + + uint32_t now = micros(); + callDelta = now - lastCalledAt; + + //UNUSED(callDelta); + debug[0] = callDelta; + + lastCalledAt = now; +#endif +} + +void configureMPUDataReadyInterruptHandling(void) +{ +#ifdef USE_MPU_DATA_READY_SIGNAL + +#ifdef STM32F10X + // enable AFIO for EXTI support + RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); +#endif + +#ifdef STM32F303xC + /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ + RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); +#endif + +#ifdef STM32F10X + gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source); +#endif + +#ifdef STM32F303xC + gpioExtiLineConfig(mpuIntExtiConfig->exti_port_source, mpuIntExtiConfig->exti_pin_source); +#endif + +#ifdef ENSURE_MPU_DATA_READY_IS_LOW + uint8_t status = GPIO_ReadInputDataBit(mpuIntExtiConfig->gpioPort, mpuIntExtiConfig->gpioPin); + if (status) { + return; + } +#endif + + registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler); + + EXTI_ClearITPendingBit(mpuIntExtiConfig->exti_line); + + EXTI_InitTypeDef EXTIInit; + EXTIInit.EXTI_Line = mpuIntExtiConfig->exti_line; + EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; + EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising; + EXTIInit.EXTI_LineCmd = ENABLE; + EXTI_Init(&EXTIInit); + + NVIC_InitTypeDef NVIC_InitStructure; + + NVIC_InitStructure.NVIC_IRQChannel = mpuIntExtiConfig->exti_irqn; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY); + NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY); + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); +#endif +} + +void mpuIntExtiInit(void) +{ + gpio_config_t gpio; + + static bool mpuExtiInitDone = false; + + if (mpuExtiInitDone || !mpuIntExtiConfig) { + return; + } + +#ifdef STM32F303 + if (mpuIntExtiConfig->gpioAHBPeripherals) { + RCC_AHBPeriphClockCmd(mpuIntExtiConfig->gpioAHBPeripherals, ENABLE); + } +#endif +#ifdef STM32F10X + if (mpuIntExtiConfig->gpioAPB2Peripherals) { + RCC_APB2PeriphClockCmd(mpuIntExtiConfig->gpioAPB2Peripherals, ENABLE); + } +#endif + + gpio.pin = mpuIntExtiConfig->gpioPin; + gpio.speed = Speed_2MHz; + gpio.mode = Mode_IN_FLOATING; + gpioInit(mpuIntExtiConfig->gpioPort, &gpio); + + configureMPUDataReadyInterruptHandling(); + + mpuExtiInitDone = true; +} + +void configureMPULPF(uint16_t lpf) +{ + if (lpf == 256) + mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; + else 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 if (lpf > 0) + mpuLowPassFilter = INV_FILTER_5HZ; + else + mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; +} + +bool mpuAccRead(int16_t *accData) +{ + uint8_t buf[6]; + + bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf); + 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]); + + return true; +} + +bool mpuGyroRead(int16_t *gyroADC) +{ + uint8_t buf[6]; + + bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf); + 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]); + + return true; +} diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h new file mode 100644 index 000000000..b36c78eee --- /dev/null +++ b/src/main/drivers/accgyro_mpu.h @@ -0,0 +1,75 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + + +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 +}; + +typedef enum { + MPU_NONE, + MPU_3050, + MPU_60x0, + MPU_65xx_I2C, + MPU_65xx_SPI +} detectedMPUSensor_e; + +typedef enum { + MPU_HALF_RESOLUTION, + MPU_FULL_RESOLUTION +} mpu6050Resolution_e; + +typedef struct mpuDetectionResult_s { + detectedMPUSensor_e sensor; + mpu6050Resolution_e resolution; +} mpuDetectionResult_t; + +void configureMPULPF(uint16_t lpf); +void configureMPUDataReadyInterruptHandling(void); +void mpuIntExtiInit(void); +bool mpuAccRead(int16_t *accData); +bool mpuGyroRead(int16_t *gyroADC); +mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse); diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 15d2c28fe..b790e6bc9 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -29,12 +29,17 @@ #include "system.h" #include "gpio.h" +#include "exti.h" #include "bus_i2c.h" #include "sensor.h" #include "accgyro.h" +#include "accgyro_mpu.h" #include "accgyro_mpu6050.h" +extern mpuDetectionResult_t mpuDetectionResult; +extern uint8_t mpuLowPassFilter; + //#define DEBUG_MPU_DATA_READY_INTERRUPT // MPU6050, Standard address 0x68 @@ -140,291 +145,55 @@ #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 -}; -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 -}; - -static uint8_t mpuLowPassFilter = INV_FILTER_42HZ; static void mpu6050AccInit(void); -static bool mpu6050AccRead(int16_t *accData); static void mpu6050GyroInit(void); -static bool mpu6050GyroRead(int16_t *gyroADC); -typedef enum { - MPU_6050_HALF_RESOLUTION, - MPU_6050_FULL_RESOLUTION -} mpu6050Resolution_e; - -static mpu6050Resolution_e mpuAccelTrim; - -static const mpu6050Config_t *mpu6050Config = NULL; - -void MPU_DATA_READY_EXTI_Handler(void) +bool mpu6050AccDetect(acc_t *acc) { - if (EXTI_GetITStatus(mpu6050Config->exti_line) == RESET) { - return; - } - - EXTI_ClearITPendingBit(mpu6050Config->exti_line); - -#ifdef DEBUG_MPU_DATA_READY_INTERRUPT - // Measure the delta in micro seconds between calls to the interrupt handler - static uint32_t lastCalledAt = 0; - static int32_t callDelta = 0; - - uint32_t now = micros(); - callDelta = now - lastCalledAt; - - //UNUSED(callDelta); - debug[0] = callDelta; - - lastCalledAt = now; -#endif -} - -void configureMPUDataReadyInterruptHandling(void) -{ -#ifdef USE_MPU_DATA_READY_SIGNAL - -#ifdef STM32F10X - // enable AFIO for EXTI support - RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); -#endif - -#ifdef STM32F303xC - /* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */ - RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); -#endif - -#ifdef STM32F10X - gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source); -#endif - -#ifdef STM32F303xC - gpioExtiLineConfig(mpu6050Config->exti_port_source, mpu6050Config->exti_pin_source); -#endif - -#ifdef ENSURE_MPU_DATA_READY_IS_LOW - uint8_t status = GPIO_ReadInputDataBit(mpu6050Config->gpioPort, mpu6050Config->gpioPin); - if (status) { - return; - } -#endif - - registerExti15_10_CallbackHandler(MPU_DATA_READY_EXTI_Handler); - - EXTI_ClearITPendingBit(mpu6050Config->exti_line); - - EXTI_InitTypeDef EXTIInit; - EXTIInit.EXTI_Line = mpu6050Config->exti_line; - EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; - EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising; - EXTIInit.EXTI_LineCmd = ENABLE; - EXTI_Init(&EXTIInit); - - NVIC_InitTypeDef NVIC_InitStructure; - - NVIC_InitStructure.NVIC_IRQChannel = mpu6050Config->exti_irqn; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_PRIORITY_BASE(NVIC_PRIO_MPU_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelSubPriority = NVIC_PRIORITY_SUB(NVIC_PRIO_MPU_DATA_READY); - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); -#endif -} - -void mpu6050GpioInit(void) { - gpio_config_t gpio; - - static bool mpu6050GpioInitDone = false; - - if (mpu6050GpioInitDone || !mpu6050Config) { - return; - } - -#ifdef STM32F303 - if (mpu6050Config->gpioAHBPeripherals) { - RCC_AHBPeriphClockCmd(mpu6050Config->gpioAHBPeripherals, ENABLE); - } -#endif -#ifdef STM32F10X - if (mpu6050Config->gpioAPB2Peripherals) { - RCC_APB2PeriphClockCmd(mpu6050Config->gpioAPB2Peripherals, ENABLE); - } -#endif - - gpio.pin = mpu6050Config->gpioPin; - gpio.speed = Speed_2MHz; - gpio.mode = Mode_IN_FLOATING; - gpioInit(mpu6050Config->gpioPort, &gpio); - - configureMPUDataReadyInterruptHandling(); - - mpu6050GpioInitDone = true; -} - -static bool mpu6050Detect(void) -{ - bool ack; - uint8_t sig; - - delay(35); // datasheet page 13 says 30ms. other stuff could have been running meanwhile. but we'll be safe - - ack = i2cRead(MPU6050_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig); - if (!ack) + if (mpuDetectionResult.sensor != MPU_60x0) { return false; - - // So like, MPU6xxx has a "WHO_AM_I" register, that is used to verify the identity of the device. - // The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0�s 7-bit I2C address. - // The least significant bit of the MPU-60X0�s I2C address is determined by the value of the AD0 pin. (we know that already). - // But here's the best part: The value of the AD0 pin is not reflected in this register. - - if (sig != (MPU6050_ADDRESS & 0x7e)) - return false; - - return true; -} - -bool mpu6050AccDetect(const mpu6050Config_t *configToUse, acc_t *acc) -{ - uint8_t readBuffer[6]; - uint8_t revision; - uint8_t productId; - - mpu6050Config = configToUse; - - if (!mpu6050Detect()) { - return false; - } - - // There is a map of revision contained in the android source tree which is quite comprehensive and may help to understand this code - // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c - - // determine product ID and accel revision - i2cRead(MPU6050_ADDRESS, MPU_RA_XA_OFFS_H, 6, readBuffer); - revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01); - if (revision) { - /* Congrats, these parts are better. */ - if (revision == 1) { - mpuAccelTrim = MPU_6050_HALF_RESOLUTION; - } else if (revision == 2) { - mpuAccelTrim = MPU_6050_FULL_RESOLUTION; - } else { - failureMode(FAILURE_ACC_INCOMPATIBLE); - } - } else { - i2cRead(MPU6050_ADDRESS, MPU_RA_PRODUCT_ID, 1, &productId); - revision = productId & 0x0F; - if (!revision) { - failureMode(FAILURE_ACC_INCOMPATIBLE); - } else if (revision == 4) { - mpuAccelTrim = MPU_6050_HALF_RESOLUTION; - } else { - mpuAccelTrim = MPU_6050_FULL_RESOLUTION; - } } acc->init = mpu6050AccInit; - acc->read = mpu6050AccRead; - acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. + acc->read = mpuAccRead; + acc->revisionCode = (mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. return true; } -bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_t lpf) +bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf) { - mpu6050Config = configToUse; - - if (!mpu6050Detect()) { - return false; + if (mpuDetectionResult.sensor != MPU_60x0) { + return false;; } - - gyro->init = mpu6050GyroInit; - gyro->read = mpu6050GyroRead; + gyro->read = mpuGyroRead; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; - if (lpf == 256) - mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; - else 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 if (lpf > 0) - mpuLowPassFilter = INV_FILTER_5HZ; - else - mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2; + configureMPULPF(lpf); return true; } static void mpu6050AccInit(void) { - mpu6050GpioInit(); + mpuIntExtiInit(); - switch (mpuAccelTrim) { - case MPU_6050_HALF_RESOLUTION: + switch (mpuDetectionResult.resolution) { + case MPU_HALF_RESOLUTION: acc_1G = 256 * 8; break; - case MPU_6050_FULL_RESOLUTION: + case MPU_FULL_RESOLUTION: acc_1G = 512 * 8; break; } } -static bool mpu6050AccRead(int16_t *accData) -{ - uint8_t buf[6]; - - bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf); - 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]); - - return true; -} - static void mpu6050GyroInit(void) { - mpu6050GpioInit(); + mpuIntExtiInit(); bool ack; ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 @@ -447,19 +216,3 @@ static void mpu6050GyroInit(void) #endif UNUSED(ack); } - -static bool mpu6050GyroRead(int16_t *gyroADC) -{ - uint8_t buf[6]; - - bool ack = i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf); - 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]); - - return true; -} diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index da25684cb..83c1caab8 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -17,23 +17,5 @@ #pragma once -typedef struct mpu6050Config_s { -#ifdef STM32F303 - uint32_t gpioAHBPeripherals; -#endif -#ifdef STM32F10X - uint32_t gpioAPB2Peripherals; -#endif - uint16_t gpioPin; - GPIO_TypeDef *gpioPort; - - uint8_t exti_port_source; - uint32_t exti_line; - uint8_t exti_pin_source; - IRQn_Type exti_irqn; -} mpu6050Config_t; - -bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc); -bool mpu6050GyroDetect(const mpu6050Config_t *config, gyro_t *gyro, uint16_t lpf); -void mpu6050DmpLoop(void); -void mpu6050DmpResetFifo(void); +bool mpu6050AccDetect(acc_t *acc); +bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf); diff --git a/src/main/drivers/exti.h b/src/main/drivers/exti.h new file mode 100644 index 000000000..4e48d9052 --- /dev/null +++ b/src/main/drivers/exti.h @@ -0,0 +1,35 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + + +#pragma once + +typedef struct extiConfig_s { +#ifdef STM32F303 + uint32_t gpioAHBPeripherals; +#endif +#ifdef STM32F10X + uint32_t gpioAPB2Peripherals; +#endif + uint16_t gpioPin; + GPIO_TypeDef *gpioPort; + + uint8_t exti_port_source; + uint32_t exti_line; + uint8_t exti_pin_source; + IRQn_Type exti_irqn; +} extiConfig_t; diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 333e39c0a..0c3487ad7 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -24,6 +24,10 @@ #include "common/axis.h" +#include "drivers/gpio.h" +#include "drivers/system.h" +#include "drivers/exti.h" + #include "drivers/sensor.h" #include "drivers/accgyro.h" @@ -31,6 +35,7 @@ #include "drivers/accgyro_bma280.h" #include "drivers/accgyro_l3g4200d.h" #include "drivers/accgyro_mma845x.h" +#include "drivers/accgyro_mpu.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_l3gd20.h" @@ -50,9 +55,6 @@ #include "drivers/sonar_hcsr04.h" -#include "drivers/gpio.h" -#include "drivers/system.h" - #include "config/runtime_config.h" #include "sensors/sensors.h" @@ -76,11 +78,11 @@ extern acc_t acc; uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; -const mpu6050Config_t *selectMPU6050Config(void) +const extiConfig_t *selectMPUIntExtiConfig(void) { #ifdef NAZE // MPU_INT output on rev4 PB13 - static const mpu6050Config_t nazeRev4MPU6050Config = { + static const extiConfig_t nazeRev4MPUIntExtiConfig = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .gpioPin = Pin_13, .gpioPort = GPIOB, @@ -90,7 +92,7 @@ const mpu6050Config_t *selectMPU6050Config(void) .exti_irqn = EXTI15_10_IRQn }; // MPU_INT output on rev5 hardware PC13 - static const mpu6050Config_t nazeRev5MPU6050Config = { + static const extiConfig_t nazeRev5MPUIntExtiConfig = { .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .gpioPin = Pin_13, .gpioPort = GPIOC, @@ -101,14 +103,14 @@ const mpu6050Config_t *selectMPU6050Config(void) }; if (hardwareRevision < NAZE32_REV5) { - return &nazeRev4MPU6050Config; + return &nazeRev4MPUIntExtiConfig; } else { - return &nazeRev5MPU6050Config; + return &nazeRev5MPUIntExtiConfig; } #endif #ifdef SPRACINGF3 - static const mpu6050Config_t spRacingF3MPU6050Config = { + static const extiConfig_t spRacingF3MPUIntExtiConfig = { .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioPort = GPIOC, .gpioPin = Pin_13, @@ -117,7 +119,7 @@ const mpu6050Config_t *selectMPU6050Config(void) .exti_line = EXTI_Line13, .exti_irqn = EXTI15_10_IRQn }; - return &spRacingF3MPU6050Config; + return &spRacingF3MPUIntExtiConfig; #endif return NULL; @@ -171,7 +173,7 @@ bool detectGyro(uint16_t gyroLpf) ; // fallthrough case GYRO_MPU6050: #ifdef USE_GYRO_MPU6050 - if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) { + if (mpu6050GyroDetect(&gyro, gyroLpf)) { #ifdef GYRO_MPU6050_ALIGN gyroHardware = GYRO_MPU6050; gyroAlign = GYRO_MPU6050_ALIGN; @@ -279,7 +281,7 @@ static void detectAcc(accelerationSensor_e accHardwareToUse) { accelerationSensor_e accHardware; - #ifdef USE_ACC_ADXL345 +#ifdef USE_ACC_ADXL345 drv_adxl345_config_t acc_params; #endif @@ -319,7 +321,7 @@ retry: ; // fallthrough case ACC_MPU6050: // MPU6050 #ifdef USE_ACC_MPU6050 - if (mpu6050AccDetect(selectMPU6050Config(), &acc)) { + if (mpu6050AccDetect(&acc)) { #ifdef ACC_MPU6050_ALIGN accAlign = ACC_MPU6050_ALIGN; #endif @@ -597,6 +599,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t memset(&acc, 0, sizeof(acc)); memset(&gyro, 0, sizeof(gyro)); +#if defined(USE_GYRO_MPU6050) || defined(USE_ACC_MPU6050) + + const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); + + mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig); + UNUSED(mpuDetectionResult); +#endif + if (!detectGyro(gyroLpf)) { return false; }