Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Made mpuDetectionResult and mpuConfiguration gyro and acc instance variables

Browse source
This commit is contained in:
Martin Budden 2016-12-12 16:45:43 +00:00
parent 115b1e76f9
commit 22ddc2ccf9
15 changed files with 113 additions and 108 deletions
Download patch file Download diff file Expand all files Collapse all files

7
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
sensorReadFuncPtr temperature; // read temperature if available sensorGyroReadDataFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus; sensorGyroInterruptStatusFuncPtr intStatus;
float scale; // scalefactor float scale; // scalefactor
volatile bool dataReady; volatile bool dataReady;
@ -45,6 +46,8 @@ typedef struct gyroDev_s {
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
sensor_align_e gyroAlign; sensor_align_e gyroAlign;
const extiConfig_t *mpuIntExtiConfig; const extiConfig_t *mpuIntExtiConfig;
mpuDetectionResult_t mpuDetectionResult;
mpuConfiguration_t mpuConfiguration;
} gyroDev_t; } gyroDev_t;
typedef struct accDev_s { typedef struct accDev_s {
@ -54,4 +57,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;

6
src/main/drivers/accgyro_fake.c
View file

@ -51,9 +51,9 @@ static bool fakeGyroRead(gyroDev_t *gyro)
return true; return true;
} }
static bool fakeGyroReadTemp(int16_t *tempData) static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
{ {
UNUSED(tempData); UNUSED(temperatureData);
return true; return true;
} }
@ -68,7 +68,7 @@ bool fakeGyroDetect(gyroDev_t *gyro)
gyro->init = fakeGyroInit; gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus; gyro->intStatus = fakeGyroInitStatus;
gyro->read = fakeGyroRead; gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp; gyro->temperature = fakeGyroReadTemperature;
gyro->scale = 1.0f / 16.4f; gyro->scale = 1.0f / 16.4f;
return true; return true;
} }

97
src/main/drivers/accgyro_mpu.c
View file

@ -49,20 +49,16 @@
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;
#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
@ -71,11 +67,8 @@ mpuConfiguration_t mpuConfiguration;
#define MPU_INQUIRY_MASK 0x7E #define MPU_INQUIRY_MASK 0x7E
mpuDetectionResult_t *mpuDetect(void) mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{ {
memset(&mpuDetectionResult, 0, sizeof(mpuDetectionResult));
memset(&mpuConfiguration, 0, sizeof(mpuConfiguration));
bool ack; bool ack;
uint8_t sig; uint8_t sig;
uint8_t inquiryResult; uint8_t inquiryResult;
@ -90,84 +83,84 @@ mpuDetectionResult_t *mpuDetect(void)
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
@ -176,7 +169,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
} }
#endif #endif
static void mpu6050FindRevision(void) static void mpu6050FindRevision(gyroDev_t *gyro)
{ {
bool ack; bool ack;
UNUSED(ack); UNUSED(ack);
@ -189,28 +182,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;
} }
} }
} }
@ -296,7 +289,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;
} }
@ -312,7 +305,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;
} }

8
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);
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,12 +181,10 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
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(void); mpuDetectionResult_t *mpuDetect(struct gyroDev_s *gyro);
bool mpuCheckDataReady(struct gyroDev_s *gyro); bool mpuCheckDataReady(struct gyroDev_s *gyro);

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

@ -52,20 +52,20 @@ 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(int16_t *tempData) static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{ {
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;
} }
@ -76,7 +76,7 @@ static bool mpu3050ReadTemperature(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,30 +79,30 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
bool ack = mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 bool ack = gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
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) ack = 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)
ack = mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) ack = gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); //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
ack = 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) ack = 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)
ack = mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec ack = 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)
ack = mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3); ack = gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 3);
ack = mpuConfiguration.write(MPU_RA_INT_PIN_CFG, ack = 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
ack = mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); ack = gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif #endif
UNUSED(ack); UNUSED(ack);
} }
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;
} }
@ -68,40 +68,40 @@ void mpu6500GyroInit(gyroDev_t *gyro)
} }
#endif #endif
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_8G << 3); gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 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, gyroMPU6xxxCalculateDivider()); // Get Divider gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider
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, 0 << 7 | 0 << 6 | 0 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 1 << 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 | 1 << 1 | 0 << 0); // INT_ANYRD_2CLEAR, BYPASS_EN
#else #else
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
#endif #endif
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
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;
} }

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

@ -101,7 +101,7 @@ void mpu6500SpiAccInit(accDev_t *acc)
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 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
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;
} }

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

@ -38,4 +38,5 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
struct gyroDev_s; struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro); typedef void (*sensorGyroInitFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroReadFuncPtr)(struct gyroDev_s *gyro);
typedef bool (*sensorGyroReadDataFuncPtr)(struct gyroDev_s *gyro, int16_t *data);
typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro); typedef bool (*sensorGyroInterruptStatusFuncPtr)(struct gyroDev_s *gyro);

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

@ -34,8 +34,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();
@ -43,8 +44,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

5
src/main/fc/mw.c
View file

@ -183,8 +183,9 @@ void annexCode(void)
} }
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.dev.temperature) if (gyro.dev.temperature) {
gyro.dev.temperature(&telemTemperature1); gyro.dev.temperature(&gyro.dev, &telemTemperature1);
}
} }
void mwDisarm(void) void mwDisarm(void)

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

@ -49,6 +49,7 @@
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/boardalignment.h" #include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
@ -222,6 +223,9 @@ retry:
bool accInit(const accelerometerConfig_t *accConfig, uint32_t targetLooptime) bool accInit(const accelerometerConfig_t *accConfig, uint32_t targetLooptime)
{ {
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, accConfig->acc_hardware)) { if (!accDetect(&acc.dev, accConfig->acc_hardware)) {
return false; return false;
} }

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

@ -208,12 +208,13 @@ static bool gyroDetect(gyroDev_t *dev, const extiConfig_t *extiConfig)
bool gyroInit(const gyroConfig_t *gyroConfigToUse) bool gyroInit(const gyroConfig_t *gyroConfigToUse)
{ {
gyroConfig = 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) #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)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig(); const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetect(); mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif #endif
memset(&gyro, 0, sizeof(gyro));
if (!gyroDetect(&gyro.dev, extiConfig)) { if (!gyroDetect(&gyro.dev, extiConfig)) {
return false; return false;
} }