rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Relocate some of the common MPU code from MPU6050 into accgyro_mpu.c.

This commit is contained in:
Dominic Clifton 2015-09-18 20:23:50 +01:00
parent 9f95334347
commit b46d56a5bd
8 changed files with 441 additions and 300 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
Makefile
View File

@ -292,6 +292,7 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \
drivers/accgyro_bma280.c \ drivers/accgyro_bma280.c \
drivers/accgyro_l3g4200d.c \ drivers/accgyro_l3g4200d.c \
drivers/accgyro_mma845x.c \ drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/accgyro_spi_mpu6500.c \ drivers/accgyro_spi_mpu6500.c \
@ -332,6 +333,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
drivers/accgyro_bma280.c \ drivers/accgyro_bma280.c \
drivers/accgyro_l3g4200d.c \ drivers/accgyro_l3g4200d.c \
drivers/accgyro_mma845x.c \ drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/accgyro_spi_mpu6000.c \ drivers/accgyro_spi_mpu6000.c \
@ -369,6 +371,7 @@ EUSTM32F103RC_SRC = startup_stm32f10x_hd_gcc.S \
PORT103R_SRC = $(EUSTM32F103RC_SRC) PORT103R_SRC = $(EUSTM32F103RC_SRC)
OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/adc.c \ drivers/adc.c \
drivers/adc_stm32f10x.c \ drivers/adc_stm32f10x.c \
@ -408,6 +411,7 @@ CJMCU_SRC = \
startup_stm32f10x_md_gcc.S \ startup_stm32f10x_md_gcc.S \
drivers/adc.c \ drivers/adc.c \
drivers/adc_stm32f10x.c \ drivers/adc_stm32f10x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/bus_i2c_stm32f10x.c \ drivers/bus_i2c_stm32f10x.c \
drivers/compass_hmc5883l.c \ drivers/compass_hmc5883l.c \
@ -499,6 +503,7 @@ STM32F3DISCOVERY_SRC = \
drivers/accgyro_adxl345.c \ drivers/accgyro_adxl345.c \
drivers/accgyro_bma280.c \ drivers/accgyro_bma280.c \
drivers/accgyro_mma845x.c \ drivers/accgyro_mma845x.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/accgyro_l3g4200d.c \ drivers/accgyro_l3g4200d.c \
@ -527,6 +532,7 @@ COLIBRI_RACE_SRC = \
SPARKY_SRC = \ SPARKY_SRC = \
$(STM32F30x_COMMON_SRC) \ $(STM32F30x_COMMON_SRC) \
drivers/display_ug2864hsweg01.c \ drivers/display_ug2864hsweg01.c \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \ drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \ drivers/compass_ak8975.c \
@ -539,6 +545,7 @@ ALIENWIIF3_SRC = $(SPARKY_SRC)
SPRACINGF3_SRC = \ SPRACINGF3_SRC = \
$(STM32F30x_COMMON_SRC) \ $(STM32F30x_COMMON_SRC) \
drivers/accgyro_mpu.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/barometer_ms5611.c \ drivers/barometer_ms5611.c \
drivers/compass_ak8975.c \ drivers/compass_ak8975.c \

2
src/main/drivers/accgyro.h
View File

@ -17,7 +17,7 @@
#pragma once #pragma once
extern uint16_t acc_1G; extern uint16_t acc_1G; // FIXME move into acc_t
typedef struct gyro_s { typedef struct gyro_s {
sensorInitFuncPtr init; // initialize function sensorInitFuncPtr init; // initialize function

279
src/main/drivers/accgyro_mpu.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#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<58>s 7-bit I2C address.
// The least significant bit of the MPU-60X0<58>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;
}

75
src/main/drivers/accgyro_mpu.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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);

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

@ -29,12 +29,17 @@
#include "system.h" #include "system.h"
#include "gpio.h" #include "gpio.h"
#include "exti.h"
#include "bus_i2c.h" #include "bus_i2c.h"
#include "sensor.h" #include "sensor.h"
#include "accgyro.h" #include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu6050.h" #include "accgyro_mpu6050.h"
extern mpuDetectionResult_t mpuDetectionResult;
extern uint8_t mpuLowPassFilter;
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
// MPU6050, Standard address 0x68 // MPU6050, Standard address 0x68
@ -140,291 +145,55 @@
#define MPU6050_SMPLRT_DIV 0 // 8000Hz #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 void mpu6050AccInit(void);
static bool mpu6050AccRead(int16_t *accData);
static void mpu6050GyroInit(void); static void mpu6050GyroInit(void);
static bool mpu6050GyroRead(int16_t *gyroADC);
typedef enum { bool mpu6050AccDetect(acc_t *acc)
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)
{ {
if (EXTI_GetITStatus(mpu6050Config->exti_line) == RESET) { if (mpuDetectionResult.sensor != MPU_60x0) {
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)
return false; 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<58>s 7-bit I2C address.
// The least significant bit of the MPU-60X0<58>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->init = mpu6050AccInit;
acc->read = mpu6050AccRead; acc->read = mpuAccRead;
acc->revisionCode = (mpuAccelTrim == MPU_6050_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES. 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; 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 (mpuDetectionResult.sensor != MPU_60x0) {
return false;;
if (!mpu6050Detect()) {
return false;
} }
gyro->init = mpu6050GyroInit; gyro->init = mpu6050GyroInit;
gyro->read = mpu6050GyroRead; gyro->read = mpuGyroRead;
// 16.4 dps/lsb scalefactor // 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
if (lpf == 256) configureMPULPF(lpf);
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;
return true; return true;
} }
static void mpu6050AccInit(void) static void mpu6050AccInit(void)
{ {
mpu6050GpioInit(); mpuIntExtiInit();
switch (mpuAccelTrim) { switch (mpuDetectionResult.resolution) {
case MPU_6050_HALF_RESOLUTION: case MPU_HALF_RESOLUTION:
acc_1G = 256 * 8; acc_1G = 256 * 8;
break; break;
case MPU_6050_FULL_RESOLUTION: case MPU_FULL_RESOLUTION:
acc_1G = 512 * 8; acc_1G = 512 * 8;
break; 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) static void mpu6050GyroInit(void)
{ {
mpu6050GpioInit(); mpuIntExtiInit();
bool ack; bool ack;
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 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 #endif
UNUSED(ack); 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;
}

22
src/main/drivers/accgyro_mpu6050.h
View File

@ -17,23 +17,5 @@
#pragma once #pragma once
typedef struct mpu6050Config_s { bool mpu6050AccDetect(acc_t *acc);
#ifdef STM32F303 bool mpu6050GyroDetect(gyro_t *gyro, uint16_t lpf);
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);

35
src/main/drivers/exti.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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;

34
src/main/sensors/initialisation.c
View File

@ -24,6 +24,10 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "drivers/exti.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
@ -31,6 +35,7 @@
#include "drivers/accgyro_bma280.h" #include "drivers/accgyro_bma280.h"
#include "drivers/accgyro_l3g4200d.h" #include "drivers/accgyro_l3g4200d.h"
#include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu.h"
#include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6050.h"
#include "drivers/accgyro_l3gd20.h" #include "drivers/accgyro_l3gd20.h"
@ -50,9 +55,6 @@
#include "drivers/sonar_hcsr04.h" #include "drivers/sonar_hcsr04.h"
#include "drivers/gpio.h"
#include "drivers/system.h"
#include "config/runtime_config.h" #include "config/runtime_config.h"
#include "sensors/sensors.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 }; 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 #ifdef NAZE
// MPU_INT output on rev4 PB13 // MPU_INT output on rev4 PB13
static const mpu6050Config_t nazeRev4MPU6050Config = { static const extiConfig_t nazeRev4MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOB, .gpioAPB2Peripherals = RCC_APB2Periph_GPIOB,
.gpioPin = Pin_13, .gpioPin = Pin_13,
.gpioPort = GPIOB, .gpioPort = GPIOB,
@ -90,7 +92,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
.exti_irqn = EXTI15_10_IRQn .exti_irqn = EXTI15_10_IRQn
}; };
// MPU_INT output on rev5 hardware PC13 // MPU_INT output on rev5 hardware PC13
static const mpu6050Config_t nazeRev5MPU6050Config = { static const extiConfig_t nazeRev5MPUIntExtiConfig = {
.gpioAPB2Peripherals = RCC_APB2Periph_GPIOC, .gpioAPB2Peripherals = RCC_APB2Periph_GPIOC,
.gpioPin = Pin_13, .gpioPin = Pin_13,
.gpioPort = GPIOC, .gpioPort = GPIOC,
@ -101,14 +103,14 @@ const mpu6050Config_t *selectMPU6050Config(void)
}; };
if (hardwareRevision < NAZE32_REV5) { if (hardwareRevision < NAZE32_REV5) {
return &nazeRev4MPU6050Config; return &nazeRev4MPUIntExtiConfig;
} else { } else {
return &nazeRev5MPU6050Config; return &nazeRev5MPUIntExtiConfig;
} }
#endif #endif
#ifdef SPRACINGF3 #ifdef SPRACINGF3
static const mpu6050Config_t spRacingF3MPU6050Config = { static const extiConfig_t spRacingF3MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC, .gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC, .gpioPort = GPIOC,
.gpioPin = Pin_13, .gpioPin = Pin_13,
@ -117,7 +119,7 @@ const mpu6050Config_t *selectMPU6050Config(void)
.exti_line = EXTI_Line13, .exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn .exti_irqn = EXTI15_10_IRQn
}; };
return &spRacingF3MPU6050Config; return &spRacingF3MPUIntExtiConfig;
#endif #endif
return NULL; return NULL;
@ -171,7 +173,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough ; // fallthrough
case GYRO_MPU6050: case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050 #ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) { if (mpu6050GyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_MPU6050_ALIGN #ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050; gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN; gyroAlign = GYRO_MPU6050_ALIGN;
@ -319,7 +321,7 @@ retry:
; // fallthrough ; // fallthrough
case ACC_MPU6050: // MPU6050 case ACC_MPU6050: // MPU6050
#ifdef USE_ACC_MPU6050 #ifdef USE_ACC_MPU6050
if (mpu6050AccDetect(selectMPU6050Config(), &acc)) { if (mpu6050AccDetect(&acc)) {
#ifdef ACC_MPU6050_ALIGN #ifdef ACC_MPU6050_ALIGN
accAlign = ACC_MPU6050_ALIGN; accAlign = ACC_MPU6050_ALIGN;
#endif #endif
@ -597,6 +599,14 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
memset(&acc, 0, sizeof(acc)); memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro)); 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)) { if (!detectGyro(gyroLpf)) {
return false; return false;
} }