Relocate and use some of the common MPU code from MPU6500 into
accgyro_mpu.c.
This commit is contained in:
parent
da46d9f1d2
commit
0361d161fb
1
Makefile
1
Makefile
|
@ -520,6 +520,7 @@ CHEBUZZF3_SRC = \
|
||||||
COLIBRI_RACE_SRC = \
|
COLIBRI_RACE_SRC = \
|
||||||
$(STM32F30x_COMMON_SRC) \
|
$(STM32F30x_COMMON_SRC) \
|
||||||
drivers/display_ug2864hsweg01.c \
|
drivers/display_ug2864hsweg01.c \
|
||||||
|
drivers/accgyro_mpu.c \
|
||||||
drivers/accgyro_spi_mpu6500.c \
|
drivers/accgyro_spi_mpu6500.c \
|
||||||
drivers/barometer_ms5611.c \
|
drivers/barometer_ms5611.c \
|
||||||
drivers/compass_ak8975.c \
|
drivers/compass_ak8975.c \
|
||||||
|
|
|
@ -37,25 +37,29 @@
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
#include "accgyro_mpu3050.h"
|
#include "accgyro_mpu3050.h"
|
||||||
#include "accgyro_mpu6050.h"
|
#include "accgyro_mpu6050.h"
|
||||||
|
#include "accgyro_spi_mpu6500.h"
|
||||||
#include "accgyro_mpu.h"
|
#include "accgyro_mpu.h"
|
||||||
|
|
||||||
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
//#define DEBUG_MPU_DATA_READY_INTERRUPT
|
||||||
|
|
||||||
|
|
||||||
|
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
|
||||||
|
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data);
|
||||||
|
|
||||||
uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
|
static void mpu6050FindRevision(void);
|
||||||
|
|
||||||
void mpu6050FindRevision(void);
|
#ifdef USE_SPI
|
||||||
|
static bool detectSPISensorsAndUpdateDetectionResult(void);
|
||||||
|
#endif
|
||||||
|
|
||||||
mpuDetectionResult_t mpuDetectionResult;
|
mpuDetectionResult_t mpuDetectionResult;
|
||||||
|
|
||||||
typedef struct mpuConfiguration_s {
|
mpuConfiguration_t mpuConfiguration;
|
||||||
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
|
|
||||||
} mpuConfiguration_t;
|
|
||||||
|
|
||||||
static mpuConfiguration_t mpuConfiguration;
|
|
||||||
static const extiConfig_t *mpuIntExtiConfig = NULL;
|
static const extiConfig_t *mpuIntExtiConfig = NULL;
|
||||||
|
|
||||||
|
// FIXME move into mpuConfiguration
|
||||||
|
uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
|
||||||
|
|
||||||
|
|
||||||
#define MPU_ADDRESS 0x68
|
#define MPU_ADDRESS 0x68
|
||||||
|
|
||||||
|
@ -87,14 +91,23 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
|
||||||
// MPU datasheet specifies 30ms.
|
// MPU datasheet specifies 30ms.
|
||||||
delay(35);
|
delay(35);
|
||||||
|
|
||||||
ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I, 1, &sig);
|
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
|
||||||
if (!ack)
|
if (ack) {
|
||||||
|
mpuConfiguration.read = mpuReadRegisterI2C;
|
||||||
|
mpuConfiguration.write = mpuWriteRegisterI2C;
|
||||||
|
} else {
|
||||||
|
#ifdef USE_SPI
|
||||||
|
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult();
|
||||||
|
UNUSED(detectedSpiSensor);
|
||||||
|
#endif
|
||||||
|
|
||||||
return &mpuDetectionResult;
|
return &mpuDetectionResult;
|
||||||
|
}
|
||||||
|
|
||||||
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
|
||||||
|
|
||||||
// If an MPU3050 is connected sig will contain 0.
|
// If an MPU3050 is connected sig will contain 0.
|
||||||
ack = i2cRead(MPU_ADDRESS, MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
|
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult);
|
||||||
inquiryResult &= MPU_INQUIRY_MASK;
|
inquiryResult &= MPU_INQUIRY_MASK;
|
||||||
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) {
|
||||||
mpuDetectionResult.sensor = MPU_3050;
|
mpuDetectionResult.sensor = MPU_3050;
|
||||||
|
@ -114,8 +127,26 @@ mpuDetectionResult_t *detectMpu(const extiConfig_t *configToUse)
|
||||||
return &mpuDetectionResult;
|
return &mpuDetectionResult;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef USE_SPI
|
||||||
|
static bool detectSPISensorsAndUpdateDetectionResult(void)
|
||||||
|
{
|
||||||
|
bool found = false;
|
||||||
|
|
||||||
void mpu6050FindRevision(void)
|
#ifdef USE_GYRO_SPI_MPU6500
|
||||||
|
found = mpu6500Detect();
|
||||||
|
if (found) {
|
||||||
|
mpuDetectionResult.sensor = MPU_65xx_SPI;
|
||||||
|
mpuConfiguration.gyroReadXRegister = MPU6500_RA_GYRO_XOUT_H;
|
||||||
|
mpuConfiguration.read = mpu6500ReadRegister;
|
||||||
|
mpuConfiguration.write = mpu6500WriteRegister;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
return found;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
static void mpu6050FindRevision(void)
|
||||||
{
|
{
|
||||||
bool ack;
|
bool ack;
|
||||||
UNUSED(ack);
|
UNUSED(ack);
|
||||||
|
@ -276,34 +307,46 @@ void configureMPULPF(uint16_t lpf)
|
||||||
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
|
mpuLowPassFilter = INV_FILTER_256HZ_NOLPF2;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data)
|
||||||
|
{
|
||||||
|
bool ack = i2cRead(MPU_ADDRESS, reg, length, data);
|
||||||
|
return ack;
|
||||||
|
}
|
||||||
|
|
||||||
|
static bool mpuWriteRegisterI2C(uint8_t reg, uint8_t data)
|
||||||
|
{
|
||||||
|
bool ack = i2cWrite(MPU_ADDRESS, reg, data);
|
||||||
|
return ack;
|
||||||
|
}
|
||||||
|
|
||||||
bool mpuAccRead(int16_t *accData)
|
bool mpuAccRead(int16_t *accData)
|
||||||
{
|
{
|
||||||
uint8_t buf[6];
|
uint8_t data[6];
|
||||||
|
|
||||||
bool ack = i2cRead(MPU_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
|
bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
accData[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
accData[0] = (int16_t)((data[0] << 8) | data[1]);
|
||||||
accData[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
accData[1] = (int16_t)((data[2] << 8) | data[3]);
|
||||||
accData[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
accData[2] = (int16_t)((data[4] << 8) | data[5]);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpuGyroRead(int16_t *gyroADC)
|
bool mpuGyroRead(int16_t *gyroADC)
|
||||||
{
|
{
|
||||||
uint8_t buf[6];
|
uint8_t data[6];
|
||||||
|
|
||||||
bool ack = i2cRead(MPU_ADDRESS, mpuConfiguration.gyroReadXRegister, 6, buf);
|
bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
|
||||||
if (!ack) {
|
if (!ack) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
gyroADC[0] = (int16_t)((buf[0] << 8) | buf[1]);
|
gyroADC[0] = (int16_t)((data[0] << 8) | data[1]);
|
||||||
gyroADC[1] = (int16_t)((buf[2] << 8) | buf[3]);
|
gyroADC[1] = (int16_t)((data[2] << 8) | data[3]);
|
||||||
gyroADC[2] = (int16_t)((buf[4] << 8) | buf[5]);
|
gyroADC[2] = (int16_t)((data[4] << 8) | data[5]);
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,6 +17,16 @@
|
||||||
|
|
||||||
#pragma once
|
#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 {
|
enum lpf_e {
|
||||||
INV_FILTER_256HZ_NOLPF2 = 0,
|
INV_FILTER_256HZ_NOLPF2 = 0,
|
||||||
|
|
|
@ -75,20 +75,20 @@ void mpu3050Init(void)
|
||||||
|
|
||||||
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
|
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)
|
if (!ack)
|
||||||
failureMode(FAILURE_ACC_INIT);
|
failureMode(FAILURE_ACC_INIT);
|
||||||
|
|
||||||
i2cWrite(MPU3050_ADDRESS, MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
|
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | mpuLowPassFilter);
|
||||||
i2cWrite(MPU3050_ADDRESS, MPU3050_INT_CFG, 0);
|
mpuConfiguration.write(MPU3050_INT_CFG, 0);
|
||||||
i2cWrite(MPU3050_ADDRESS, MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
|
||||||
i2cWrite(MPU3050_ADDRESS, MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpu3050ReadTemp(int16_t *tempData)
|
static bool mpu3050ReadTemp(int16_t *tempData)
|
||||||
{
|
{
|
||||||
uint8_t buf[2];
|
uint8_t buf[2];
|
||||||
if (!i2cRead(MPU3050_ADDRESS, MPU3050_TEMP_OUT, 2, buf)) {
|
if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -196,23 +196,23 @@ static void mpu6050GyroInit(void)
|
||||||
mpuIntExtiInit();
|
mpuIntExtiInit();
|
||||||
|
|
||||||
bool ack;
|
bool ack;
|
||||||
ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
|
||||||
delay(100);
|
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 = mpuConfiguration.write(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_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
|
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 = 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 = 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_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. Moved into gyro init because the reset above would screw up accel config. Oops.
|
||||||
// Accel scale 8g (4096 LSB/g)
|
// 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
|
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
|
#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
|
#endif
|
||||||
UNUSED(ack);
|
UNUSED(ack);
|
||||||
}
|
}
|
||||||
|
|
|
@ -25,73 +25,45 @@
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
|
#include "exti.h"
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
#include "bus_spi.h"
|
#include "bus_spi.h"
|
||||||
|
|
||||||
#include "sensor.h"
|
#include "sensor.h"
|
||||||
#include "accgyro.h"
|
#include "accgyro.h"
|
||||||
|
#include "accgyro_mpu.h"
|
||||||
#include "accgyro_spi_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 DISABLE_MPU6500 GPIO_SetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
|
||||||
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
|
#define ENABLE_MPU6500 GPIO_ResetBits(MPU6500_CS_GPIO, MPU6500_CS_PIN)
|
||||||
|
|
||||||
static uint8_t mpuLowPassFilter = INV_FILTER_42HZ;
|
extern mpuDetectionResult_t mpuDetectionResult;
|
||||||
|
extern uint8_t mpuLowPassFilter;
|
||||||
|
|
||||||
|
|
||||||
static void mpu6500AccInit(void);
|
static void mpu6500AccInit(void);
|
||||||
static bool mpu6500AccRead(int16_t *accData);
|
|
||||||
static void mpu6500GyroInit(void);
|
static void mpu6500GyroInit(void);
|
||||||
static bool mpu6500GyroRead(int16_t *gyroADC);
|
|
||||||
|
|
||||||
extern uint16_t acc_1G;
|
extern uint16_t acc_1G;
|
||||||
|
|
||||||
static void mpu6500WriteRegister(uint8_t reg, uint8_t data)
|
bool mpu6500WriteRegister(uint8_t reg, uint8_t data)
|
||||||
{
|
{
|
||||||
ENABLE_MPU6500;
|
ENABLE_MPU6500;
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
|
spiTransferByte(MPU6500_SPI_INSTANCE, reg);
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, data);
|
spiTransferByte(MPU6500_SPI_INSTANCE, data);
|
||||||
DISABLE_MPU6500;
|
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;
|
ENABLE_MPU6500;
|
||||||
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
|
spiTransferByte(MPU6500_SPI_INSTANCE, reg | 0x80); // read transaction
|
||||||
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
|
spiTransfer(MPU6500_SPI_INSTANCE, data, NULL, length);
|
||||||
DISABLE_MPU6500;
|
DISABLE_MPU6500;
|
||||||
|
|
||||||
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void mpu6500SpiInit(void)
|
static void mpu6500SpiInit(void)
|
||||||
|
@ -133,13 +105,13 @@ static void mpu6500SpiInit(void)
|
||||||
hardwareInitialised = true;
|
hardwareInitialised = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
static bool mpu6500Detect(void)
|
bool mpu6500Detect(void)
|
||||||
{
|
{
|
||||||
uint8_t tmp;
|
uint8_t tmp;
|
||||||
|
|
||||||
mpu6500SpiInit();
|
mpu6500SpiInit();
|
||||||
|
|
||||||
mpu6500ReadRegister(MPU6500_RA_WHOAMI, &tmp, 1);
|
mpu6500ReadRegister(MPU6500_RA_WHOAMI, 1, &tmp);
|
||||||
|
|
||||||
if (tmp != MPU6500_WHO_AM_I_CONST)
|
if (tmp != MPU6500_WHO_AM_I_CONST)
|
||||||
return false;
|
return false;
|
||||||
|
@ -149,42 +121,29 @@ static bool mpu6500Detect(void)
|
||||||
|
|
||||||
bool mpu6500SpiAccDetect(acc_t *acc)
|
bool mpu6500SpiAccDetect(acc_t *acc)
|
||||||
{
|
{
|
||||||
if (!mpu6500Detect()) {
|
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
acc->init = mpu6500AccInit;
|
acc->init = mpu6500AccInit;
|
||||||
acc->read = mpu6500AccRead;
|
acc->read = mpuAccRead;
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
|
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
|
||||||
{
|
{
|
||||||
if (!mpu6500Detect()) {
|
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
gyro->init = mpu6500GyroInit;
|
gyro->init = mpu6500GyroInit;
|
||||||
gyro->read = mpu6500GyroRead;
|
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;
|
||||||
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
|
|
||||||
|
|
||||||
// default lpf is 42Hz
|
configureMPULPF(lpf);
|
||||||
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;
|
return true;
|
||||||
}
|
}
|
||||||
|
@ -194,19 +153,6 @@ static void mpu6500AccInit(void)
|
||||||
acc_1G = 512 * 8;
|
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)
|
static void mpu6500GyroInit(void)
|
||||||
{
|
{
|
||||||
#ifdef NAZE
|
#ifdef NAZE
|
||||||
|
@ -222,6 +168,8 @@ static void mpu6500GyroInit(void)
|
||||||
|
|
||||||
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
|
||||||
delay(100);
|
delay(100);
|
||||||
|
mpu6500WriteRegister(MPU6500_RA_SIGNAL_PATH_RST, 0x07);
|
||||||
|
delay(100);
|
||||||
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
|
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, 0);
|
||||||
delay(100);
|
delay(100);
|
||||||
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
|
mpu6500WriteRegister(MPU6500_RA_PWR_MGMT_1, INV_CLK_PLL);
|
||||||
|
@ -230,16 +178,3 @@ static void mpu6500GyroInit(void)
|
||||||
mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
|
mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter);
|
||||||
mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R
|
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;
|
|
||||||
}
|
|
||||||
|
|
|
@ -15,16 +15,17 @@
|
||||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#define MPU6500_RA_WHOAMI (0x75)
|
#define MPU6500_RA_RATE_DIV (0x19)
|
||||||
|
#define MPU6500_RA_LPF (0x1A)
|
||||||
|
#define MPU6500_RA_GYRO_CFG (0x1B)
|
||||||
|
#define MPU6500_RA_ACCEL_CFG (0x1C)
|
||||||
#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
|
#define MPU6500_RA_ACCEL_XOUT_H (0x3B)
|
||||||
#define MPU6500_RA_GYRO_XOUT_H (0x43)
|
#define MPU6500_RA_GYRO_XOUT_H (0x43)
|
||||||
|
#define MPU6500_RA_SIGNAL_PATH_RST (0x68)
|
||||||
|
#define MPU6500_RA_PWR_MGMT_1 (0x6B)
|
||||||
#define MPU6500_RA_BANK_SEL (0x6D)
|
#define MPU6500_RA_BANK_SEL (0x6D)
|
||||||
#define MPU6500_RA_MEM_RW (0x6F)
|
#define MPU6500_RA_MEM_RW (0x6F)
|
||||||
#define MPU6500_RA_GYRO_CFG (0x1B)
|
#define MPU6500_RA_WHOAMI (0x75)
|
||||||
#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_WHO_AM_I_CONST (0x70)
|
||||||
|
|
||||||
|
@ -32,5 +33,10 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
bool mpu6500Detect(void);
|
||||||
|
|
||||||
bool mpu6500SpiAccDetect(acc_t *acc);
|
bool mpu6500SpiAccDetect(acc_t *acc);
|
||||||
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
|
bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf);
|
||||||
|
|
||||||
|
bool mpu6500WriteRegister(uint8_t reg, uint8_t data);
|
||||||
|
bool mpu6500ReadRegister(uint8_t reg, uint8_t length, uint8_t *data);
|
||||||
|
|
|
@ -599,7 +599,7 @@ 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_GYRO_MPU3050) || defined(USE_ACC_MPU6050)
|
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_ACC_MPU6050)
|
||||||
|
|
||||||
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
|
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue