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

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

@ -20,6 +20,7 @@
#include "common/axis.h" #include "common/axis.h"
#include "drivers/exti.h" #include "drivers/exti.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro_mpu.h"
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE #define MPU_I2C_INSTANCE I2C_DEVICE
@ -37,7 +38,7 @@
typedef struct gyroDev_s { typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function sensorGyroReadFuncPtr read; // read 3 axis data function
sensorGyroReadDataFuncPtr temperature; // read temperature if available sensorGyroReadDataFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroInterruptStatusFuncPtr intStatus;
extiCallbackRec_t exti; extiCallbackRec_t exti;
float scale; // scalefactor float scale; // scalefactor
@ -45,6 +46,9 @@ typedef struct gyroDev_s {
uint16_t lpf; uint16_t lpf;
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
sensor_align_e gyroAlign; sensor_align_e gyroAlign;
const extiConfig_t *mpuIntExtiConfig;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} gyroDev_t; } gyroDev_t;
typedef struct accDev_s { typedef struct accDev_s {
@ -54,4 +58,6 @@ typedef struct accDev_s {
int16_t ADCRaw[XYZ_AXIS_COUNT]; int16_t ADCRaw[XYZ_AXIS_COUNT];
char revisionCode; // a revision code for the sensor, if known char revisionCode; // a revision code for the sensor, if known
sensor_align_e accAlign; sensor_align_e accAlign;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} accDev_t; } accDev_t;

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

@ -25,7 +25,6 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h" #include "build/debug.h"
#include "common/filter.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/utils.h" #include "common/utils.h"
@ -49,24 +48,21 @@
//#define DEBUG_MPU_DATA_READY_INTERRUPT //#define DEBUG_MPU_DATA_READY_INTERRUPT
mpuResetFuncPtr mpuReset;
static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data); static bool mpuReadRegisterI2C(uint8_t reg, uint8_t length, uint8_t* data);
static bool mpuWriteRegisterI2C(uint8_t reg, 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 #ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void); static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro);
#endif #endif
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE #define MPU_I2C_INSTANCE I2C_DEVICE
#endif #endif
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
static const extiConfig_t *mpuIntExtiConfig = NULL;
#define MPU_ADDRESS 0x68 #define MPU_ADDRESS 0x68
// WHO_AM_I register contents for MPU3050, 6050 and 6500 // 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 #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; bool ack;
uint8_t sig; uint8_t sig;
uint8_t inquiryResult; uint8_t inquiryResult;
@ -96,93 +87,94 @@ mpuDetectionResult_t *mpuDetect(const extiConfig_t *configToUse)
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif #endif
if (ack) { if (ack) {
mpuConfiguration.read = mpuReadRegisterI2C; gyro->mpuConfiguration.read = mpuReadRegisterI2C;
mpuConfiguration.write = mpuWriteRegisterI2C; gyro->mpuConfiguration.write = mpuWriteRegisterI2C;
} else { } else {
#ifdef USE_SPI #ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(); bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
UNUSED(detectedSpiSensor); UNUSED(detectedSpiSensor);
#endif #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. // If an MPU3050 is connected sig will contain 0.
ack = mpuReadRegisterI2C(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; gyro->mpuDetectionResult.sensor = MPU_3050;
mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT; gyro->mpuConfiguration.gyroReadXRegister = MPU3050_GYRO_OUT;
return &mpuDetectionResult; return &gyro->mpuDetectionResult;
} }
sig &= MPU_INQUIRY_MASK; sig &= MPU_INQUIRY_MASK;
if (sig == MPUx0x0_WHO_AM_I_CONST) { 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) { } 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 #ifdef USE_SPI
static bool detectSPISensorsAndUpdateDetectionResult(void) static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
{ {
#ifdef USE_GYRO_SPI_MPU6500 #ifdef USE_GYRO_SPI_MPU6500
if (mpu6500SpiDetect()) { if (mpu6500SpiDetect()) {
mpuDetectionResult.sensor = MPU_65xx_SPI; gyro->mpuDetectionResult.sensor = MPU_65xx_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6500ReadRegister; gyro->mpuConfiguration.read = mpu6500ReadRegister;
mpuConfiguration.write = mpu6500WriteRegister; gyro->mpuConfiguration.write = mpu6500WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_ICM20689 #ifdef USE_GYRO_SPI_ICM20689
if (icm20689SpiDetect()) { if (icm20689SpiDetect()) {
mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = icm20689ReadRegister; gyro->mpuConfiguration.read = icm20689ReadRegister;
mpuConfiguration.write = icm20689WriteRegister; gyro->mpuConfiguration.write = icm20689WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_MPU6000 #ifdef USE_GYRO_SPI_MPU6000
if (mpu6000SpiDetect()) { if (mpu6000SpiDetect()) {
mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu6000ReadRegister; gyro->mpuConfiguration.read = mpu6000ReadRegister;
mpuConfiguration.write = mpu6000WriteRegister; gyro->mpuConfiguration.write = mpu6000WriteRegister;
return true; return true;
} }
#endif #endif
#ifdef USE_GYRO_SPI_MPU9250 #ifdef USE_GYRO_SPI_MPU9250
if (mpu9250SpiDetect()) { if (mpu9250SpiDetect()) {
mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
mpuConfiguration.read = mpu9250ReadRegister; gyro->mpuConfiguration.read = mpu9250ReadRegister;
mpuConfiguration.slowread = mpu9250SlowReadRegister; gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister;
mpuConfiguration.verifywrite = verifympu9250WriteRegister; gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister;
mpuConfiguration.write = mpu9250WriteRegister; gyro->mpuConfiguration.write = mpu9250WriteRegister;
mpuConfiguration.reset = mpu9250ResetGyro; gyro->mpuConfiguration.reset = mpu9250ResetGyro;
return true; return true;
} }
#endif #endif
UNUSED(gyro);
return false; return false;
} }
#endif #endif
static void mpu6050FindRevision(void) static void mpu6050FindRevision(gyroDev_t *gyro)
{ {
bool ack; bool ack;
UNUSED(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 // See https://android.googlesource.com/kernel/msm.git/+/eaf36994a3992b8f918c18e4f7411e8b2320a35f/drivers/misc/mpu6050/mldl_cfg.c
// determine product ID and accel revision // 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); revision = ((readBuffer[5] & 0x01) << 2) | ((readBuffer[3] & 0x01) << 1) | (readBuffer[1] & 0x01);
if (revision) { if (revision) {
/* Congrats, these parts are better. */ /* Congrats, these parts are better. */
if (revision == 1) { if (revision == 1) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else if (revision == 2) { } else if (revision == 2) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else if ((revision == 3) || (revision == 7)) { } else if ((revision == 3) || (revision == 7)) {
mpuDetectionResult.resolution = MPU_FULL_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_FULL_RESOLUTION;
} else { } else {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} }
} else { } else {
ack = mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F; revision = productId & 0x0F;
if (!revision) { if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} else if (revision == 4) { } else if (revision == 4) {
mpuDetectionResult.resolution = MPU_HALF_RESOLUTION; gyro->mpuDetectionResult.resolution = MPU_HALF_RESOLUTION;
} else { } 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) static void mpuIntExtiInit(gyroDev_t *gyro)
{ {
#if defined(MPU_INT_EXTI) #if defined(MPU_INT_EXTI)
static bool mpuExtiInitDone = false; if (!gyro->mpuIntExtiConfig) {
if (mpuExtiInitDone || !mpuIntExtiConfig) {
return; return;
} }
IO_t mpuIntIO = IOGetByTag(mpuIntExtiConfig->tag); IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiConfig->tag);
#ifdef ENSURE_MPU_DATA_READY_IS_LOW #ifdef ENSURE_MPU_DATA_READY_IS_LOW
uint8_t status = IORead(mpuIntIO); uint8_t status = IORead(mpuIntIO);
@ -272,7 +262,6 @@ static void mpuIntExtiInit(gyroDev_t *gyro)
EXTIEnable(mpuIntIO, true); EXTIEnable(mpuIntIO, true);
#endif #endif
mpuExtiInitDone = true;
#else #else
UNUSED(gyro); UNUSED(gyro);
#endif #endif
@ -294,7 +283,7 @@ bool mpuAccRead(accDev_t *acc)
{ {
uint8_t data[6]; 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) { if (!ack) {
return false; return false;
} }
@ -310,7 +299,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{ {
uint8_t data[6]; 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) { if (!ack) {
return false; 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 bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void); typedef void(*mpuResetFuncPtr)(void);
extern mpuResetFuncPtr mpuReset;
typedef struct mpuConfiguration_s { typedef struct mpuConfiguration_s {
uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each
mpuReadRegisterFunc read; mpuReadRegisterFunc read;
@ -130,8 +132,6 @@ typedef struct mpuConfiguration_s {
mpuResetFuncPtr reset; mpuResetFuncPtr reset;
} mpuConfiguration_t; } mpuConfiguration_t;
extern mpuConfiguration_t mpuConfiguration;
enum gyro_fsr_e { enum gyro_fsr_e {
INV_FSR_250DPS = 0, INV_FSR_250DPS = 0,
INV_FSR_500DPS, INV_FSR_500DPS,
@ -181,13 +181,10 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
void mpuConfigureDataReadyInterruptHandling(void);
struct gyroDev_s; struct gyroDev_s;
void mpuGyroInit(struct gyroDev_s *gyro); void mpuGyroInit(struct gyroDev_s *gyro);
struct accDev_s; struct accDev_s;
bool mpuAccRead(struct accDev_s *acc); bool mpuAccRead(struct accDev_s *acc);
bool mpuGyroRead(struct gyroDev_s *gyro); 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); 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 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) if (!ack)
failureMode(FAILURE_ACC_INIT); failureMode(FAILURE_ACC_INIT);
mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
mpuConfiguration.write(MPU3050_INT_CFG, 0); gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0);
mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET); gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET);
mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX);
} }
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{ {
UNUSED(gyro); UNUSED(gyro);
uint8_t buf[2]; uint8_t buf[2];
if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) { if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
return false; return false;
} }
@ -78,7 +78,7 @@ static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
bool mpu3050Detect(gyroDev_t *gyro) bool mpu3050Detect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_3050) { if (gyro->mpuDetectionResult.sensor != MPU_3050) {
return false; return false;
} }
gyro->init = mpu3050Init; gyro->init = mpu3050Init;

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

@ -52,7 +52,7 @@
static void mpu6050AccInit(accDev_t *acc) static void mpu6050AccInit(accDev_t *acc)
{ {
switch (mpuDetectionResult.resolution) { switch (acc->mpuDetectionResult.resolution) {
case MPU_HALF_RESOLUTION: case MPU_HALF_RESOLUTION:
acc->acc_1G = 256 * 4; acc->acc_1G = 256 * 4;
break; break;
@ -64,13 +64,13 @@ static void mpu6050AccInit(accDev_t *acc)
bool mpu6050AccDetect(accDev_t *acc) bool mpu6050AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (acc->mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
} }
acc->init = mpu6050AccInit; acc->init = mpu6050AccInit;
acc->read = mpuAccRead; 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; return true;
} }
@ -79,29 +79,29 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(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); 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) 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)
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_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 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) 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)
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_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec
// ACC Init stuff. // ACC Init stuff.
// Accel scale 8g (4096 LSB/g) // 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 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
mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif #endif
} }
bool mpu6050GyroDetect(gyroDev_t *gyro) bool mpu6050GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0) { if (gyro->mpuDetectionResult.sensor != MPU_60x0) {
return false; return false;
} }
gyro->init = mpu6050GyroInit; 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) bool mpu6500AccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (acc->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; return false;
} }
@ -55,40 +55,40 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(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); delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100); delay(100);
mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
delay(100); 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); 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); 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); delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15); 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); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
#ifdef USE_MPU9250_MAG #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 #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 #endif
delay(15); delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #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 #endif
delay(15); delay(15);
} }
bool mpu6500GyroDetect(gyroDev_t *gyro) bool mpu6500GyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_I2C) { if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
return false; 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) bool icm20689SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (acc->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; return false;
} }
@ -130,31 +130,31 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); 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); delay(100);
mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100); delay(100);
// mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); // gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0);
// delay(100); // 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); 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); 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); delay(15);
mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf);
delay(15); 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); delay(100);
// Data ready interrupt configuration // 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 // 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
mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN
delay(15); delay(15);
#ifdef USE_MPU_DATA_READY_SIGNAL #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 #endif
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD);
@ -162,7 +162,7 @@ void icm20689GyroInit(gyroDev_t *gyro)
bool icm20689SpiGyroDetect(gyroDev_t *gyro) bool icm20689SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != ICM_20689_SPI) { if (gyro->mpuDetectionResult.sensor != ICM_20689_SPI) {
return false; 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) bool mpu6000SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (acc->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; return false;
} }
@ -268,7 +268,7 @@ bool mpu6000SpiAccDetect(accDev_t *acc)
bool mpu6000SpiGyroDetect(gyroDev_t *gyro) bool mpu6000SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_60x0_SPI) { if (gyro->mpuDetectionResult.sensor != MPU_60x0_SPI) {
return false; 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) bool mpu6500SpiAccDetect(accDev_t *acc)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
} }
@ -128,7 +128,7 @@ bool mpu6500SpiAccDetect(accDev_t *acc)
bool mpu6500SpiGyroDetect(gyroDev_t *gyro) bool mpu6500SpiGyroDetect(gyroDev_t *gyro)
{ {
if (mpuDetectionResult.sensor != MPU_65xx_SPI) { if (gyro->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false; return false;
} }

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

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

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

@ -31,8 +31,9 @@ void SetSysClock(void);
void systemReset(void) void systemReset(void)
{ {
if (mpuConfiguration.reset) if (mpuReset) {
mpuConfiguration.reset(); mpuReset();
}
__disable_irq(); __disable_irq();
NVIC_SystemReset(); NVIC_SystemReset();
@ -40,8 +41,9 @@ void systemReset(void)
void systemResetToBootloader(void) void systemResetToBootloader(void)
{ {
if (mpuConfiguration.reset) if (mpuReset) {
mpuConfiguration.reset(); mpuReset();
}
*((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX *((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) void systemReset(void)
{ {
if (mpuConfiguration.reset) if (mpuReset) {
mpuConfiguration.reset(); mpuReset();
}
__disable_irq(); __disable_irq();
NVIC_SystemReset(); NVIC_SystemReset();
@ -42,9 +43,9 @@ void systemReset(void)
void systemResetToBootloader(void) void systemResetToBootloader(void)
{ {
if (mpuConfiguration.reset) if (mpuReset) {
mpuConfiguration.reset(); mpuReset();
}
(*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot (*(__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 "io/beeper.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "config/feature.h" #include "config/feature.h"
@ -230,6 +231,9 @@ retry:
bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval) bool accInit(const accelerometerConfig_t *accelerometerConfig, uint32_t gyroSamplingInverval)
{ {
memset(&acc, 0, sizeof(acc)); 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)) { if (!accDetect(&acc.dev, accelerometerConfig->acc_hardware)) {
return false; return false;
} }

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

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