diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 9ec2e64dd..682fb05eb 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 46247082c..12fbfe7cf 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.h +++ b/src/main/drivers/accgyro/accgyro_mpu.h @@ -176,6 +176,13 @@ enum accel_fsr_e { NUM_ACCEL_FSR }; +typedef enum { + GYRO_OVERFLOW_NONE = 0x00, + GYRO_OVERFLOW_X = 0x01, + GYRO_OVERFLOW_Y = 0x02, + GYRO_OVERFLOW_Z = 0x04 +} gyroOverflow_e; + typedef enum { MPU_NONE, MPU_3050, @@ -204,8 +211,10 @@ typedef struct mpuDetectionResult_s { struct gyroDev_s; void mpuGyroInit(struct gyroDev_s *gyro); -struct accDev_s; -bool mpuAccRead(struct accDev_s *acc); +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); + +struct accDev_s; +bool mpuAccRead(struct accDev_s *acc); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ba32b5f70..0c5b8b0fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -440,21 +440,7 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT BEEP_ON; } if (axis == FD_YAW) { - // on yaw axis, prevent "yaw spin to the moon" after crash by constraining errorRate -#if !(defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_ICM20649)) -#define GYRO_POTENTIAL_OVERFLOW_RATE 1990.0f - if (gyroRate > GYRO_POTENTIAL_OVERFLOW_RATE || gyroRate < -GYRO_POTENTIAL_OVERFLOW_RATE) { - // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec. - // 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 there is a sign reversal we will actually increase crash-induced yaw spin - // so best thing to do is set error to zero. - errorRate = 0.0f; - } else -#endif - { - errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw); - } + errorRate = constrainf(errorRate, -crashLimitYaw, crashLimitYaw); } else { // on roll and pitch axes calculate currentPidSetpoint and errorRate to level the aircraft to recover from crash if (sensors(SENSOR_ACC)) { @@ -522,8 +508,9 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT previousRateError[axis] = rD; - // if crash recovery is on and accelerometer enabled then check for a crash - if (pidProfile->crash_recovery) { + // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash + // no point in trying to recover if the crash is so severe that the gyro overflows + if (pidProfile->crash_recovery && !gyroOverflowDetected()) { if (ARMING_FLAG(ARMED)) { if (motorMixRange >= 1.0f && !inCrashRecoveryMode && ABS(delta) > crashDtermThreshold @@ -545,8 +532,8 @@ FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchT axisPID_D[axis] = Kd[axis] * delta * tpaFactor; } - // Disable PID control at zero throttle - if (!pidStabilisationEnabled) { + // Disable PID control if at zero throttle or if gyro overflow detected + if (!pidStabilisationEnabled || gyroOverflowDetected()) { axisPID_P[axis] = 0; axisPID_I[axis] = 0; axisPID_D[axis] = 0; diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index d5244c41a..d38a6221c 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -335,6 +335,7 @@ const clivalue_t valueTable[] = { { "gyro_notch2_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_hz_2) }, { "gyro_notch2_cutoff", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_notch_cutoff_2) }, { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, + { "gyro_overflow_detect", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) }, #if defined(GYRO_USES_SPI) #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) { "gyro_use_32khz", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_use_32khz) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b071b5797..014d7853d 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -110,6 +110,8 @@ typedef struct gyroSensor_s { biquadFilter_t notchFilter2[XYZ_AXIS_COUNT]; filterApplyFnPtr notchFilterDynApplyFn; biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; + timeUs_t overflowTimeUs; + bool overflowDetected; } gyroSensor_t; static FAST_RAM gyroSensor_t gyroSensor1; @@ -127,6 +129,16 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); #define GYRO_SYNC_DENOM_DEFAULT 4 #endif +#if (defined(USE_GYRO_SPI_MPU6500) \ + || defined(USE_GYRO_SPI_MPU9250) \ + || defined(USE_GYRO_SPI_ICM20601) \ + || defined(USE_GYRO_SPI_ICM20649) \ + || defined(USE_GYRO_SPI_ICM20689)) +#define GYRO_CHECK_OVERFLOW_DEFAULT true +#else +#define GYRO_CHECK_OVERFLOW_DEFAULT false +#endif + PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 1); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, @@ -142,7 +154,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_hz_1 = 400, .gyro_soft_notch_cutoff_1 = 300, .gyro_soft_notch_hz_2 = 200, - .gyro_soft_notch_cutoff_2 = 100 + .gyro_soft_notch_cutoff_2 = 100, + .checkOverflow = GYRO_CHECK_OVERFLOW_DEFAULT ); @@ -607,16 +620,52 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t #if defined(USE_GYRO_SLEW_LIMITER) FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis) { - int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; - if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { - newRawGyro = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; - } else { - gyroSensor->gyroDev.gyroADCRawPrevious[axis] = newRawGyro; + int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis]; + if (gyroConfig()->checkOverflow) { + // don't use the slew limiter if overflow checking is on + return ret; } - return newRawGyro; + if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) { + // there has been a large change in value, so assume overflow has occurred and return the previous value + ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis]; + } else { + gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret; + } + return ret; } #endif +static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) +{ + // check for overflow to handle Yaw Spin To The Moon (YSTTM) + // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec. + // 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) { + const float gyroRateX = (float)gyroSensor->gyroDev.gyroADC[X] * gyroSensor->gyroDev.scale; + const float gyroRateY = (float)gyroSensor->gyroDev.gyroADC[Y] * gyroSensor->gyroDev.scale; + const float gyroRateZ = (float)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 + if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) { + gyroSensor->overflowDetected = false; + } + } else { + // not a consecutive OK value, so reset the overflow time + gyroSensor->overflowTimeUs = currentTimeUs; + } + } +#ifndef SIMULATOR_BUILD + if (mpuGyroCheckOverflow(&gyroSensor->gyroDev) != GYRO_OVERFLOW_NONE) { + gyroSensor->overflowDetected = true; + gyroSensor->overflowTimeUs = currentTimeUs; + } +#endif +} + static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs) { if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) { @@ -654,6 +703,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 @@ -665,9 +717,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; - // integrate using trapezium rule to avoid bias - accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; - gyroPrevious[axis] = gyroADCf; + if (!gyroSensor->overflowDetected) { + // integrate using trapezium rule to avoid bias + accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; + gyroPrevious[axis] = gyroADCf; + } } } else { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -699,9 +753,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; - // integrate using trapezium rule to avoid bias - accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; - gyroPrevious[axis] = gyroADCf; + if (!gyroSensor->overflowDetected) { + // integrate using trapezium rule to avoid bias + accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs; + gyroPrevious[axis] = gyroADCf; + } } } } @@ -745,3 +801,8 @@ int16_t gyroRateDps(int axis) { return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale); } + +bool gyroOverflowDetected(void) +{ + return gyroSensor1.overflowDetected; +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 803953a39..5db1a8148 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -63,6 +63,8 @@ typedef struct gyroConfig_s { uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; uint16_t gyro_soft_notch_cutoff_2; + uint16_t overflowResetThreshold; + bool checkOverflow; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -83,3 +85,4 @@ bool isGyroCalibrationComplete(void); void gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); +bool gyroOverflowDetected(void);