diff --git a/src/main/drivers/accgyro_mpu3050.c b/src/main/drivers/accgyro_mpu3050.c index d6dd625049..4f642c2d6a 100644 --- a/src/main/drivers/accgyro_mpu3050.c +++ b/src/main/drivers/accgyro_mpu3050.c @@ -157,5 +157,9 @@ void checkMPU3050Interrupt(bool *gyroIsUpdated) { i2cRead(MPU3050_ADDRESS, MPU3050_INT_STATUS, 1, &mpuIntStatus); - (mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); + if (mpuIntStatus) { + *gyroIsUpdated = true; + } else { + *gyroIsUpdated = false; + } } diff --git a/src/main/drivers/accgyro_mpu6050.c b/src/main/drivers/accgyro_mpu6050.c index 0bc959a506..bcb876de38 100644 --- a/src/main/drivers/accgyro_mpu6050.c +++ b/src/main/drivers/accgyro_mpu6050.c @@ -167,7 +167,7 @@ static void mpu6050GyroInit(void); static bool mpu6050GyroRead(int16_t *gyroADC); static void checkMPU6050DataReady(bool *mpuDataReadyPtr); -static bool mpuDataReady; +static volatile bool mpuDataReady; typedef enum { MPU_6050_HALF_RESOLUTION, @@ -425,9 +425,8 @@ static void mpu6050GyroInit(void) ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x80); //PWR_MGMT_1 -- DEVICE_RESET 1 delay(100); ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_PWR_MGMT_1, 0x03); //PWR_MGMT_1 -- SLEEP 0; CYCLE 0; TEMP_DIS 0; CLKSEL 3 (PLL with Z Gyro reference) + ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); //SMPLRT_DIV -- SMPLRT_DIV = 7 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 = i2cWrite(MPU6050_ADDRESS, MPU_RA_SMPLRT_DIV, gyroMPU6xxxGetDivider()); //SMPLRT_DIV -- SMPLRT_DIV = 7 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 = i2cWrite(MPU6050_ADDRESS, MPU_RA_CONFIG, mpuLowPassFilter); //CONFIG -- EXT_SYNC_SET 0 (disable input pin for data sync) ; default DLPF_CFG = 0 => ACC bandwidth = 260Hz GYRO bandwidth = 256Hz) ack = i2cWrite(MPU6050_ADDRESS, MPU_RA_GYRO_CONFIG, INV_FSR_2000DPS << 3); //GYRO_CONFIG -- FS_SEL = 3: Full scale set to 2000 deg/sec diff --git a/src/main/drivers/accgyro_mpu6050.h b/src/main/drivers/accgyro_mpu6050.h index b4213c86a0..11213bec29 100644 --- a/src/main/drivers/accgyro_mpu6050.h +++ b/src/main/drivers/accgyro_mpu6050.h @@ -42,7 +42,7 @@ enum lpf_e { INV_FILTER_10HZ, INV_FILTER_5HZ, INV_FILTER_2100HZ_NOLPF, - NUM_FILTER + INV_FILTER_COUNT }; bool mpu6050AccDetect(const mpu6050Config_t *config,acc_t *acc); diff --git a/src/main/drivers/accgyro_spi_mpu6000.c b/src/main/drivers/accgyro_spi_mpu6000.c index 8200352bbd..8077db4799 100644 --- a/src/main/drivers/accgyro_spi_mpu6000.c +++ b/src/main/drivers/accgyro_spi_mpu6000.c @@ -298,7 +298,7 @@ bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf) spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER); // Determine the new sample divider - mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDivider()); + mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops()); delayMicroseconds(1); // Accel and Gyro DLPF Setting @@ -356,7 +356,9 @@ void checkMPU6000Interrupt(bool *gyroIsUpdated) { mpu6000ReadRegister(MPU6000_INT_STATUS, &mpuIntStatus, 1); - delayMicroseconds(5); - - (mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); + if (mpuIntStatus) { + *gyroIsUpdated = true; + } else { + *gyroIsUpdated = false; + } } diff --git a/src/main/drivers/accgyro_spi_mpu6500.c b/src/main/drivers/accgyro_spi_mpu6500.c index 4d298be9c4..0b19e3c7e5 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.c +++ b/src/main/drivers/accgyro_spi_mpu6500.c @@ -231,7 +231,7 @@ static void mpu6500GyroInit(void) mpu6500WriteRegister(MPU6500_RA_GYRO_CFG, INV_FSR_2000DPS << 3); mpu6500WriteRegister(MPU6500_RA_ACCEL_CFG, INV_FSR_8G << 3); mpu6500WriteRegister(MPU6500_RA_LPF, mpuLowPassFilter); - mpu6500WriteRegister(MPU6500_RA_RATE_DIV, 0); // 1kHz S/R + mpu6500WriteRegister(MPU6500_RA_RATE_DIV, gyroMPU6xxxGetDividerDrops()); // Get Divider drop count } static bool mpu6500GyroRead(int16_t *gyroADC) @@ -250,9 +250,11 @@ static bool mpu6500GyroRead(int16_t *gyroADC) void checkMPU6500Interrupt(bool *gyroIsUpdated) { uint8_t mpuIntStatus; - mpu6500ReadRegister(MPU6500_INT_STATUS, &mpuIntStatus, 1); + mpu6500ReadRegister(MPU6500_RA_INT_STATUS, &mpuIntStatus, 1); - delayMicroseconds(5); - - (mpuIntStatus) ? (*gyroIsUpdated= true) : (*gyroIsUpdated= false); + if (mpuIntStatus) { + *gyroIsUpdated = true; + } else { + *gyroIsUpdated = false; + } } diff --git a/src/main/drivers/accgyro_spi_mpu6500.h b/src/main/drivers/accgyro_spi_mpu6500.h index 6e9ec78b94..a2f8ad4ae0 100755 --- a/src/main/drivers/accgyro_spi_mpu6500.h +++ b/src/main/drivers/accgyro_spi_mpu6500.h @@ -25,7 +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_RA_INT_STATUS (0x38) #define MPU6500_WHO_AM_I_CONST (0x70) diff --git a/src/main/drivers/gyro_sync.c b/src/main/drivers/gyro_sync.c index b8b6bedfdf..3bd72d53d8 100644 --- a/src/main/drivers/gyro_sync.c +++ b/src/main/drivers/gyro_sync.c @@ -29,7 +29,7 @@ extern gyro_t gyro; uint32_t targetLooptime; -static uint8_t mpuDivider; +static uint8_t mpuDividerDrops; bool getMpuDataStatus(gyro_t *gyro) { @@ -48,52 +48,73 @@ void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop int minLooptime; if (syncGyroToLoop) { -#ifdef STM32F303xC +#if defined(SPRACINGF3) || defined(ALIENWIIF3) || defined(NAZE32PRO) || defined(STM32F3DISCOVERY) || defined(CHEBUZZF3) || defined(EUSTM32F103RC) || defined(PORT103R) || defined(MOTOLAB) if (lpf == INV_FILTER_256HZ_NOLPF2) { gyroSamplePeriod = 125; if(!sensors(SENSOR_ACC)) { minLooptime = 500; // Max refresh 2khz - } - else { + } else { minLooptime = 625; // Max refresh 1,6khz } - } - else { + } else { gyroSamplePeriod = 1000; minLooptime = 1000; // Full sampling } -#elif STM32F10X +#elif defined(CC3D) if (lpf == INV_FILTER_256HZ_NOLPF2) { gyroSamplePeriod = 125; if(!sensors(SENSOR_ACC)) { - minLooptime = 625; // Max refresh 1,33khz + minLooptime = 890; // Max refresh 1,12khz + } else { + minLooptime = 1000; // Max refresh 1khz } - else { + } else { + gyroSamplePeriod = 1000; + minLooptime = 1000; // Full sampling + } +#elif defined(COLIBRI_RACE) + if (lpf == INV_FILTER_256HZ_NOLPF2) { + gyroSamplePeriod = 125; + + if(!sensors(SENSOR_ACC)) { // TODO - increase to 8khz when oneshot125 can be limited + minLooptime = 250; // Max refresh 4khz + } else { + minLooptime = 250; // Max refresh 4khz + } + } else { + gyroSamplePeriod = 1000; + minLooptime = 1000; // Full sampling + } +#else + if (lpf == INV_FILTER_256HZ_NOLPF2) { + gyroSamplePeriod = 125; + + if(!sensors(SENSOR_ACC)) { + minLooptime = 625; // Max refresh 1,6khz + } else { minLooptime = 1625; // Max refresh 615hz } - } - else { + } else { gyroSamplePeriod = 1000; + if(!sensors(SENSOR_ACC)) { minLooptime = 1000; // Full sampling without ACC - } - else { + } else { minLooptime = 2000; } } #endif looptime = constrain(looptime, minLooptime, 4000); - mpuDivider = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1; - targetLooptime = (mpuDivider + 1) * gyroSamplePeriod; - } - else { - mpuDivider = 0; + mpuDividerDrops = (looptime + gyroSamplePeriod -1 ) / gyroSamplePeriod - 1; + targetLooptime = (mpuDividerDrops + 1) * gyroSamplePeriod; + } else { + mpuDividerDrops = 0; targetLooptime = looptime; } } -uint8_t gyroMPU6xxxGetDivider(void) { - return mpuDivider; +uint8_t gyroMPU6xxxGetDividerDrops(void) { + return mpuDividerDrops; } diff --git a/src/main/drivers/gyro_sync.h b/src/main/drivers/gyro_sync.h index 375d5df2dd..274b97ef40 100644 --- a/src/main/drivers/gyro_sync.h +++ b/src/main/drivers/gyro_sync.h @@ -8,5 +8,5 @@ extern uint32_t targetLooptime; bool gyroSyncCheckUpdate(void); -uint8_t gyroMPU6xxxGetDivider(void); +uint8_t gyroMPU6xxxGetDividerDrops(void); void gyroUpdateSampleRate(uint32_t looptime, uint8_t lpf, uint8_t syncGyroToLoop); diff --git a/src/main/mw.c b/src/main/mw.c index f5069762a1..8acf582099 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -746,7 +746,7 @@ bool runLoop(uint32_t loopTime) { if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) { loopTrigger = true; } - } else if ((int32_t)(currentTime - loopTime) >= 0){ + } else if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0){ loopTrigger = true; }