diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index ff90dd06c1..32f480f619 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -28,6 +28,7 @@ #include "sensor.h" #include "accgyro.h" #include "accgyro_mpu3050.h" +#include "gyro_sync.h" @@ -60,6 +61,7 @@ static uint8_t mpuLowPassFilter = MPU3050_DLPF_42HZ; static void mpu3050Init(void); static void mpu3050Read(int16_t *gyroADC); static void mpu3050ReadTemp(int16_t *tempData); +static void checkMPU3050Interrupt(bool *gyroIsUpdated); bool mpu3050Detect(gyro_t *gyro, uint16_t lpf) { @@ -74,6 +76,7 @@ bool mpu3050Detect(gyro_t *gyro, uint16_t lpf) gyro->init = mpu3050Init; gyro->read = mpu3050Read; gyro->temperature = mpu3050ReadTemp; + gyro->intStatus = checkMPU3050Interrupt; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; @@ -139,3 +142,7 @@ static void mpu3050ReadTemp(int16_t *tempData) *tempData = 35 + ((int32_t)(buf[0] << 8 | buf[1]) + 13200) / 280; } + +void checkMPU3050Interrupt(bool *gyroIsUpdated) { + // TODO - Without interrupt looptime watchdog will still make it work +} diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index e83c5d8889..4e51273060 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -31,6 +31,7 @@ #include "sensor.h" #include "accgyro.h" #include "accgyro_spi_mpu6500.h" +#include "gyro_sync.h" enum lpf_e { INV_FILTER_256HZ_NOLPF2 = 0, @@ -75,6 +76,7 @@ static void mpu6500AccInit(void); static void mpu6500AccRead(int16_t *accData); static void mpu6500GyroInit(void); static void mpu6500GyroRead(int16_t *gyroADC); +static void checkMPU6500Interrupt(bool *gyroIsUpdated); extern uint16_t acc_1G; @@ -167,6 +169,7 @@ bool mpu6500SpiGyroDetect(gyro_t *gyro, uint16_t lpf) gyro->init = mpu6500GyroInit; gyro->read = mpu6500GyroRead; + gyro->intStatus = checkMPU6500Interrupt; // 16.4 dps/lsb scalefactor gyro->scale = 1.0f / 16.4f; @@ -239,3 +242,11 @@ static void mpu6500GyroRead(int16_t *gyroADC) gyroADC[Y] = (int16_t)((buf[2] << 8) | buf[3]); gyroADC[Z] = (int16_t)((buf[4] << 8) | buf[5]); } + +void checkMPU6500Interrupt(bool *gyroIsUpdated) { + uint8_t mpuIntStatus; + + mpu6500ReadRegister(MPU6500_INT_STATUS, &mpuIntStatus, 1); + + (mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); +} diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 20f4e06b96..6e9ec78b94 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -25,6 +25,7 @@ #define MPU6500_RA_ACCEL_CFG (0x1C) #define MPU6500_RA_LPF (0x1A) #define MPU6500_RA_RATE_DIV (0x19) +#define MPU6500_INT_STATUS (0x38) #define MPU6500_WHO_AM_I_CONST (0x70)