diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 1e5c82997..0bc959a50 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -165,7 +165,9 @@ static void mpu6050AccInit(void); static bool mpu6050AccRead(int16_t *accData); static void mpu6050GyroInit(void); static bool mpu6050GyroRead(int16_t *gyroADC); -static void checkMPU6050Interrupt(bool *gyroIsUpdated); +static void checkMPU6050DataReady(bool *mpuDataReadyPtr); + +static bool mpuDataReady; typedef enum { MPU_6050_HALF_RESOLUTION, @@ -184,6 +186,8 @@ void MPU_DATA_READY_EXTI_Handler(void) EXTI_ClearITPendingBit(mpu6050Config->exti_line); + mpuDataReady = true; + #ifdef DEBUG_MPU_DATA_READY_INTERRUPT // Measure the delta in micro seconds between calls to the interrupt handler static uint32_t lastCalledAt = 0; @@ -358,7 +362,7 @@ bool mpu6050GyroDetect(const mpu6050Config_t *configToUse, gyro_t *gyro, uint16_ gyro->init = mpu6050GyroInit; gyro->read = mpu6050GyroRead; - gyro->intStatus = checkMPU6050Interrupt; + gyro->intStatus = checkMPU6050DataReady; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; @@ -458,12 +462,11 @@ static bool mpu6050GyroRead(int16_t *gyroADC) return true; } -void checkMPU6050Interrupt(bool *gyroIsUpdated) { - uint8_t mpuIntStatus; - - i2cRead(MPU6050_ADDRESS, MPU_RA_INT_STATUS, 1, &mpuIntStatus); - - delayMicroseconds(5); - - (mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); +void checkMPU6050DataReady(bool *mpuDataReadyPtr) { + if (mpuDataReady) { + *mpuDataReadyPtr = true; + mpuDataReady= false; + } else { + *mpuDataReadyPtr = false; + } } diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index c2c2a4b18..b8b6bedfd 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -31,16 +31,16 @@ extern gyro_t gyro; uint32_t targetLooptime; static uint8_t mpuDivider; -bool getMpuInterrupt(gyro_t *gyro) +bool getMpuDataStatus(gyro_t *gyro) { - bool gyroIsUpdated; + bool mpuDataStatus; - gyro->intStatus(&gyroIsUpdated); - return gyroIsUpdated; + gyro->intStatus(&mpuDataStatus); + return mpuDataStatus; } bool gyroSyncCheckUpdate(void) { - return getMpuInterrupt(&gyro); + return getMpuDataStatus(&gyro); } void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop) { diff --git a/src/main/mw.c b/src/main/mw.c index 808a204a9..f5069762a 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -92,7 +92,7 @@ enum { #define VBATINTERVAL (6 * 3500) /* IBat monitoring interval (in microseconds) - 6 default looptimes */ #define IBATINTERVAL (6 * 3500) -#define GYRO_WATCHDOG_DELAY 150 // Watchdog for boards without interrupt for gyro +#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro uint32_t currentTime = 0; uint32_t previousTime = 0; @@ -738,6 +738,21 @@ void filterRc(void){ } } +// Function for loop trigger +bool runLoop(uint32_t loopTime) { + bool loopTrigger = false; + + if (masterConfig.syncGyroToLoop) { + if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) { + loopTrigger = true; + } + } else if ((int32_t)(currentTime - loopTime) >= 0){ + loopTrigger = true; + } + + return loopTrigger; +} + void loop(void) { static uint32_t loopTime; @@ -784,7 +799,7 @@ void loop(void) } currentTime = micros(); - if ((!masterConfig.syncGyroToLoop && masterConfig.looptime == 0) || ((int32_t)(currentTime - loopTime) >= 0)) { + if (runLoop(loopTime)) { loopTime = currentTime + targetLooptime;