Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Renamed MPU function pointers, align with iNav

Browse source
This commit is contained in:
Michael Jakob 2017-01-29 18:07:18 +01:00 • committed by Martin Budden
parent 48e7c62506
commit 61d72b8738
9 changed files with 79 additions and 80 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -48,7 +48,7 @@
#include "accgyro_mpu.h" #include "accgyro_mpu.h"
mpuResetFuncPtr mpuReset; mpuResetFnPtr mpuResetFn;
#ifndef MPU_I2C_INSTANCE #ifndef MPU_I2C_INSTANCE
#define MPU_I2C_INSTANCE I2C_DEVICE #define MPU_I2C_INSTANCE I2C_DEVICE
@ -75,7 +75,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
// 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 = gyro->mpuConfiguration.read(MPU_RA_XA_OFFS_H, 6, readBuffer); ack = gyro->mpuConfiguration.readFn(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. */
@ -89,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro)
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
} }
} else { } else {
ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId);
revision = productId & 0x0F; revision = productId & 0x0F;
if (!revision) { if (!revision) {
failureMode(FAILURE_ACC_INCOMPATIBLE); failureMode(FAILURE_ACC_INCOMPATIBLE);
@ -176,7 +176,7 @@ bool mpuAccRead(accDev_t *acc)
{ {
uint8_t data[6]; uint8_t data[6];
bool ack = acc->mpuConfiguration.read(MPU_RA_ACCEL_XOUT_H, 6, data); bool ack = acc->mpuConfiguration.readFn(MPU_RA_ACCEL_XOUT_H, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -199,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro)
{ {
uint8_t data[6]; uint8_t data[6];
const bool ack = gyro->mpuConfiguration.read(gyro->mpuConfiguration.gyroReadXRegister, 6, data); const bool ack = gyro->mpuConfiguration.readFn(gyro->mpuConfiguration.gyroReadXRegister, 6, data);
if (!ack) { if (!ack) {
return false; return false;
} }
@ -230,8 +230,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu6000SpiDetect()) { if (mpu6000SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuDetectionResult.sensor = MPU_60x0_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6000ReadRegister; gyro->mpuConfiguration.readFn = mpu6000ReadRegister;
gyro->mpuConfiguration.write = mpu6000WriteRegister; gyro->mpuConfiguration.writeFn = mpu6000WriteRegister;
return true; return true;
} }
#endif #endif
@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu6500Sensor != MPU_NONE) { if (mpu6500Sensor != MPU_NONE) {
gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuDetectionResult.sensor = mpu6500Sensor;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu6500ReadRegister; gyro->mpuConfiguration.readFn = mpu6500ReadRegister;
gyro->mpuConfiguration.write = mpu6500WriteRegister; gyro->mpuConfiguration.writeFn = mpu6500WriteRegister;
return true; return true;
} }
#endif #endif
@ -251,11 +251,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (mpu9250SpiDetect()) { if (mpu9250SpiDetect()) {
gyro->mpuDetectionResult.sensor = MPU_9250_SPI; gyro->mpuDetectionResult.sensor = MPU_9250_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = mpu9250ReadRegister; gyro->mpuConfiguration.readFn = mpu9250ReadRegister;
gyro->mpuConfiguration.slowread = mpu9250SlowReadRegister; gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister;
gyro->mpuConfiguration.verifywrite = verifympu9250WriteRegister; gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister;
gyro->mpuConfiguration.write = mpu9250WriteRegister; gyro->mpuConfiguration.writeFn = mpu9250WriteRegister;
gyro->mpuConfiguration.reset = mpu9250ResetGyro; gyro->mpuConfiguration.resetFn = mpu9250ResetGyro;
return true; return true;
} }
#endif #endif
@ -264,8 +264,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
if (icm20689SpiDetect()) { if (icm20689SpiDetect()) {
gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuDetectionResult.sensor = ICM_20689_SPI;
gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H;
gyro->mpuConfiguration.read = icm20689ReadRegister; gyro->mpuConfiguration.readFn = icm20689ReadRegister;
gyro->mpuConfiguration.write = icm20689WriteRegister; gyro->mpuConfiguration.writeFn = icm20689WriteRegister;
return true; return true;
} }
#endif #endif
@ -277,22 +277,20 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro)
mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
{ {
bool ack;
uint8_t sig;
uint8_t inquiryResult;
// MPU datasheet specifies 30ms. // MPU datasheet specifies 30ms.
delay(35); delay(35);
#ifndef USE_I2C #ifndef USE_I2C
ack = false; uint8_t sig = 0;
sig = 0; bool ack = false;
#else #else
ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); uint8_t sig;
bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig);
#endif #endif
if (ack) { if (ack) {
gyro->mpuConfiguration.read = mpuReadRegisterI2C; gyro->mpuConfiguration.readFn = mpuReadRegisterI2C;
gyro->mpuConfiguration.write = mpuWriteRegisterI2C; gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C;
} else { } else {
#ifdef USE_SPI #ifdef USE_SPI
bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro);
@ -305,6 +303,7 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro)
gyro->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.
uint8_t inquiryResult;
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) {

22
src/main/drivers/accgyro_mpu.h
View file

@ -124,18 +124,18 @@
// RF = Register Flag // RF = Register Flag
#define MPU_RF_DATA_RDY_EN (1 << 0) #define MPU_RF_DATA_RDY_EN (1 << 0)
typedef bool (*mpuReadRegisterFunc)(uint8_t reg, uint8_t length, uint8_t* data); typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data);
typedef bool (*mpuWriteRegisterFunc)(uint8_t reg, uint8_t data); typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data);
typedef void(*mpuResetFuncPtr)(void); typedef void(*mpuResetFnPtr)(void);
extern mpuResetFuncPtr mpuReset; extern mpuResetFnPtr mpuResetFn;
typedef struct mpuConfiguration_s { typedef struct mpuConfiguration_s {
mpuReadRegisterFunc read; mpuReadRegisterFnPtr readFn;
mpuWriteRegisterFunc write; mpuWriteRegisterFnPtr writeFn;
mpuReadRegisterFunc slowread; mpuReadRegisterFnPtr slowreadFn;
mpuWriteRegisterFunc verifywrite; mpuWriteRegisterFnPtr verifywriteFn;
mpuResetFuncPtr reset; mpuResetFnPtr resetFn;
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
} mpuConfiguration_t; } mpuConfiguration_t;
@ -178,7 +178,7 @@ typedef enum {
ICM_20689_SPI, ICM_20689_SPI,
ICM_20608_SPI, ICM_20608_SPI,
ICM_20602_SPI ICM_20602_SPI
} detectedMPUSensor_e; } mpuSensor_e;
typedef enum { typedef enum {
MPU_HALF_RESOLUTION, MPU_HALF_RESOLUTION,
@ -186,7 +186,7 @@ typedef enum {
} mpu6050Resolution_e; } mpu6050Resolution_e;
typedef struct mpuDetectionResult_s { typedef struct mpuDetectionResult_s {
detectedMPUSensor_e sensor; mpuSensor_e sensor;
mpu6050Resolution_e resolution; mpu6050Resolution_e resolution;
} mpuDetectionResult_t; } mpuDetectionResult_t;

12
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 = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0);
if (!ack) if (!ack)
failureMode(FAILURE_ACC_INIT); failureMode(FAILURE_ACC_INIT);
gyro->mpuConfiguration.write(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf);
gyro->mpuConfiguration.write(MPU3050_INT_CFG, 0); gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0);
gyro->mpuConfiguration.write(MPU3050_USER_CTRL, MPU3050_USER_RESET); gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET);
gyro->mpuConfiguration.write(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); gyro->mpuConfiguration.writeFn(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 (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) { if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) {
return false; return false;
} }

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

@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1
delay(100); delay(100);
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.writeFn(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(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //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
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.writeFn(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 gyro->mpuConfiguration.writeFn(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)
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, gyro->mpuConfiguration.writeFn(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
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN); gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU_RF_DATA_RDY_EN);
#endif #endif
} }

22
src/main/drivers/accgyro_mpu6500.c
View file

@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *gyro)
{ {
mpuGyroInit(gyro); mpuGyroInit(gyro);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, MPU6500_BIT_RESET);
delay(100); delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07);
delay(100); delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
delay(100); delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
#ifdef USE_MPU9250_MAG #ifdef USE_MPU9250_MAG
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN gyro->mpuConfiguration.writeFn(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR | MPU6500_BIT_BYPASS_EN); // INT_ANYRD_2CLEAR, BYPASS_EN
#else #else
gyro->mpuConfiguration.write(MPU_RA_INT_PIN_CFG, MPU6500_BIT_INT_ANYRD_2CLEAR); // INT_ANYRD_2CLEAR gyro->mpuConfiguration.writeFn(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
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable gyro->mpuConfiguration.writeFn(MPU_RA_INT_ENABLE, MPU6500_BIT_RAW_RDY_EN); // RAW_RDY_EN interrupt enable
#endif #endif
delay(15); delay(15);
} }

22
src/main/drivers/accgyro_spi_icm20689.c
View file

@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro)
spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON); spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_INITIALIZATON);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, ICM20689_BIT_RESET);
delay(100); delay(100);
gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03);
delay(100); delay(100);
// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); // gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0);
// delay(100); // delay(100);
gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, INV_CLK_PLL); gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, INV_CLK_PLL);
delay(15); delay(15);
const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED); const uint8_t raGyroConfigData = gyro->gyroRateKHz > GYRO_RATE_8_kHz ? (INV_FSR_2000DPS << 3 | FCB_3600_32) : (INV_FSR_2000DPS << 3 | FCB_DISABLED);
gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, raGyroConfigData); gyro->mpuConfiguration.writeFn(MPU_RA_GYRO_CONFIG, raGyroConfigData);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf);
delay(15); delay(15);
gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops gyro->mpuConfiguration.writeFn(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); // Get Divider Drops
delay(100); delay(100);
// Data ready interrupt configuration // Data ready interrupt configuration
// 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.writeFn(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 gyro->mpuConfiguration.writeFn(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
gyro->mpuConfiguration.write(MPU_RA_INT_ENABLE, 0x01); // RAW_RDY_EN interrupt enable gyro->mpuConfiguration.writeFn(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);

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

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

8
src/main/drivers/system_stm32f7xx.c
View file

@ -33,8 +33,8 @@ void SystemClock_Config(void);
void systemReset(void) void systemReset(void)
{ {
if (mpuReset) { if (mpuResetFn) {
mpuReset(); mpuResetFn();
} }
__disable_irq(); __disable_irq();
@ -43,8 +43,8 @@ void systemReset(void)
void systemResetToBootloader(void) void systemResetToBootloader(void)
{ {
if (mpuReset) { if (mpuResetFn) {
mpuReset(); mpuResetFn();
} }
(*(__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

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

@ -238,7 +238,7 @@ bool gyroInit(const gyroConfig_t *gyroConfigToUse)
#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)
gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig();
mpuDetect(&gyro.dev); mpuDetect(&gyro.dev);
mpuReset = gyro.dev.mpuConfiguration.reset; mpuResetFn = gyro.dev.mpuConfiguration.resetFn;
#endif #endif
if (!gyroDetect(&gyro.dev)) { if (!gyroDetect(&gyro.dev)) {