diff --git a/src/main/drivers/accgyro_mpu.c b/src/main/drivers/accgyro_mpu.c index 93405ba687..9e7c3ea98f 100644 --- a/src/main/drivers/accgyro_mpu.c +++ b/src/main/drivers/accgyro_mpu.c @@ -48,7 +48,7 @@ #include "accgyro_mpu.h" -mpuResetFuncPtr mpuReset; +mpuResetFnPtr mpuResetFn; #ifndef MPU_I2C_INSTANCE #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 // 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); if (revision) { /* Congrats, these parts are better. */ @@ -89,7 +89,7 @@ static void mpu6050FindRevision(gyroDev_t *gyro) failureMode(FAILURE_ACC_INCOMPATIBLE); } } else { - ack = gyro->mpuConfiguration.read(MPU_RA_PRODUCT_ID, 1, &productId); + ack = gyro->mpuConfiguration.readFn(MPU_RA_PRODUCT_ID, 1, &productId); revision = productId & 0x0F; if (!revision) { failureMode(FAILURE_ACC_INCOMPATIBLE); @@ -176,7 +176,7 @@ bool mpuAccRead(accDev_t *acc) { 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) { return false; } @@ -199,7 +199,7 @@ bool mpuGyroRead(gyroDev_t *gyro) { 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) { return false; } @@ -230,8 +230,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6000SpiDetect()) { gyro->mpuDetectionResult.sensor = MPU_60x0_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6000ReadRegister; - gyro->mpuConfiguration.write = mpu6000WriteRegister; + gyro->mpuConfiguration.readFn = mpu6000ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6000WriteRegister; return true; } #endif @@ -241,8 +241,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu6500Sensor != MPU_NONE) { gyro->mpuDetectionResult.sensor = mpu6500Sensor; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = mpu6500ReadRegister; - gyro->mpuConfiguration.write = mpu6500WriteRegister; + gyro->mpuConfiguration.readFn = mpu6500ReadRegister; + gyro->mpuConfiguration.writeFn = mpu6500WriteRegister; return true; } #endif @@ -251,11 +251,11 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (mpu9250SpiDetect()) { 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; + gyro->mpuConfiguration.readFn = mpu9250ReadRegister; + gyro->mpuConfiguration.slowreadFn = mpu9250SlowReadRegister; + gyro->mpuConfiguration.verifywriteFn = verifympu9250WriteRegister; + gyro->mpuConfiguration.writeFn = mpu9250WriteRegister; + gyro->mpuConfiguration.resetFn = mpu9250ResetGyro; return true; } #endif @@ -264,8 +264,8 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) if (icm20689SpiDetect()) { gyro->mpuDetectionResult.sensor = ICM_20689_SPI; gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; - gyro->mpuConfiguration.read = icm20689ReadRegister; - gyro->mpuConfiguration.write = icm20689WriteRegister; + gyro->mpuConfiguration.readFn = icm20689ReadRegister; + gyro->mpuConfiguration.writeFn = icm20689WriteRegister; return true; } #endif @@ -277,22 +277,20 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) { - bool ack; - uint8_t sig; - uint8_t inquiryResult; // MPU datasheet specifies 30ms. delay(35); #ifndef USE_I2C - ack = false; - sig = 0; + uint8_t sig = 0; + bool ack = false; #else - ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); + uint8_t sig; + bool ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I, 1, &sig); #endif if (ack) { - gyro->mpuConfiguration.read = mpuReadRegisterI2C; - gyro->mpuConfiguration.write = mpuWriteRegisterI2C; + gyro->mpuConfiguration.readFn = mpuReadRegisterI2C; + gyro->mpuConfiguration.writeFn = mpuWriteRegisterI2C; } else { #ifdef USE_SPI bool detectedSpiSensor = detectSPISensorsAndUpdateDetectionResult(gyro); @@ -305,6 +303,7 @@ mpuDetectionResult_t *mpuDetect(gyroDev_t *gyro) gyro->mpuConfiguration.gyroReadXRegister = MPU_RA_GYRO_XOUT_H; // If an MPU3050 is connected sig will contain 0. + uint8_t inquiryResult; ack = mpuReadRegisterI2C(MPU_RA_WHO_AM_I_LEGACY, 1, &inquiryResult); inquiryResult &= MPU_INQUIRY_MASK; if (ack && inquiryResult == MPUx0x0_WHO_AM_I_CONST) { diff --git a/src/main/drivers/accgyro_mpu.h b/src/main/drivers/accgyro_mpu.h index 8ba01540a3..8a44af0464 100644 --- a/src/main/drivers/accgyro_mpu.h +++ b/src/main/drivers/accgyro_mpu.h @@ -124,18 +124,18 @@ // RF = Register Flag #define MPU_RF_DATA_RDY_EN (1 << 0) -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); +typedef bool (*mpuReadRegisterFnPtr)(uint8_t reg, uint8_t length, uint8_t* data); +typedef bool (*mpuWriteRegisterFnPtr)(uint8_t reg, uint8_t data); +typedef void(*mpuResetFnPtr)(void); -extern mpuResetFuncPtr mpuReset; +extern mpuResetFnPtr mpuResetFn; typedef struct mpuConfiguration_s { - mpuReadRegisterFunc read; - mpuWriteRegisterFunc write; - mpuReadRegisterFunc slowread; - mpuWriteRegisterFunc verifywrite; - mpuResetFuncPtr reset; + mpuReadRegisterFnPtr readFn; + mpuWriteRegisterFnPtr writeFn; + mpuReadRegisterFnPtr slowreadFn; + mpuWriteRegisterFnPtr verifywriteFn; + mpuResetFnPtr resetFn; uint8_t gyroReadXRegister; // Y and Z must registers follow this, 2 words each } mpuConfiguration_t; @@ -178,7 +178,7 @@ typedef enum { ICM_20689_SPI, ICM_20608_SPI, ICM_20602_SPI -} detectedMPUSensor_e; +} mpuSensor_e; typedef enum { MPU_HALF_RESOLUTION, @@ -186,7 +186,7 @@ typedef enum { } mpu6050Resolution_e; typedef struct mpuDetectionResult_s { - detectedMPUSensor_e sensor; + mpuSensor_e sensor; mpu6050Resolution_e resolution; } mpuDetectionResult_t; diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index 15728d9e12..374604b7c1 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -53,21 +53,21 @@ static void mpu3050Init(gyroDev_t *gyro) delay(25); // datasheet page 13 says 20ms. other stuff could have been running meanwhile. but we'll be safe - ack = gyro->mpuConfiguration.write(MPU3050_SMPLRT_DIV, 0); + ack = gyro->mpuConfiguration.writeFn(MPU3050_SMPLRT_DIV, 0); if (!ack) failureMode(FAILURE_ACC_INIT); - 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); + gyro->mpuConfiguration.writeFn(MPU3050_DLPF_FS_SYNC, MPU3050_FS_SEL_2000DPS | gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU3050_INT_CFG, 0); + gyro->mpuConfiguration.writeFn(MPU3050_USER_CTRL, MPU3050_USER_RESET); + gyro->mpuConfiguration.writeFn(MPU3050_PWR_MGM, MPU3050_CLK_SEL_PLL_GX); } static bool mpu3050ReadTemperature(gyroDev_t *gyro, int16_t *tempData) { UNUSED(gyro); uint8_t buf[2]; - if (!gyro->mpuConfiguration.read(MPU3050_TEMP_OUT, 2, buf)) { + if (!gyro->mpuConfiguration.readFn(MPU3050_TEMP_OUT, 2, buf)) { return false; } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 631f7ec805..0a1fe6f336 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -79,23 +79,23 @@ static void mpu6050GyroInit(gyroDev_t *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); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) - gyro->mpuConfiguration.write(MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops(gyro)); //SMPLRT_DIV -- SMPLRT_DIV = 0 Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) + 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.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 - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) - gyro->mpuConfiguration.write(MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec + 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.writeFn(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) - 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 #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 } diff --git a/src/main/drivers/accgyro_mpu6500.c b/src/main/drivers/accgyro_mpu6500.c index f9a46fbfc1..30d7caa0fb 100644 --- a/src/main/drivers/accgyro_mpu6500.c +++ b/src/main/drivers/accgyro_mpu6500.c @@ -55,34 +55,34 @@ void mpu6500GyroInit(gyroDev_t *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); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x07); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x07); delay(100); - gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); + gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); 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); 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); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); 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); // Data ready interrupt configuration #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 - 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 delay(15); #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 delay(15); } diff --git a/src/main/drivers/accgyro_spi_icm20689.c b/src/main/drivers/accgyro_spi_icm20689.c index 29e22d5743..8d0af213df 100644 --- a/src/main/drivers/accgyro_spi_icm20689.c +++ b/src/main/drivers/accgyro_spi_icm20689.c @@ -130,32 +130,32 @@ void icm20689GyroInit(gyroDev_t *gyro) 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); - gyro->mpuConfiguration.write(MPU_RA_SIGNAL_PATH_RESET, 0x03); + gyro->mpuConfiguration.writeFn(MPU_RA_SIGNAL_PATH_RESET, 0x03); delay(100); -// gyro->mpuConfiguration.write(MPU_RA_PWR_MGMT_1, 0); +// gyro->mpuConfiguration.writeFn(MPU_RA_PWR_MGMT_1, 0); // 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); 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); - gyro->mpuConfiguration.write(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); + gyro->mpuConfiguration.writeFn(MPU_RA_ACCEL_CONFIG, INV_FSR_16G << 3); delay(15); - gyro->mpuConfiguration.write(MPU_RA_CONFIG, gyro->lpf); + gyro->mpuConfiguration.writeFn(MPU_RA_CONFIG, gyro->lpf); 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); // 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.write(MPU_RA_INT_PIN_CFG, 0x10); // 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.writeFn(MPU_RA_INT_PIN_CFG, 0x10); // INT_ANYRD_2CLEAR, BYPASS_EN delay(15); #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 spiSetDivisor(ICM20689_SPI_INSTANCE, SPI_CLOCK_STANDARD); diff --git a/src/main/drivers/system_stm32f4xx.c b/src/main/drivers/system_stm32f4xx.c index 55d570eecb..0216d45427 100644 --- a/src/main/drivers/system_stm32f4xx.c +++ b/src/main/drivers/system_stm32f4xx.c @@ -31,8 +31,8 @@ void SetSysClock(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -41,8 +41,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } *((uint32_t *)0x2001FFFC) = 0xDEADBEEF; // 128KB SRAM STM32F4XX diff --git a/src/main/drivers/system_stm32f7xx.c b/src/main/drivers/system_stm32f7xx.c index 3254c475d6..0099833674 100644 --- a/src/main/drivers/system_stm32f7xx.c +++ b/src/main/drivers/system_stm32f7xx.c @@ -33,8 +33,8 @@ void SystemClock_Config(void); void systemReset(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } __disable_irq(); @@ -43,8 +43,8 @@ void systemReset(void) void systemResetToBootloader(void) { - if (mpuReset) { - mpuReset(); + if (mpuResetFn) { + mpuResetFn(); } (*(__IO uint32_t *) (BKPSRAM_BASE + 4)) = 0xDEADBEEF; // flag that will be readable after reboot diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9732a96859..d0b15ece6b 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -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) gyro.dev.mpuIntExtiConfig = selectMPUIntExtiConfig(); mpuDetect(&gyro.dev); - mpuReset = gyro.dev.mpuConfiguration.reset; + mpuResetFn = gyro.dev.mpuConfiguration.resetFn; #endif if (!gyroDetect(&gyro.dev)) {