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 "drivers/exti.h"
#include "drivers/sensor.h"
#include "drivers/accgyro_mpu.h"
#ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE
@ -37,7 +38,7 @@
typedef struct gyroDev_s {
sensorGyroInitFuncPtr init; // initialize function
sensorGyroReadFuncPtr read; // read 3 axis data function
sensorReadFuncPtr temperature; // read temperature if available
sensorGyroReadDataFuncPtr temperature; // read temperature if available
sensorGyroInterruptStatusFuncPtr intStatus;
float scale; // scalefactor
volatile bool dataReady;
@ -45,6 +46,8 @@ typedef struct gyroDev_s {
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 +57,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;

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

@ -51,9 +51,9 @@ static bool fakeGyroRead(gyroDev_t *gyro)
return true;
}
static bool fakeGyroReadTemp(int16_t *tempData)
static bool fakeGyroReadTemperature(gyroDev_t *gyro, int16_t *temperatureData)
{
UNUSED(tempData);
UNUSED(temperatureData);
return true;
}
@ -68,7 +68,7 @@ bool fakeGyroDetect(gyroDev_t *gyro)
gyro->init = fakeGyroInit;
gyro->intStatus = fakeGyroInitStatus;
gyro->read = fakeGyroRead;
gyro->temperature = fakeGyroReadTemp;
gyro->temperature = fakeGyroReadTemperature;
gyro->scale = 1.0f / 16.4f;
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 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;
#define MPU_ADDRESS 0x68
// WHO_AM_I register contents for MPU3050, 6050 and 6500
@ -71,11 +67,8 @@ mpuConfiguration_t mpuConfiguration;
#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;
uint8_t sig;
uint8_t inquiryResult;
@ -90,84 +83,84 @@ mpuDetectionResult_t *mpuDetect(void)
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
@ -176,7 +169,7 @@ static bool detectSPISensorsAndUpdateDetectionResult(void)
}
#endif
static void mpu6050FindRevision(void)
static void mpu6050FindRevision(gyroDev_t *gyro)
{
bool 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
// 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;
}
}
}
@ -296,7 +289,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;
}
@ -312,7 +305,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;
}

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 void(*mpuResetFuncPtr)(void);
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,12 +181,10 @@ typedef struct mpuDetectionResult_s {
mpu6050Resolution_e resolution;
} mpuDetectionResult_t;
extern mpuDetectionResult_t mpuDetectionResult;
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(void);
mpuDetectionResult_t *mpuDetect(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
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(int16_t *tempData)
static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData)
{
uint8_t buf[2];
if (!mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) {
return false;
}
@ -76,7 +76,7 @@ static bool mpu3050ReadTemperature(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,30 +79,30 @@ static void mpu6050GyroInit(gyroDev_t *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);
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 = 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_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_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
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 = 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_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_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)
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
#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
UNUSED(ack);
}
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;
}
@ -68,40 +68,40 @@ void mpu6500GyroInit(gyroDev_t *gyro)
}
#endif
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_8G << 3);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_8G << 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, gyroMPU6xxxCalculateDivider()); // Get Divider
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxCalculateDivider()); // Get Divider
delay(100);
// Data ready interrupt configuration
#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
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
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
delay(15);
}
bool mpu6500GyroDetect(gyroDev_t *gyro)
{
if (mpuDetectionResult.sensor != MPU_65xx_I2C) {
if (gyro->mpuDetectionResult.sensor != MPU_65xx_I2C) {
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

@ -101,7 +101,7 @@ void mpu6500SpiAccInit(accDev_t *acc)
bool mpu6500SpiAccDetect(accDev_t *acc)
{
if (mpuDetectionResult.sensor != MPU_65xx_SPI) {
if (acc->mpuDetectionResult.sensor != MPU_65xx_SPI) {
return false;
}
@ -128,7 +128,7 @@ void mpu6500SpiGyroInit(gyroDev_t *gyro)
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;
}

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

@ -38,4 +38,5 @@ typedef bool (*sensorAccReadFuncPtr)(struct accDev_s *acc);
struct gyroDev_s;
typedef void (*sensorGyroInitFuncPtr)(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);

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

@ -34,8 +34,9 @@ void SetSysClock(void);
void systemReset(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
__disable_irq();
NVIC_SystemReset();
@ -43,8 +44,9 @@ void systemReset(void)
void systemResetToBootloader(void)
{
if (mpuConfiguration.reset)
mpuConfiguration.reset();
if (mpuReset) {
mpuReset();
}
*((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.
if (gyro.dev.temperature)
gyro.dev.temperature(&telemTemperature1);
if (gyro.dev.temperature) {
gyro.dev.temperature(&gyro.dev, &telemTemperature1);
}
}
void mwDisarm(void)

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

@ -49,6 +49,7 @@
#include "sensors/acceleration.h"
#include "sensors/battery.h"
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
#include "fc/runtime_config.h"
@ -222,6 +223,9 @@ retry:
bool accInit(const accelerometerConfig_t *accConfig, uint32_t targetLooptime)
{
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)) {
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)
{
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)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetect();
mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset;
#endif
memset(&gyro, 0, sizeof(gyro));
if (!gyroDetect(&gyro.dev, extiConfig)) {
return false;
}