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

Made gyro device code reentrant

This commit is contained in:
Martin Budden 2016-12-18 00:16:23 +00:00
parent a562b8fa12
commit 42344a8fe6
14 changed files with 131 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,6 +20,7 @@
#include "common/axis.h"
#include "drivers/exti.h"
#include "drivers/sensor.h"
#include "drivers/accgyro_mpu.h"
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
@ -45,6 +46,9 @@ typedef struct gyroDev_s {
uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
sensor_align_e gyroAlign;
const extiConfig_t *mpuIntExtiConfig;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} gyroDev_t;
typedef struct accDev_s {
@ -54,4 +58,6 @@ typedef struct accDev_s {
int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known
sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} accDev_t;

111
src/main/drivers/accgyro_mpu.c
View File

@ -25,7 +25,6 @@
#include "build/build_config.h"
#include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
@ -49,24 +48,21 @@
//#define DEBUG_MPU_DATA_READY_INTERRUPT
mpuResetFuncPtr mpuReset;
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);
static void mpu6050FindRevision(gyroDev_t *gyro);
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void);
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#endif
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
#endif
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_ADDRESS 0x68
// WHO_AM_I register contents for MPU3050, 6050 and 6500
@ -75,13 +71,8 @@ static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse)
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{
memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
mpuIntExtiConfig = configToUse;
bool ack;
uint8_t sig;
uint8_t inquiryResult;
@ -96,93 +87,94 @@ mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse)
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif
if (ack) {
mpuConfiguration.read = mpuReadRegisterI2C;
mpuConfiguration.write = mpuWriteRegisterI2C;
gyro->mpuConfiguration.read = mpuReadRegisterI2C;
gyro->mpuConfiguration.write = mpuWriteRegisterI2C;
} else {
#ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult();
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor);
#endif
return &mpuDetectionResult;
return &gyro->mpuDetectionResult;
}
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->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;
gyro->mpuDetectionResult.sensor = MPU_3050;
gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &gyro->mpuDetectionResult;
}
sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_60x0;
gyro->mpuDetectionResult.sensor = MPU_60x0;
mpu6050FindRevision();
mpu6050FindRevision(gyro);
} else if (sig == MPU6500_WHO_AM_I_CONST) {
mpuDetectionResult.sensor = MPU_65xx_I2C;
gyro->mpuDetectionResult.sensor = MPU_65xx_I2C;
}
return &mpuDetectionResult;
return &gyro->mpuDetectionResult;
}
#ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void)
static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{
#ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) {
mpuDetectionResult.sensor = MPU_65xx_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister;
gyro->mpuDetectionResult.sensor = MPU_65xx_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6500ReadRegister;
gyro->mpuConfiguration.write = mpu6500WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) {
mpuDetectionResult.sensor = ICM_20689_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = icm20689ReadRegister;
mpuConfiguration.write = icm20689WriteRegister;
gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = icm20689ReadRegister;
gyro->mpuConfiguration.write = icm20689WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) {
mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister;
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6000ReadRegister;
gyro->mpuConfiguration.write = mpu6000WriteRegister;
return true;
}
#endif
#ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) {
mpuDetectionResult.sensor = MPU_9250_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu9250ReadRegister;
mpuConfiguration.slowread = mpu9250SlowReadRegister;
mpuConfiguration.verifywrite = verifympu9250WriteRegister;
mpuConfiguration.write = mpu9250WriteRegister;
mpuConfiguration.reset = mpu9250ResetGyro;
gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu9250ReadRegister;
gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
gyro->mpuConfiguration.write = mpu9250WriteRegister;
gyro->mpuConfiguration.reset = mpu9250ResetGyro;
return true;
}
#endif
UNUSED(gyro);
return false;
}
#endif
static void mpu6050FindRevision(void)
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool ack;
UNUSED(ack);
@ -195,28 +187,28 @@ static void mpu6050FindRevision(void)
// 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);
ack = gyro->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;
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else {
failureMode(FAILURE_ACC_INCOMPATIBLE);
}
} else {
ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
ack = gyro->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;
gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
}
}
}
@ -243,13 +235,11 @@ static void mpuIntExtiHandler(extiCallbackRec_t *cb)
static void mpuIntExtiInit(gyroDev_t *gyro)
{
#if defined(MPU_INT_EXTI)
static bool mpuExtiInitDone = false;
if (mpuExtiInitDone || !mpuIntExtiConfig) {
if (!gyro->mpuIntExtiConfig) {
return;
}
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag);
IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = IORead(mpuIntIO);
@ -272,7 +262,6 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
EXTIEnable(mpuIntIO, true);
#endif
mpuExtiInitDone = true;
#else
UNUSED(gyro);
#endif
@ -294,7 +283,7 @@ bool mpuAccRead(accDev_t *acc)
{
uint8_t data[6];
bool ack = mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) {
return false;
}
@ -310,7 +299,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{
uint8_t data[6];
const bool ack = mpuConfiguration.read(mpuConfiguration.gyroReadXRegister, 6, data);
const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) {
return false;
}

9
src/main/drivers/accgyro_mpu.h
View File

@ -121,6 +121,8 @@ typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void);
extern mpuResetFuncPtr mpuReset;
typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read;
@ -130,8 +132,6 @@ typedef struct mpuConfiguration_s {
mpuResetFuncPtr reset;
} mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration;
enum gyro_fsr_e {
INV_FSR_250DPS = 0,
INV_FSR_500DPS,
@ -181,13 +181,10 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
void mpuConfigureDataReadyInterruptHandling(void);
struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro);
struct accDev_s;
bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro);
mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse);
mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro);

14
src/main/drivers/accgyro_mpu3050.c
View File

@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro)
delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe
ack = mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0);
if (!ack)
failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
}
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
UNUSED(gyro);
uint8_t buf[2];
if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
return false;
}
@ -78,7 +78,7 @@ static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
bool mpu3050Detect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_3050) {
if (gyro->mpuDetectionResult.sensor != MPU_3050) {
return false;
}
gyro->init = mpu3050Init;

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

@ -52,7 +52,7 @@
static void mpu6050AccInit(accDev_t *acc)
{
switch (mpuDetectionResult.resolution) {
switch (acc->mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 4;
break;
@ -64,13 +64,13 @@ static void mpu6050AccInit(accDev_t *acc)
bool mpu6050AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
if (acc->mpuDetectionResult.sensor != MPU_60x0) {
return false;
}
acc->init = mpu6050AccInit;
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.
acc->revisionCode = (acc->mpuDetectionResult.resolution == MPU_HALF_RESOLUTION ? 'o' : 'n'); // es/non-es variance between MPU6050 sensors, half of the naze boards are mpu6000ES.
return true;
}
@ -79,29 +79,29 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100);
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)
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
gyro->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)
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
delay(15); //PLL Settling time when changing CLKSEL is max 10ms. Use 15ms to be sure
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz)
gyro->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.
// Accel scale 8g (4096 LSB/g)
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
mpuConfiguration.write(MPU_RA_INT_PIN_CFG,
gyro->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
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif
}
bool mpu6050GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0) {
if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
return false;
}
gyro->init = mpu6050GyroInit;

26
src/main/drivers/accgyro_mpu6500.c
View File

@ -41,7 +41,7 @@ void mpu6500AccInit(accDev_t *acc)
bool mpu6500AccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
if (acc->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}
@ -55,40 +55,40 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{
mpuGyroInit(gyro);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
#ifdef USE_MPU9250_MAG
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR
#endif
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif
delay(15);
}
bool mpu6500GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false;
}

26
src/main/drivers/accgyro_spi_icm20689.c
View File

@ -114,7 +114,7 @@ void icm20689AccInit(accDev_t *acc)
bool icm20689SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
if (acc->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
}
@ -130,31 +130,31 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100);
// mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15);
mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3);
delay(15);
mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15);
mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider Drops
delay(100);
// Data ready interrupt configuration
// mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
// gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL
mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable
#endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
@ -162,7 +162,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != ICM_20689_SPI) {
if (gyro->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false;
}

4
src/main/drivers/accgyro_spi_mpu6000.c
View File

@ -256,7 +256,7 @@ static void mpu6000AccAndGyroInit(void)
bool mpu6000SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}
@ -268,7 +268,7 @@ bool mpu6000SpiAccDetect(accDev_t *acc)
bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_60x0_SPI) {
if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false;
}

4
src/main/drivers/accgyro_spi_mpu6500.c
View File

@ -116,7 +116,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
bool mpu6500SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
}
@ -128,7 +128,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
if (gyro->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
}

4
src/main/drivers/accgyro_spi_mpu9250.c
View File

@ -211,7 +211,7 @@ bool mpu9250SpiDetect(void)
bool mpu9250SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
if (acc->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
}
@ -223,7 +223,7 @@ bool mpu9250SpiAccDetect(accDev_t *acc)
bool mpu9250SpiGyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_9250_SPI) {
if (gyro->mpuDetectionResult.sensor != MPU_9250_SPI) {
return false;
}

10
src/main/drivers/system_stm32f4xx.c
View File

@ -31,8 +31,9 @@ void SetSysClock(void);
void systemReset(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
__disable_irq();
NVIC_SystemReset();
@ -40,8 +41,9 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX

11
src/main/drivers/system_stm32f7xx.c
View File

@ -33,8 +33,9 @@ void SystemClock_Config(void);
void systemReset(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
__disable_irq();
NVIC_SystemReset();
@ -42,9 +43,9 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot

6
src/main/sensors/acceleration.c
View File

@ -51,9 +51,10 @@
#include "io/beeper.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "config/feature.h"
@ -230,6 +231,9 @@ retry:
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval)
{
memset(&acc, 0, sizeof(acc));
// copy over the common gyro mpu settings
acc.dev.mpuConfiguration = gyro.dev.mpuConfiguration;
acc.dev.mpuDetectionResult = gyro.dev.mpuDetectionResult;
if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
return false;
}

5
src/main/sensors/gyro.c
View File

@ -231,8 +231,9 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
gyroConfig = gyroConfigToUse;
memset(&gyro, 0, sizeof(gyro));
#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) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetect(extiConfig);
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif
if (!gyroDetect(&gyro.dev)) {