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

Merge pull request #1326 from cleanflight/nazerev6

Browse source 
Naze32 Revision 6 support
This commit is contained in:
Dominic Clifton 2015-10-06 18:23:35 +01:00
commit 0f87d1ff87
36 changed files with 1168 additions and 734 deletions
Download patch file Download diff file Expand all files Collapse all files

11
Makefile
View file

@ -292,13 +292,16 @@ 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_mpu6500.c \
drivers/accgyro_spi_mpu6500.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
drivers/barometer_bmp085.c \
drivers/barometer_ms5611.c \
drivers/barometer_bmp280.c \
drivers/bus_spi.c \
drivers/bus_i2c_stm32f10x.c \
drivers/compass_hmc5883l.c \
@ -332,6 +335,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 +373,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 +413,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 \
@ -429,6 +435,7 @@ CJMCU_SRC = \
CC3D_SRC = \
startup_stm32f10x_md_gcc.S \
drivers/accgyro_mpu.c \
drivers/accgyro_spi_mpu6000.c \
drivers/adc.c \
drivers/adc_stm32f10x.c \
@ -499,6 +506,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 \
@ -515,6 +523,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 \
@ -527,6 +536,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 +549,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 \

2
docs/Cli.md
View file

@ -204,7 +204,7 @@ Re-apply any new defaults as desired.
| `rx_min_usec` | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 2000 | 985 | Profile | UINT16 |
| `rx_max_usec` | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of `mid_rc`. | 100 | 3000 | 2115 | Profile | UINT16 |
| `gimbal_mode` | When feature SERVO_TILT is enabled, this can be either 0=normal gimbal (default) or 1=tiltmix gimbal | 0 | 1 | 0 | Profile | UINT8 |
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for SPI_MPU6000, 8 for SPI_MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| `acc_hardware` | This is used to suggest which accelerometer driver should load, or to force no accelerometer in case gyro-only flight is needed. Default (0) will attempt to auto-detect among enabled drivers. Otherwise, to force a particular device, set it to 2 for ADXL345, 3 for MPU6050 integrated accelerometer, 4 for MMA8452, 5 for BMA280, 6 for LSM303DLHC, 7 for MPU6000, 8 for MPU6500 or 1 to disable accelerometer alltogether - resulting in gyro-only operation. | 0 | 9 | 0 | Master | UINT8 |
| `acc_lpf_factor` | This setting controls the Low Pass Filter factor for ACC. Increasing this value reduces ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter | 0 | 250 | 4 | Profile | UINT8 |
| `accxy_deadband` | | 0 | 100 | 40 | Profile | UINT8 |
| `accz_deadband` | | 0 | 100 | 40 | Profile | UINT8 |

4
src/main/drivers/accgyro.h
View file

@ -17,10 +17,10 @@
#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
sensorGyroInitFuncPtr init; // initialize function
sensorReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
float scale; // scalefactor

2
src/main/drivers/accgyro_adxl345.c
View file

@ -111,7 +111,7 @@ static bool adxl345Read(int16_t *accelData)
i++;
if (!i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf)) {
return false;;
return false;
}
x += (int16_t)(buf[0] + (buf[1] << 8));

23
src/main/drivers/accgyro_l3g4200d.c
View file

@ -54,12 +54,10 @@
#define L3G4200D_DLPF_78HZ 0x80
#define L3G4200D_DLPF_93HZ 0xC0
static uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
static void l3g4200dInit(void);
static void l3g4200dInit(uint16_t lpf);
static bool l3g4200dRead(int16_t *gyroADC);
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
bool l3g4200dDetect(gyro_t *gyro)
{
uint8_t deviceid;
@ -75,7 +73,15 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
// 14.2857dps/lsb scalefactor
gyro->scale = 1.0f / 14.2857f;
// default LPF is set to 32Hz
return true;
}
static void l3g4200dInit(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = L3G4200D_DLPF_32HZ;
switch (lpf) {
default:
case 32:
@ -92,13 +98,6 @@ bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf)
break;
}
return true;
}
static void l3g4200dInit(void)
{
bool ack;
delay(100);
ack = i2cWrite(L3G4200D_ADDRESS, L3G4200D_CTRL_REG4, L3G4200D_FS_SEL_2000DPS);

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

@ -17,4 +17,4 @@
#pragma once
bool l3g4200dDetect(gyro_t *gyro, uint16_t lpf);
bool l3g4200dDetect(gyro_t *gyro);

8
src/main/drivers/accgyro_l3gd20.c
View file

@ -86,8 +86,10 @@ static void l3gd20SpiInit(SPI_TypeDef *SPIx)
spiSetDivisor(L3GD20_SPI, SPI_9MHZ_CLOCK_DIVIDER);
}
void l3gd20GyroInit(void)
void l3gd20GyroInit(uint16_t lpf)
{
UNUSED(lpf); // FIXME use it!
l3gd20SpiInit(L3GD20_SPI);
GPIO_ResetBits(L3GD20_CS_GPIO, L3GD20_CS_PIN);
@ -150,10 +152,8 @@ static bool l3gd20GyroRead(int16_t *gyroADC)
// Page 9 in datasheet, So - Sensitivity, Full Scale = 2000, 70 mdps/digit
#define L3GD20_GYRO_SCALE_FACTOR 0.07f
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf)
bool l3gd20Detect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = l3gd20GyroInit;
gyro->read = l3gd20GyroRead;

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

@ -17,4 +17,4 @@
#pragma once
bool l3gd20Detect(gyro_t *gyro, uint16_t lpf);
bool l3gd20Detect(gyro_t *gyro);

364
src/main/drivers/accgyro_mpu.c Normal file
View file

@ -0,0 +1,364 @@
/*
* 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_mpu3050.h"
#include "accgyro_mpu6050.h"
#include "accgyro_mpu6500.h"
#include "accgyro_spi_mpu6000.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);
static void mpu6050FindRevision(void);
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void);
#endif
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_ADDRESS 0x68
// MPU6050
#define MPU_RA_WHO_AM_I 0x75
#define MPU_RA_WHO_AM_I_LEGACY 0x00
#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
// WHO_AM_I register contents for MPU3050, 6050 and 6500
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPUx0x0_WHO_AM_I_CONST (0x68)
#define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
{
memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
mpuIntExtiConfig = configToUse;
bool ack;
uint8_t sig;
uint8_t inquiryResult;
// MPU datasheet specifies 30ms.
delay(35);
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 = 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;
mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision();
} else if (sig == MPU6500_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &mpuDetectionResult;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void)
{
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) {
mpuDetectionResult.sensor = MPU_65xx_SPI;
mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU6000_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
return false;
}
#endif
static void mpu6050FindRevision(void)
{
bool ack;
UNUSED(ack);
uint8_t readBuffer[6];
uint8_t revision;
uint8_t productId;
// 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
ack = mpuConfiguration.read(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 {
ack = mpuConfiguration.read(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;
}
}
}
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;
}
uint8_t determineMPULPF(uint16_t lpf)
{
uint8_t mpuLowPassFilter;
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;
return mpuLowPassFilter;
}
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 data[6];
bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
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 data[6];
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}
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;
}

88
src/main/drivers/accgyro_mpu.h Normal file
View file

@ -0,0 +1,88 @@
/*
* 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 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,
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_60x0_SPI,
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;
extern mpuDetectionResult_t mpuDetectionResult;
uint8_t determineMPULPF(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);

86
src/main/drivers/accgyro_mpu3050.c
View file

@ -23,26 +23,17 @@
#include "common/maths.h"
#include "system.h"
#include "exti.h"
#include "bus_i2c.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu3050.h"
// MPU3050, Standard address 0x68
#define MPU3050_ADDRESS 0x68
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
// Bits
#define MPU3050_FS_SEL_2000DPS 0x18
#define MPU3050_DLPF_10HZ 0x05
@ -55,91 +46,46 @@
#define MPU3050_USER_RESET 0x01
#define MPU3050_CLK_SEL_PLL_GX 0x01
static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ;
static void mpu3050Init(void);
static bool mpu3050Read(int16_t *gyroADC);
static void mpu3050Init(uint16_t lpf);
static bool mpu3050ReadTemp(int16_t *tempData);
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf)
bool mpu3050Detect(gyro_t *gyro)
{
bool ack;
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);
if (!ack)
if (mpuDetectionResult.sensor != MPU_3050) {
return false;
}
gyro->init = mpu3050Init;
gyro->read = mpu3050Read;
gyro->read = mpuGyroRead;
gyro->temperature = mpu3050ReadTemp;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = MPU3050_DLPF_256HZ;
break;
case 188:
mpuLowPassFilter = MPU3050_DLPF_188HZ;
break;
case 98:
mpuLowPassFilter = MPU3050_DLPF_98HZ;
break;
default:
case 42:
mpuLowPassFilter = MPU3050_DLPF_42HZ;
break;
case 20:
mpuLowPassFilter = MPU3050_DLPF_20HZ;
break;
case 10:
mpuLowPassFilter = MPU3050_DLPF_10HZ;
break;
}
return true;
}
static void mpu3050Init(void)
static void mpu3050Init(uint16_t lpf)
{
bool ack;
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
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);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static bool mpu3050Read(int16_t *gyroADC)
{
uint8_t buf[6];
if (!i2cRead(MPU3050_ADDRESS, MPU3050_GYRO_OUT, 6, buf)) {
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;
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;
}

11
src/main/drivers/accgyro_mpu3050.h
View file

@ -17,4 +17,13 @@
#pragma once
bool mpu3050Detect(gyro_t *gyro, uint16_t lpf);
// Registers
#define MPU3050_SMPLRT_DIV 0x15
#define MPU3050_DLPF_FS_SYNC 0x16
#define MPU3050_INT_CFG 0x17
#define MPU3050_TEMP_OUT 0x1B
#define MPU3050_GYRO_OUT 0x1D
#define MPU3050_USER_CTRL 0x3D
#define MPU3050_PWR_MGM 0x3E
bool mpu3050Detect(gyro_t *gyro);

309
src/main/drivers/accgyro_mpu6050.c
View file

@ -29,12 +29,16 @@
#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 uint8_t mpuLowPassFilter;
//#define DEBUG_MPU_DATA_READY_INTERRUPT
// MPU6050, Standard address 0x68
@ -140,326 +144,75 @@
#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);
static void mpu6050GyroInit(uint16_t lpf);
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<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->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)
{
mpu6050Config = configToUse;
if (!mpu6050Detect()) {
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;
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)
static void mpu6050GyroInit(uint16_t lpf)
{
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();
bool ack;
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
mpuIntExtiInit();
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
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.
// ACC Init stuff.
// 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);
}
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
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);

105
src/main/drivers/accgyro_mpu6500.c Normal file
View file

@ -0,0 +1,105 @@
/*
* 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 "platform.h"
#include "common/axis.h"
#include "common/maths.h"
#include "system.h"
#include "exti.h"
#include "gpio.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_mpu6500.h"
extern uint16_t acc_1G;
bool mpu6500AccDetect(acc_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}
acc->init = mpu6500AccInit;
acc->read = mpuAccRead;
return true;
}
bool mpu6500GyroDetect(gyro_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}
gyro->init = mpu6500GyroInit;
gyro->read = mpuGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
return true;
}
void mpu6500AccInit(void)
{
acc_1G = 512 * 8;
}
void mpu6500GyroInit(uint16_t lpf)
{
#ifdef NAZE
// FIXME target specific code in driver code.
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
delay(100);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, 0);
delay(100);
mpuConfiguration.write(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
mpuConfiguration.write(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpuConfiguration.write(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
mpuConfiguration.write(MPU6500_RA_LPF, mpuLowPassFilter);
mpuConfiguration.write(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
// Data ready interrupt configuration
mpuConfiguration.write(MPU6500_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU6500_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
}

44
src/main/drivers/accgyro_mpu6500.h Normal file
View file

@ -0,0 +1,44 @@
/*
* 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/>.
*/
#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_INT_PIN_CFG (0x37)
#define MPU6500_RA_INT_ENABLE (0x38)
#define MPU6500_RA_GYRO_XOUT_H (0x43)
#define MPU6500_RA_SIGNAL_PATH_RST (0x68)
#define MPU6500_RA_USER_CTRL (0x6A)
#define MPU6500_RA_PWR_MGMT_1 (0x6B)
#define MPU6500_RA_BANK_SEL (0x6D)
#define MPU6500_RA_MEM_RW (0x6F)
#define MPU6500_RA_WHOAMI (0x75)
#define MPU6500_RA_XA_OFFS_H (0x77)
#define MPU6500_WHO_AM_I_CONST (0x70)
#define MPU6500_BIT_RESET (0x80)
#pragma once
bool mpu6500AccDetect(acc_t *acc);
bool mpu6500GyroDetect(gyro_t *gyro);
void mpu6500AccInit(void);
void mpu6500GyroInit(uint16_t lpf);

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

@ -33,45 +33,19 @@
#include "system.h"
#include "gpio.h"
#include "exti.h"
#include "bus_spi.h"
#include "sensor.h"
#include "accgyro.h"
#include "accgyro_mpu.h"
#include "accgyro_spi_mpu6000.h"
static bool mpuSpi6000InitDone = false;
static void mpu6000AccAndGyroInit(void);
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
extern uint8_t mpuLowPassFilter;
static bool mpuSpi6000InitDone = false;
// Bits
#define BIT_SLEEP 0x40
@ -125,27 +99,48 @@ static bool mpuSpi6000InitDone = false;
#define DISABLE_MPU6000 GPIO_SetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
#define ENABLE_MPU6000 GPIO_ResetBits(MPU6000_CS_GPIO, MPU6000_CS_PIN)
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
static void mpu6000WriteRegister(uint8_t reg, uint8_t data)
bool mpu6000WriteRegister(uint8_t reg, uint8_t data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg);
spiTransferByte(MPU6000_SPI_INSTANCE, data);
DISABLE_MPU6000;
return true;
}
static void mpu6000ReadRegister(uint8_t reg, uint8_t *data, int length)
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data)
{
ENABLE_MPU6000;
spiTransferByte(MPU6000_SPI_INSTANCE, reg | 0x80); // read transaction
spiTransfer(MPU6000_SPI_INSTANCE, data, NULL, length);
DISABLE_MPU6000;
return true;
}
void mpu6000SpiGyroInit(void)
void mpu6000SpiGyroInit(uint16_t lpf)
{
uint8_t mpuLowPassFilter = determineMPULPF(lpf);
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
int16_t data[3];
mpuGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiAccInit(void)
@ -157,9 +152,6 @@ bool mpu6000SpiDetect(void)
{
uint8_t in;
uint8_t attemptsRemaining = 5;
if (mpuSpi6000InitDone) {
return true;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
@ -168,7 +160,7 @@ bool mpu6000SpiDetect(void)
do {
delay(150);
mpu6000ReadRegister(MPU6000_WHOAMI, &in, 1);
mpu6000ReadRegister(MPU6000_WHOAMI, 1, &in);
if (in == MPU6000_WHO_AM_I_CONST) {
break;
}
@ -178,7 +170,7 @@ bool mpu6000SpiDetect(void)
} while (attemptsRemaining--);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, &in, 1);
mpu6000ReadRegister(MPU6000_PRODUCT_ID, 1, &in);
/* look for a product ID we recognise */
@ -202,7 +194,7 @@ bool mpu6000SpiDetect(void)
return false;
}
void mpu6000AccAndGyroInit() {
static void mpu6000AccAndGyroInit(void) {
if (mpuSpi6000InitDone) {
return;
@ -241,104 +233,34 @@ void mpu6000AccAndGyroInit() {
mpu6000WriteRegister(MPU6000_GYRO_CONFIG, BITS_FS_2000DPS);
delayMicroseconds(1);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpuSpi6000InitDone = true;
}
bool mpu6000SpiAccDetect(acc_t *acc)
{
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
acc->init = mpu6000SpiAccInit;
acc->read = mpu6000SpiAccRead;
acc->read = mpuAccRead;
delay(100);
return true;
}
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
bool mpu6000SpiGyroDetect(gyro_t *gyro)
{
if (!mpu6000SpiDetect()) {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
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;
delay(100);
return true;
}
bool mpu6000SpiGyroRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_GYRO_XOUT_H, buf, 6);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}
bool mpu6000SpiAccRead(int16_t *gyroData)
{
uint8_t buf[6];
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); // 18 MHz SPI clock
mpu6000ReadRegister(MPU6000_ACCEL_XOUT_H, buf, 6);
gyroData[X] = (int16_t)((buf[0] << 8) | buf[1]);
gyroData[Y] = (int16_t)((buf[2] << 8) | buf[3]);
gyroData[Z] = (int16_t)((buf[4] << 8) | buf[5]);
return true;
}

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

@ -3,6 +3,38 @@
#define MPU6000_CONFIG 0x1A
// Registers
#define MPU6000_PRODUCT_ID 0x0C
#define MPU6000_SMPLRT_DIV 0x19
#define MPU6000_GYRO_CONFIG 0x1B
#define MPU6000_ACCEL_CONFIG 0x1C
#define MPU6000_FIFO_EN 0x23
#define MPU6000_INT_PIN_CFG 0x37
#define MPU6000_INT_ENABLE 0x38
#define MPU6000_INT_STATUS 0x3A
#define MPU6000_ACCEL_XOUT_H 0x3B
#define MPU6000_ACCEL_XOUT_L 0x3C
#define MPU6000_ACCEL_YOUT_H 0x3D
#define MPU6000_ACCEL_YOUT_L 0x3E
#define MPU6000_ACCEL_ZOUT_H 0x3F
#define MPU6000_ACCEL_ZOUT_L 0x40
#define MPU6000_TEMP_OUT_H 0x41
#define MPU6000_TEMP_OUT_L 0x42
#define MPU6000_GYRO_XOUT_H 0x43
#define MPU6000_GYRO_XOUT_L 0x44
#define MPU6000_GYRO_YOUT_H 0x45
#define MPU6000_GYRO_YOUT_L 0x46
#define MPU6000_GYRO_ZOUT_H 0x47
#define MPU6000_GYRO_ZOUT_L 0x48
#define MPU6000_USER_CTRL 0x6A
#define MPU6000_SIGNAL_PATH_RESET 0x68
#define MPU6000_PWR_MGMT_1 0x6B
#define MPU6000_PWR_MGMT_2 0x6C
#define MPU6000_FIFO_COUNTH 0x72
#define MPU6000_FIFO_COUNTL 0x73
#define MPU6000_FIFO_R_W 0x74
#define MPU6000_WHOAMI 0x75
#define BITS_DLPF_CFG_256HZ 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
@ -12,9 +44,10 @@
#define MPU6000_WHO_AM_I_CONST (0x68)
bool mpu6000SpiDetect(void);
bool mpu6000SpiAccDetect(acc_t *acc);
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6000SpiGyroDetect(gyro_t *gyro);
bool mpu6000SpiGyroRead(int16_t *gyroADC);
bool mpu6000SpiAccRead(int16_t *gyroADC);
bool mpu6000WriteRegister(uint8_t reg, uint8_t data);
bool mpu6000ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

135
src/main/drivers/accgyro_spi_mpu6500.c
View file

@ -25,73 +25,39 @@
#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_mpu6500.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;
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 +99,13 @@ static void mpu6500SpiInit(void)
hardwareInitialised = true;
}
static bool mpu6500Detect(void)
bool mpu6500SpiDetect(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,97 +115,28 @@ 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)
bool mpu6500SpiGyroDetect(gyro_t *gyro)
{
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;
return true;
}
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
gpio_config_t gpio;
// MPU_INT output on rev5 hardware (PC13). rev4 was on PB13, conflicts with SPI devices
if (hse_value == 12000000) {
gpio.pin = Pin_13;
gpio.speed = Speed_2MHz;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOC, &gpio);
}
#endif
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
delay(100);
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3);
mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3);
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;
}

22
src/main/drivers/accgyro_spi_mpu6500.h
View file

@ -15,22 +15,12 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#define MPU6500_RA_WHOAMI (0x75)
#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
#define MPU6500_RA_GYRO_XOUT_H (0x43)
#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_WHO_AM_I_CONST (0x70)
#define MPU6500_BIT_RESET (0x80)
#pragma once
bool mpu6500SpiDetect(void);
bool mpu6500SpiAccDetect(acc_t *acc);
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
bool mpu6500SpiGyroDetect(gyro_t *gyro);
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);

212
src/main/drivers/barometer_bmp280.c Normal file
View file

@ -0,0 +1,212 @@
/*
* 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 <platform.h>
#include "build_config.h"
#include "barometer.h"
#include "system.h"
#include "bus_i2c.h"
#include "barometer_bmp280.h"
#ifdef BARO
// BMP280, address 0x76
#define BMP280_I2C_ADDR (0x76)
#define BMP280_DEFAULT_CHIP_ID (0x58)
#define BMP280_CHIP_ID_REG (0xD0) /* Chip ID Register */
#define BMP280_RST_REG (0xE0) /* Softreset Register */
#define BMP280_STAT_REG (0xF3) /* Status Register */
#define BMP280_CTRL_MEAS_REG (0xF4) /* Ctrl Measure Register */
#define BMP280_CONFIG_REG (0xF5) /* Configuration Register */
#define BMP280_PRESSURE_MSB_REG (0xF7) /* Pressure MSB Register */
#define BMP280_PRESSURE_LSB_REG (0xF8) /* Pressure LSB Register */
#define BMP280_PRESSURE_XLSB_REG (0xF9) /* Pressure XLSB Register */
#define BMP280_TEMPERATURE_MSB_REG (0xFA) /* Temperature MSB Reg */
#define BMP280_TEMPERATURE_LSB_REG (0xFB) /* Temperature LSB Reg */
#define BMP280_TEMPERATURE_XLSB_REG (0xFC) /* Temperature XLSB Reg */
#define BMP280_FORCED_MODE (0x01)
#define BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG (0x88)
#define BMP280_PRESSURE_TEMPERATURE_CALIB_DATA_LENGTH (24)
#define BMP280_DATA_FRAME_SIZE (6)
#define BMP280_OVERSAMP_SKIPPED (0x00)
#define BMP280_OVERSAMP_1X (0x01)
#define BMP280_OVERSAMP_2X (0x02)
#define BMP280_OVERSAMP_4X (0x03)
#define BMP280_OVERSAMP_8X (0x04)
#define BMP280_OVERSAMP_16X (0x05)
// configure pressure and temperature oversampling, forced sampling mode
#define BMP280_PRESSURE_OSR (BMP280_OVERSAMP_8X)
#define BMP280_TEMPERATURE_OSR (BMP280_OVERSAMP_1X)
#define BMP280_MODE (BMP280_PRESSURE_OSR << 2 | BMP280_TEMPERATURE_OSR << 5 | BMP280_FORCED_MODE)
#define T_INIT_MAX (20)
// 20/16 = 1.25 ms
#define T_MEASURE_PER_OSRS_MAX (37)
// 37/16 = 2.3125 ms
#define T_SETUP_PRESSURE_MAX (10)
// 10/16 = 0.625 ms
typedef struct bmp280_calib_param_t {
uint16_t dig_T1; /* calibration T1 data */
int16_t dig_T2; /* calibration T2 data */
int16_t dig_T3; /* calibration T3 data */
uint16_t dig_P1; /* calibration P1 data */
int16_t dig_P2; /* calibration P2 data */
int16_t dig_P3; /* calibration P3 data */
int16_t dig_P4; /* calibration P4 data */
int16_t dig_P5; /* calibration P5 data */
int16_t dig_P6; /* calibration P6 data */
int16_t dig_P7; /* calibration P7 data */
int16_t dig_P8; /* calibration P8 data */
int16_t dig_P9; /* calibration P9 data */
int32_t t_fine; /* calibration t_fine data */
} bmp280_calib_param_t;
static uint8_t bmp280_chip_id = 0;
static bool bmp280InitDone = false;
static bmp280_calib_param_t bmp280_cal;
// uncompensated pressure and temperature
static int32_t bmp280_up = 0;
static int32_t bmp280_ut = 0;
static void bmp280_start_ut(void);
static void bmp280_get_ut(void);
static void bmp280_start_up(void);
static void bmp280_get_up(void);
static void bmp280_calculate(int32_t *pressure, int32_t *temperature);
bool bmp280Detect(baro_t *baro)
{
if (bmp280InitDone)
return true;
delay(20);
i2cRead(BMP280_I2C_ADDR, BMP280_CHIP_ID_REG, 1, &bmp280_chip_id); /* read Chip Id */
if (bmp280_chip_id != BMP280_DEFAULT_CHIP_ID)
return false;
// read calibration
i2cRead(BMP280_I2C_ADDR, BMP280_TEMPERATURE_CALIB_DIG_T1_LSB_REG, 24, (uint8_t *)&bmp280_cal);
// set oversampling + power mode (forced), and start sampling
i2cWrite(BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
bmp280InitDone = true;
// these are dummy as temperature is measured as part of pressure
baro->ut_delay = 0;
baro->get_ut = bmp280_get_ut;
baro->start_ut = bmp280_start_ut;
// only _up part is executed, and gets both temperature and pressure
baro->up_delay = ((T_INIT_MAX + T_MEASURE_PER_OSRS_MAX * (((1 << BMP280_TEMPERATURE_OSR) >> 1) + ((1 << BMP280_PRESSURE_OSR) >> 1)) + (BMP280_PRESSURE_OSR ? T_SETUP_PRESSURE_MAX : 0) + 15) / 16) * 1000;
baro->start_up = bmp280_start_up;
baro->get_up = bmp280_get_up;
baro->calculate = bmp280_calculate;
return true;
}
static void bmp280_start_ut(void)
{
// dummy
}
static void bmp280_get_ut(void)
{
// dummy
}
static void bmp280_start_up(void)
{
// start measurement
// set oversampling + power mode (forced), and start sampling
i2cWrite(BMP280_I2C_ADDR, BMP280_CTRL_MEAS_REG, BMP280_MODE);
}
static void bmp280_get_up(void)
{
uint8_t data[BMP280_DATA_FRAME_SIZE];
// read data from sensor
i2cRead(BMP280_I2C_ADDR, BMP280_PRESSURE_MSB_REG, BMP280_DATA_FRAME_SIZE, data);
bmp280_up = (int32_t)((((uint32_t)(data[0])) << 12) | (((uint32_t)(data[1])) << 4) | ((uint32_t)data[2] >> 4));
bmp280_ut = (int32_t)((((uint32_t)(data[3])) << 12) | (((uint32_t)(data[4])) << 4) | ((uint32_t)data[5] >> 4));
}
// Returns temperature in DegC, float precision. Output value of “51.23” equals 51.23 DegC.
// t_fine carries fine temperature as global value
float bmp280_compensate_T(int32_t adc_T)
{
float var1, var2, T;
var1 = (((float)adc_T) / 16384.0f - ((float)bmp280_cal.dig_T1) / 1024.0f) * ((float)bmp280_cal.dig_T2);
var2 = ((((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f) * (((float)adc_T) / 131072.0f - ((float)bmp280_cal.dig_T1) / 8192.0f)) * ((float)bmp280_cal.dig_T3);
bmp280_cal.t_fine = (int32_t)(var1 + var2);
T = (var1 + var2) / 5120.0f;
return T;
}
// Returns pressure in Pa as float. Output value of “96386.2” equals 96386.2 Pa = 963.862 hPa
float bmp280_compensate_P(int32_t adc_P)
{
float var1, var2, p;
var1 = ((float)bmp280_cal.t_fine / 2.0f) - 64000.0f;
var2 = var1 * var1 * ((float)bmp280_cal.dig_P6) / 32768.0f;
var2 = var2 + var1 * ((float)bmp280_cal.dig_P5) * 2.0f;
var2 = (var2 / 4.0f) + (((float)bmp280_cal.dig_P4) * 65536.0f);
var1 = (((float)bmp280_cal.dig_P3) * var1 * var1 / 524288.0f + ((float)bmp280_cal.dig_P2) * var1) / 524288.0f;
var1 = (1.0f + var1 / 32768.0f) * ((float)bmp280_cal.dig_P1);
if (var1 == 0.0f)
return 0.0f; // avoid exception caused by division by zero
p = 1048576.0f - (float)adc_P;
p = (p - (var2 / 4096.0f)) * 6250.0f / var1;
var1 = ((float)bmp280_cal.dig_P9) * p * p / 2147483648.0f;
var2 = p * ((float)bmp280_cal.dig_P8) / 32768.0f;
p = p + (var1 + var2 + ((float)bmp280_cal.dig_P7)) / 16.0f;
return p;
}
static void bmp280_calculate(int32_t *pressure, int32_t *temperature)
{
// calculate
float t, p;
t = bmp280_compensate_T(bmp280_ut);
p = bmp280_compensate_P(bmp280_up);
if (pressure)
*pressure = (int32_t)p;
if (temperature)
*temperature = (int32_t)t * 100;
}
#endif

21
src/main/drivers/barometer_bmp280.h Normal file
View file

@ -0,0 +1,21 @@
/*
* 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
bool bmp280Detect(baro_t *baro);

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;

1
src/main/drivers/sensor.h
View file

@ -19,3 +19,4 @@
typedef void (*sensorInitFuncPtr)(void); // sensor init prototype
typedef bool (*sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef void (*sensorGyroInitFuncPtr)(uint16_t lpf); // gyro sensor init prototype

3
src/main/drivers/system.h
View file

@ -49,6 +49,7 @@ typedef enum {
FAILURE_ACC_INIT,
FAILURE_ACC_INCOMPATIBLE,
FAILURE_INVALID_EEPROM_CONTENTS,
FAILURE_FLASH_WRITE_FAILED
FAILURE_FLASH_WRITE_FAILED,
FAILURE_GYRO_INIT_FAILED
} failureMode_e;

7
src/main/flight/mixer.h
View file

@ -194,16 +194,19 @@ void filterServos(void);
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
struct escAndServoConfig_s;
struct rxConfig_s;
void mixerUseConfigs(
#ifdef USE_SERVOS
servoParam_t *servoConfToUse,
struct gimbalConfig_s *gimbalConfigToUse,
#endif
flight3DConfig_t *flight3DConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
struct escAndServoConfig_s *escAndServoConfigToUse,
mixerConfig_t *mixerConfigToUse,
airplaneConfig_t *airplaneConfigToUse,
struct rxConfig_s *rxConfigToUse);
struct rxConfig_s *rxConfigToUse);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);

2
src/main/io/serial_cli.c
View file

@ -192,7 +192,7 @@ static const char * const sensorTypeNames[] = {
static const char * const sensorHardwareNames[4][11] = {
{ "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL },
{ "", "None", "BMP085", "MS5611", NULL },
{ "", "None", "BMP085", "MS5611", "BMP280", NULL },
{ "", "None", "HMC5883", "AK8975", NULL }
};
#endif

4
src/main/sensors/acceleration.h
View file

@ -26,8 +26,8 @@ typedef enum {
ACC_MMA8452 = 4,
ACC_BMA280 = 5,
ACC_LSM303DLHC = 6,
ACC_SPI_MPU6000 = 7,
ACC_SPI_MPU6500 = 8,
ACC_MPU6000 = 7,
ACC_MPU6500 = 8,
ACC_FAKE = 9,
} accelerationSensor_e;

3
src/main/sensors/barometer.h
View file

@ -21,7 +21,8 @@ typedef enum {
BARO_DEFAULT = 0,
BARO_NONE = 1,
BARO_BMP085 = 2,
BARO_MS5611 = 3
BARO_MS5611 = 3,
BARO_BMP280 = 4
} baroSensor_e;
#define BARO_SAMPLE_COUNT_MAX 48

4
src/main/sensors/gyro.h
View file

@ -24,8 +24,8 @@ typedef enum {
GYRO_L3G4200D,
GYRO_MPU3050,
GYRO_L3GD20,
GYRO_SPI_MPU6000,
GYRO_SPI_MPU6500,
GYRO_MPU6000,
GYRO_MPU6500,
GYRO_FAKE
} gyroSensor_e;

142
src/main/sensors/initialisation.c
View file

@ -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,8 +35,10 @@
#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_mpu6500.h"
#include "drivers/accgyro_l3gd20.h"
#include "drivers/accgyro_lsm303dlhc.h"
@ -42,6 +48,7 @@
#include "drivers/barometer.h"
#include "drivers/barometer_bmp085.h"
#include "drivers/barometer_bmp280.h"
#include "drivers/barometer_ms5611.h"
#include "drivers/compass.h"
@ -50,9 +57,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 +80,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 +94,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 +105,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 = {
#if defined(SPRACINGF3)
static const extiConfig_t spRacingF3MPUIntExtiConfig = {
.gpioAHBPeripherals = RCC_AHBPeriph_GPIOC,
.gpioPort = GPIOC,
.gpioPin = Pin_13,
@ -117,26 +121,32 @@ const mpu6050Config_t *selectMPU6050Config(void)
.exti_line = EXTI_Line13,
.exti_irqn = EXTI15_10_IRQn
};
return &spRacingF3MPU6050Config;
return &spRacingF3MPUIntExtiConfig;
#endif
return NULL;
}
#ifdef USE_FAKE_GYRO
static void fakeGyroInit(void) {}
static bool fakeGyroRead(int16_t *gyroADC) {
static void fakeGyroInit(uint16_t lpf)
{
UNUSED(lpf);
}
static bool fakeGyroRead(int16_t *gyroADC)
{
memset(gyroADC, 0, sizeof(int16_t[XYZ_AXIS_COUNT]));
return true;
}
static bool fakeGyroReadTemp(int16_t *tempData) {
static bool fakeGyroReadTemp(int16_t *tempData)
{
UNUSED(tempData);
return true;
}
bool fakeGyroDetect(gyro_t *gyro, uint16_t lpf)
bool fakeGyroDetect(gyro_t *gyro)
{
UNUSED(lpf);
gyro->init = fakeGyroInit;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
@ -160,7 +170,7 @@ bool fakeAccDetect(acc_t *acc)
}
#endif
bool detectGyro(uint16_t gyroLpf)
bool detectGyro(void)
{
gyroSensor_e gyroHardware = GYRO_DEFAULT;
@ -171,7 +181,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough
case GYRO_MPU6050:
#ifdef USE_GYRO_MPU6050
if (mpu6050GyroDetect(selectMPU6050Config(), &gyro, gyroLpf)) {
if (mpu6050GyroDetect(&gyro)) {
#ifdef GYRO_MPU6050_ALIGN
gyroHardware = GYRO_MPU6050;
gyroAlign = GYRO_MPU6050_ALIGN;
@ -182,7 +192,7 @@ bool detectGyro(uint16_t gyroLpf)
; // fallthrough
case GYRO_L3G4200D:
#ifdef USE_GYRO_L3G4200D
if (l3g4200dDetect(&gyro, gyroLpf)) {
if (l3g4200dDetect(&gyro)) {
#ifdef GYRO_L3G4200D_ALIGN
gyroHardware = GYRO_L3G4200D;
gyroAlign = GYRO_L3G4200D_ALIGN;
@ -194,7 +204,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_MPU3050:
#ifdef USE_GYRO_MPU3050
if (mpu3050Detect(&gyro, gyroLpf)) {
if (mpu3050Detect(&gyro)) {
#ifdef GYRO_MPU3050_ALIGN
gyroHardware = GYRO_MPU3050;
gyroAlign = GYRO_MPU3050_ALIGN;
@ -206,7 +216,7 @@ bool detectGyro(uint16_t gyroLpf)
case GYRO_L3GD20:
#ifdef USE_GYRO_L3GD20
if (l3gd20Detect(&gyro, gyroLpf)) {
if (l3gd20Detect(&gyro)) {
#ifdef GYRO_L3GD20_ALIGN
gyroHardware = GYRO_L3GD20;
gyroAlign = GYRO_L3GD20_ALIGN;
@ -216,46 +226,39 @@ bool detectGyro(uint16_t gyroLpf)
#endif
; // fallthrough
case GYRO_SPI_MPU6000:
case GYRO_MPU6000:
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6000_ALIGN
gyroHardware = GYRO_SPI_MPU6000;
gyroAlign = GYRO_SPI_MPU6000_ALIGN;
if (mpu6000SpiGyroDetect(&gyro)) {
#ifdef GYRO_MPU6000_ALIGN
gyroHardware = GYRO_MPU6000;
gyroAlign = GYRO_MPU6000_ALIGN;
#endif
break;
}
#endif
; // fallthrough
case GYRO_SPI_MPU6500:
case GYRO_MPU6500:
#ifdef USE_GYRO_MPU6500
#ifdef USE_GYRO_SPI_MPU6500
#ifdef USE_HARDWARE_REVISION_DETECTION
spiBusInit();
#endif
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
#endif
break;
}
if (mpu6500GyroDetect(&gyro) || mpu6500SpiGyroDetect(&gyro))
#else
if (mpu6500SpiGyroDetect(&gyro, gyroLpf)) {
#ifdef GYRO_SPI_MPU6500_ALIGN
gyroHardware = GYRO_SPI_MPU6500;
gyroAlign = GYRO_SPI_MPU6500_ALIGN;
if (mpu6500GyroDetect(&gyro))
#endif
{
gyroHardware = GYRO_MPU6500;
#ifdef GYRO_MPU6500_ALIGN
gyroAlign = GYRO_MPU6500_ALIGN;
#endif
break;
}
#endif
#endif
; // fallthrough
case GYRO_FAKE:
#ifdef USE_FAKE_GYRO
if (fakeGyroDetect(&gyro, gyroLpf)) {
if (fakeGyroDetect(&gyro)) {
gyroHardware = GYRO_FAKE;
break;
}
@ -279,7 +282,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 +322,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
@ -355,28 +358,29 @@ retry:
}
#endif
; // fallthrough
case ACC_SPI_MPU6000:
case ACC_MPU6000:
#ifdef USE_ACC_SPI_MPU6000
if (mpu6000SpiAccDetect(&acc)) {
#ifdef ACC_SPI_MPU6000_ALIGN
accAlign = ACC_SPI_MPU6000_ALIGN;
#ifdef ACC_MPU6000_ALIGN
accAlign = ACC_MPU6000_ALIGN;
#endif
accHardware = ACC_SPI_MPU6000;
accHardware = ACC_MPU6000;
break;
}
#endif
; // fallthrough
case ACC_SPI_MPU6500:
case ACC_MPU6500:
#ifdef USE_ACC_MPU6500
#ifdef USE_ACC_SPI_MPU6500
#ifdef NAZE
if (hardwareRevision == NAZE32_SP && mpu6500SpiAccDetect(&acc)) {
if (mpu6500AccDetect(&acc) || mpu6500SpiAccDetect(&acc))
#else
if (mpu6500SpiAccDetect(&acc)) {
if (mpu6500AccDetect(&acc))
#endif
#ifdef ACC_SPI_MPU6500_ALIGN
accAlign = ACC_SPI_MPU6500_ALIGN;
{
#ifdef ACC_MPU6500_ALIGN
accAlign = ACC_MPU6500_ALIGN;
#endif
accHardware = ACC_SPI_MPU6500;
accHardware = ACC_MPU6500;
break;
}
#endif
@ -413,7 +417,9 @@ retry:
static void detectBaro(baroSensor_e baroHardwareToUse)
{
#ifdef BARO
#ifndef BARO
UNUSED(baroHardwareToUse);
#else
// Detect what pressure sensors are available. baro->update() is set to sensor-specific update function
baroSensor_e baroHardware = baroHardwareToUse;
@ -459,6 +465,14 @@ static void detectBaro(baroSensor_e baroHardwareToUse)
baroHardware = BARO_BMP085;
break;
}
#endif
; // fallthough
case BARO_BMP280:
#ifdef USE_BARO_BMP280
if (bmp280Detect(&baro)) {
baroHardware = BARO_BMP280;
break;
}
#endif
case BARO_NONE:
baroHardware = BARO_NONE;
@ -597,7 +611,15 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
if (!detectGyro(gyroLpf)) {
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
detectAcc(accHardwareToUse);
@ -608,7 +630,7 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init();
gyro.init(gyroLpf);
detectMag(magHardwareToUse);

4
src/main/target/CC3D/target.h
View file

@ -47,12 +47,12 @@
#define GYRO
#define USE_GYRO_SPI_MPU6000
#define GYRO_SPI_MPU6000_ALIGN CW270_DEG
#define GYRO_MPU6000_ALIGN CW270_DEG
#define ACC
#define USE_ACC_SPI_MPU6000
#define ACC_SPI_MPU6000_ALIGN CW270_DEG
#define ACC_MPU6000_ALIGN CW270_DEG
// External I2C BARO
#define BARO

4
src/main/target/CJMCU/hardware_revision.c
View file

@ -51,7 +51,3 @@ void detectHardwareRevision(void)
void updateHardwareRevision(void)
{
}
void spiBusInit(void)
{
}

4
src/main/target/COLIBRI_RACE/target.h
View file

@ -57,11 +57,11 @@
#define GYRO
#define USE_GYRO_SPI_MPU6500
#define GYRO_SPI_MPU6500_ALIGN CW270_DEG
#define GYRO_MPU6500_ALIGN CW270_DEG
#define ACC
#define USE_ACC_SPI_MPU6500
#define ACC_SPI_MPU6500_ALIGN CW270_DEG
#define ACC_MPU6500_ALIGN CW270_DEG
#define BARO
#define USE_BARO_MS5611

6
src/main/target/NAZE/hardware_revision.c
View file

@ -27,7 +27,7 @@
#include "drivers/bus_spi.h"
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/accgyro_spi_mpu6500.h"
#include "drivers/accgyro_mpu6500.h"
#include "hardware_revision.h"
@ -101,7 +101,3 @@ void updateHardwareRevision(void)
hardwareRevision = NAZE32_SP;
#endif
}
void spiBusInit(void)
{
}

7
src/main/target/NAZE/target.h
View file

@ -84,29 +84,32 @@
#define GYRO
#define USE_GYRO_MPU3050
#define USE_GYRO_MPU6050
#define USE_GYRO_MPU6500
#define USE_GYRO_SPI_MPU6500
#define GYRO_MPU3050_ALIGN CW0_DEG
#define GYRO_MPU6050_ALIGN CW0_DEG
#define GYRO_SPI_MPU6500_ALIGN CW0_DEG
#define GYRO_MPU6500_ALIGN CW0_DEG
#define ACC
#define USE_ACC_ADXL345
#define USE_ACC_BMA280
#define USE_ACC_MMA8452
#define USE_ACC_MPU6050
#define USE_ACC_MPU6500
#define USE_ACC_SPI_MPU6500
#define ACC_ADXL345_ALIGN CW270_DEG
#define ACC_MPU6050_ALIGN CW0_DEG
#define ACC_MMA8452_ALIGN CW90_DEG
#define ACC_BMA280_ALIGN CW0_DEG
#define ACC_SPI_MPU6500_ALIGN CW0_DEG
#define ACC_MPU6500_ALIGN CW0_DEG
#define BARO
#define USE_BARO_MS5611
#define USE_BARO_BMP085
#define USE_BARO_BMP280
#define MAG
#define USE_MAG_HMC5883