diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9ec2e64dd7..682fb05eb3 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -183,6 +183,23 @@ bool mpuGyroRead(gyroDev_t *gyro) return true; } +gyroOverflow_e mpuGyroCheckOverflow(const gyroDev_t *gyro) +{ + // we cannot detect overflow directly, so assume overflow if absolute gyro rate is large + gyroOverflow_e ret = GYRO_OVERFLOW_NONE; + const int16_t overflowValue = 0x7C00; // this is a slightly conservative value, could probably be as high as 0x7FF0 + if (gyro->gyroADCRaw[X] > overflowValue || gyro->gyroADCRaw[X] < -overflowValue) { + ret |= GYRO_OVERFLOW_X; + } + if (gyro->gyroADCRaw[Y] > overflowValue || gyro->gyroADCRaw[Y] < -overflowValue) { + ret |= GYRO_OVERFLOW_Y; + } + if (gyro->gyroADCRaw[Z] > overflowValue || gyro->gyroADCRaw[Z] < -overflowValue) { + ret |= GYRO_OVERFLOW_Z; + } + return ret; +} + bool mpuGyroReadSPI(gyroDev_t *gyro) { static const uint8_t dataToSend[7] = {MPU_RA_GYRO_XOUT_H | 0x80, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index 6e42fe2a44..f7c115511e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -211,6 +211,7 @@ typedef struct mpuDetectionResult_s { struct gyroDev_s; void mpuGyroInit(struct gyroDev_s *gyro); +gyroOverflow_e mpuGyroCheckOverflow(const struct gyroDev_s *gyro); bool mpuGyroRead(struct gyroDev_s *gyro); bool mpuGyroReadSPI(struct gyroDev_s *gyro); void mpuDetect(struct gyroDev_s *gyro); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 18b5d709de..550694a166 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -153,9 +153,6 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #define GYRO_SYNC_DENOM_DEFAULT 4 #endif -#define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980 // 97.5% full scale (1950dps) -#define GYRO_OVERFLOW_RESET_THRESHOLD 30340 // 92.5% full scale (1850dps) - PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, @@ -731,9 +728,13 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) // This can cause an overflow and sign reversal in the output. // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996. if (gyroSensor->overflowDetected) { - if (abs(gyroSensor->gyroDev.gyroADC[X]) < GYRO_OVERFLOW_RESET_THRESHOLD - && abs(gyroSensor->gyroDev.gyroADC[Y]) < GYRO_OVERFLOW_RESET_THRESHOLD - && abs(gyroSensor->gyroDev.gyroADC[Z]) < GYRO_OVERFLOW_RESET_THRESHOLD) { + const float gyroRateX = gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale; + const float gyroRateY = gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale; + const float gyroRateZ = gyroSensor->gyroDev.gyroADC[Z] * gyroSensor->gyroDev.scale; + static const int overflowResetThreshold = 1800; + if (abs(gyroRateX) < overflowResetThreshold + && abs(gyroRateY) < overflowResetThreshold + && abs(gyroRateZ) < overflowResetThreshold) { // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected // reset requires good OK values on all axes if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { @@ -743,25 +744,14 @@ static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) // not a consecutive OK value, so reset the overflow time gyroSensor->overflowTimeUs = currentTimeUs; } - } else { -#ifndef SIMULATOR_BUILD - // check for overflow in the axes set in overflowAxisMask - gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE; - if (abs(gyroSensor->gyroDev.gyroADC[X]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) { - overflowCheck |= GYRO_OVERFLOW_X; - } - if (abs(gyroSensor->gyroDev.gyroADC[Y]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) { - overflowCheck |= GYRO_OVERFLOW_Y; - } - if (abs(gyroSensor->gyroDev.gyroADC[Z]) > GYRO_OVERFLOW_TRIGGER_THRESHOLD) { - overflowCheck |= GYRO_OVERFLOW_Z; - } - if (overflowCheck & overflowAxisMask) { - gyroSensor->overflowDetected = true; - gyroSensor->overflowTimeUs = currentTimeUs; - } -#endif // SIMULATOR_BUILD } +#ifndef SIMULATOR_BUILD + // check for overflow in the axes set in overflowAxisMask + if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) & overflowAxisMask) { + gyroSensor->overflowDetected = true; + gyroSensor->overflowTimeUs = currentTimeUs; + } +#endif // SIMULATOR_BUILD #else UNUSED(gyroSensor); UNUSED(currentTimeUs); @@ -809,6 +799,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren accumulationLastTimeSampledUs = currentTimeUs; accumulatedMeasurementTimeUs += sampleDeltaUs; + if (gyroConfig()->checkOverflow) { + checkForOverflow(gyroSensor, currentTimeUs); + } if (gyroDebugMode == DEBUG_NONE) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch @@ -876,9 +869,6 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren } } } - if (gyroConfig()->checkOverflow) { - checkForOverflow(gyroSensor, currentTimeUs); - } } FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)