src/main/sensors/gyro.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24 #include <math.h>
  25 #include <stdlib.h>
  26
  27 #include "platform.h"
  28
  29 #include "build/debug.h"
  30
  31 #include "common/axis.h"
  32 #include "common/maths.h"
  33 #include "common/filter.h"
  34
  35 #include "config/feature.h"
  36 #include "config/simplified_tuning.h"
  37
  38 #include "pg/pg.h"
  39 #include "pg/pg_ids.h"
  40 #include "pg/gyrodev.h"
  41
  42 #include "drivers/bus_spi.h"
  43 #include "drivers/io.h"
  44
  45 #include "config/config.h"
  46 #include "fc/runtime_config.h"
  47
  48 #ifdef USE_DYN_NOTCH_FILTER
  49 #include "flight/dyn_notch_filter.h"
  50 #endif
  51 #include "flight/rpm_filter.h"
  52
  53 #include "io/beeper.h"
  54 #include "io/statusindicator.h"
  55
  56 #include "scheduler/scheduler.h"
  57
  58 #include "sensors/boardalignment.h"
  59 #include "sensors/gyro.h"
  60 #include "sensors/gyro_init.h"
  61
  62 #if ((TARGET_FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
  63 #define USE_GYRO_SLEW_LIMITER
  64 #endif
  65
  66 FAST_DATA_ZERO_INIT gyro_t gyro;
  67
  68 static FAST_DATA_ZERO_INIT bool overflowDetected;
  69 #ifdef USE_GYRO_OVERFLOW_CHECK
  70 static FAST_DATA_ZERO_INIT timeUs_t overflowTimeUs;
  71 #endif
  72
  73 #ifdef USE_YAW_SPIN_RECOVERY
  74 static FAST_DATA_ZERO_INIT bool yawSpinRecoveryEnabled;
  75 static FAST_DATA_ZERO_INIT int yawSpinRecoveryThreshold;
  76 static FAST_DATA_ZERO_INIT bool yawSpinDetected;
  77 static FAST_DATA_ZERO_INIT timeUs_t yawSpinTimeUs;
  78 #endif
  79
  80 static FAST_DATA_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
  81 static FAST_DATA_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
  82 static FAST_DATA_ZERO_INIT int accumulatedMeasurementCount;
  83
  84 static FAST_DATA_ZERO_INIT int16_t gyroSensorTemperature;
  85
  86 FAST_DATA uint8_t activePidLoopDenom = 1;
  87
  88 static bool firstArmingCalibrationWasStarted = false;
  89
  90 #ifdef UNIT_TEST
  91 STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyro.gyroSensor1;
  92 STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
  93 #endif
  94
  95
  96 #define DEBUG_GYRO_CALIBRATION 3
  97
  98 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
  99 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 100
 101 PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
 102
 103 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 104 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
 105 #endif
 106
 107 void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
 108 {
 109     gyroConfig->gyroCalibrationDuration = 125;        // 1.25 seconds
 110     gyroConfig->gyroMovementCalibrationThreshold = 48;
 111     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
 112     gyroConfig->gyro_lpf1_type = FILTER_PT1;
 113     gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
 114         // NOTE: dynamic lpf is enabled by default so this setting is actually
 115         // overridden and the static lowpass 1 is disabled. We can't set this
 116         // value to 0 otherwise Configurator versions 10.4 and earlier will also
 117         // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
 118     gyroConfig->gyro_lpf2_type = FILTER_PT1;
 119     gyroConfig->gyro_lpf2_static_hz = GYRO_LPF2_HZ_DEFAULT;
 120     gyroConfig->gyro_high_fsr = false;
 121     gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
 122     gyroConfig->gyro_soft_notch_hz_1 = 0;
 123     gyroConfig->gyro_soft_notch_cutoff_1 = 0;
 124     gyroConfig->gyro_soft_notch_hz_2 = 0;
 125     gyroConfig->gyro_soft_notch_cutoff_2 = 0;
 126     gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
 127     gyroConfig->gyro_offset_yaw = 0;
 128     gyroConfig->yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO;
 129     gyroConfig->yaw_spin_threshold = 1950;
 130     gyroConfig->gyro_lpf1_dyn_min_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
 131     gyroConfig->gyro_lpf1_dyn_max_hz = GYRO_LPF1_DYN_MAX_HZ_DEFAULT;
 132     gyroConfig->gyro_filter_debug_axis = FD_ROLL;
 133     gyroConfig->gyro_lpf1_dyn_expo = 5;
 134     gyroConfig->simplified_gyro_filter = true;
 135     gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
 136 }
 137
 138 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
 139 {
 140     return gyroSensor->calibration.cyclesRemaining == 0;
 141 }
 142
 143 FAST_CODE bool gyroIsCalibrationComplete(void)
 144 {
 145     switch (gyro.gyroToUse) {
 146         default:
 147         case GYRO_CONFIG_USE_GYRO_1: {
 148             return isGyroSensorCalibrationComplete(&gyro.gyroSensor1);
 149         }
 150 #ifdef USE_MULTI_GYRO
 151         case GYRO_CONFIG_USE_GYRO_2: {
 152             return isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
 153         }
 154         case GYRO_CONFIG_USE_GYRO_BOTH: {
 155             return isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
 156         }
 157 #endif
 158     }
 159 }
 160
 161 static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 162 {
 163     return gyroCalibration->cyclesRemaining == 1;
 164 }
 165
 166 static int32_t gyroCalculateCalibratingCycles(void)
 167 {
 168     return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.sampleLooptime;
 169 }
 170
 171 static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 172 {
 173     return gyroCalibration->cyclesRemaining == gyroCalculateCalibratingCycles();
 174 }
 175
 176 static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
 177 {
 178 #if defined(USE_FAKE_GYRO) && !defined(UNIT_TEST)
 179     if (gyroSensor->gyroDev.gyroHardware == GYRO_FAKE) {
 180         gyroSensor->calibration.cyclesRemaining = 0;
 181         return;
 182     }
 183 #endif
 184     gyroSensor->calibration.cyclesRemaining = gyroCalculateCalibratingCycles();
 185 }
 186
 187 void gyroStartCalibration(bool isFirstArmingCalibration)
 188 {
 189     if (isFirstArmingCalibration && firstArmingCalibrationWasStarted) {
 190         return;
 191     }
 192
 193     gyroSetCalibrationCycles(&gyro.gyroSensor1);
 194 #ifdef USE_MULTI_GYRO
 195     gyroSetCalibrationCycles(&gyro.gyroSensor2);
 196 #endif
 197
 198     if (isFirstArmingCalibration) {
 199         firstArmingCalibrationWasStarted = true;
 200     }
 201 }
 202
 203 bool isFirstArmingGyroCalibrationRunning(void)
 204 {
 205     return firstArmingCalibrationWasStarted && !gyroIsCalibrationComplete();
 206 }
 207
 208 STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
 209 {
 210     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 211         // Reset g[axis] at start of calibration
 212         if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
 213             gyroSensor->calibration.sum[axis] = 0.0f;
 214             devClear(&gyroSensor->calibration.var[axis]);
 215             // gyroZero is set to zero until calibration complete
 216             gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
 217         }
 218
 219         // Sum up CALIBRATING_GYRO_TIME_US readings
 220         gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
 221         devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
 222
 223         if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 224             const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
 225             // DEBUG_GYRO_CALIBRATION records the standard deviation of roll
 226             // into the spare field - debug[3], in DEBUG_GYRO_RAW
 227             if (axis == X) {
 228                 DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
 229             }
 230
 231             // check deviation and startover in case the model was moved
 232             if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
 233                 gyroSetCalibrationCycles(gyroSensor);
 234                 return;
 235             }
 236
 237             // please take care with exotic boardalignment !!
 238             gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
 239             if (axis == Z) {
 240               gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
 241             }
 242         }
 243     }
 244
 245     if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 246         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
 247         if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
 248             beeper(BEEPER_GYRO_CALIBRATED);
 249         }
 250     }
 251
 252     --gyroSensor->calibration.cyclesRemaining;
 253 }
 254
 255 #if defined(USE_GYRO_SLEW_LIMITER)
 256 FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 257 {
 258     int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
 259     if (gyroConfig()->checkOverflow || gyro.gyroHasOverflowProtection) {
 260         // don't use the slew limiter if overflow checking is on or gyro is not subject to overflow bug
 261         return ret;
 262     }
 263     if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
 264         // there has been a large change in value, so assume overflow has occurred and return the previous value
 265         ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
 266     } else {
 267         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
 268     }
 269     return ret;
 270 }
 271 #endif
 272
 273 #ifdef USE_GYRO_OVERFLOW_CHECK
 274 static FAST_CODE_NOINLINE void handleOverflow(timeUs_t currentTimeUs)
 275 {
 276     // This will need to be revised if we ever allow different sensor types to be
 277     // used simultaneously. In that case the scale might be different between sensors.
 278     // It's complicated by the fact that we're using filtered gyro data here which is
 279     // after both sensors are scaled and averaged.
 280     const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyro.scale;
 281
 282     if ((fabsf(gyro.gyroADCf[X]) < gyroOverflowResetRate)
 283           && (fabsf(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
 284           && (fabsf(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
 285         // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
 286         // reset requires good OK values on all axes
 287         if (cmpTimeUs(currentTimeUs, overflowTimeUs) > 50000) {
 288             overflowDetected = false;
 289         }
 290     } else {
 291         // not a consecutive OK value, so reset the overflow time
 292         overflowTimeUs = currentTimeUs;
 293     }
 294 }
 295
 296 static FAST_CODE void checkForOverflow(timeUs_t currentTimeUs)
 297 {
 298     // check for overflow to handle Yaw Spin To The Moon (YSTTM)
 299     // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
 300     // This can cause an overflow and sign reversal in the output.
 301     // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
 302     if (overflowDetected) {
 303         handleOverflow(currentTimeUs);
 304     } else {
 305 #ifndef SIMULATOR_BUILD
 306         // check for overflow in the axes set in overflowAxisMask
 307         gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
 308
 309         // This will need to be revised if we ever allow different sensor types to be
 310         // used simultaneously. In that case the scale might be different between sensors.
 311         // It's complicated by the fact that we're using filtered gyro data here which is
 312         // after both sensors are scaled and averaged.
 313         const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyro.scale;
 314
 315         if (fabsf(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) {
 316             overflowCheck |= GYRO_OVERFLOW_X;
 317         }
 318         if (fabsf(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) {
 319             overflowCheck |= GYRO_OVERFLOW_Y;
 320         }
 321         if (fabsf(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) {
 322             overflowCheck |= GYRO_OVERFLOW_Z;
 323         }
 324         if (overflowCheck & gyro.overflowAxisMask) {
 325             overflowDetected = true;
 326             overflowTimeUs = currentTimeUs;
 327 #ifdef USE_YAW_SPIN_RECOVERY
 328             yawSpinDetected = false;
 329 #endif // USE_YAW_SPIN_RECOVERY
 330         }
 331 #endif // SIMULATOR_BUILD
 332     }
 333 }
 334 #endif // USE_GYRO_OVERFLOW_CHECK
 335
 336 #ifdef USE_YAW_SPIN_RECOVERY
 337 static FAST_CODE_NOINLINE void handleYawSpin(timeUs_t currentTimeUs)
 338 {
 339     const float yawSpinResetRate = yawSpinRecoveryThreshold - 100.0f;
 340     if (fabsf(gyro.gyroADCf[Z]) < yawSpinResetRate) {
 341         // testing whether 20ms of consecutive OK gyro yaw values is enough
 342         if (cmpTimeUs(currentTimeUs, yawSpinTimeUs) > 20000) {
 343             yawSpinDetected = false;
 344         }
 345     } else {
 346         // reset the yaw spin time
 347         yawSpinTimeUs = currentTimeUs;
 348     }
 349 }
 350
 351 static FAST_CODE void checkForYawSpin(timeUs_t currentTimeUs)
 352 {
 353     // if not in overflow mode, handle yaw spins above threshold
 354 #ifdef USE_GYRO_OVERFLOW_CHECK
 355     if (overflowDetected) {
 356         yawSpinDetected = false;
 357         return;
 358     }
 359 #endif // USE_GYRO_OVERFLOW_CHECK
 360
 361     if (yawSpinDetected) {
 362         handleYawSpin(currentTimeUs);
 363     } else {
 364 #ifndef SIMULATOR_BUILD
 365         // check for spin on yaw axis only
 366          if (abs((int)gyro.gyroADCf[Z]) > yawSpinRecoveryThreshold) {
 367             yawSpinDetected = true;
 368             yawSpinTimeUs = currentTimeUs;
 369         }
 370 #endif // SIMULATOR_BUILD
 371     }
 372 }
 373 #endif // USE_YAW_SPIN_RECOVERY
 374
 375 static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
 376 {
 377     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
 378         return;
 379     }
 380     gyroSensor->gyroDev.dataReady = false;
 381
 382     if (isGyroSensorCalibrationComplete(gyroSensor)) {
 383         // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
 384
 385 #if defined(USE_GYRO_SLEW_LIMITER)
 386         gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
 387         gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
 388         gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
 389 #else
 390         gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
 391         gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
 392         gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
 393 #endif
 394
 395         if (gyroSensor->gyroDev.gyroAlign == ALIGN_CUSTOM) {
 396             alignSensorViaMatrix(gyroSensor->gyroDev.gyroADC, &gyroSensor->gyroDev.rotationMatrix);
 397         } else {
 398             alignSensorViaRotation(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
 399         }
 400     } else {
 401         performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
 402     }
 403 }
 404
 405 FAST_CODE void gyroUpdate(void)
 406 {
 407     switch (gyro.gyroToUse) {
 408     case GYRO_CONFIG_USE_GYRO_1:
 409         gyroUpdateSensor(&gyro.gyroSensor1);
 410         if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) {
 411             gyro.gyroADC[X] = gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale;
 412             gyro.gyroADC[Y] = gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale;
 413             gyro.gyroADC[Z] = gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale;
 414         }
 415         break;
 416 #ifdef USE_MULTI_GYRO
 417     case GYRO_CONFIG_USE_GYRO_2:
 418         gyroUpdateSensor(&gyro.gyroSensor2);
 419         if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
 420             gyro.gyroADC[X] = gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale;
 421             gyro.gyroADC[Y] = gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale;
 422             gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale;
 423         }
 424         break;
 425     case GYRO_CONFIG_USE_GYRO_BOTH:
 426         gyroUpdateSensor(&gyro.gyroSensor1);
 427         gyroUpdateSensor(&gyro.gyroSensor2);
 428         if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
 429             gyro.gyroADC[X] = ((gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
 430             gyro.gyroADC[Y] = ((gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
 431             gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
 432         }
 433         break;
 434 #endif
 435     }
 436
 437     if (gyro.downsampleFilterEnabled) {
 438         // using gyro lowpass 2 filter for downsampling
 439         gyro.sampleSum[X] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[X], gyro.gyroADC[X]);
 440         gyro.sampleSum[Y] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Y], gyro.gyroADC[Y]);
 441         gyro.sampleSum[Z] = gyro.lowpass2FilterApplyFn((filter_t *)&gyro.lowpass2Filter[Z], gyro.gyroADC[Z]);
 442     } else {
 443         // using simple averaging for downsampling
 444         gyro.sampleSum[X] += gyro.gyroADC[X];
 445         gyro.sampleSum[Y] += gyro.gyroADC[Y];
 446         gyro.sampleSum[Z] += gyro.gyroADC[Z];
 447         gyro.sampleCount++;
 448     }
 449 }
 450
 451 #define GYRO_FILTER_FUNCTION_NAME filterGyro
 452 #define GYRO_FILTER_DEBUG_SET(mode, index, value) do { UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
 453 #define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) do { UNUSED(axis); UNUSED(mode); UNUSED(index); UNUSED(value); } while (0)
 454 #include "gyro_filter_impl.c"
 455 #undef GYRO_FILTER_FUNCTION_NAME
 456 #undef GYRO_FILTER_DEBUG_SET
 457 #undef GYRO_FILTER_AXIS_DEBUG_SET
 458
 459 #define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
 460 #define GYRO_FILTER_DEBUG_SET DEBUG_SET
 461 #define GYRO_FILTER_AXIS_DEBUG_SET(axis, mode, index, value) if (axis == (int)gyro.gyroDebugAxis) DEBUG_SET(mode, index, value)
 462 #include "gyro_filter_impl.c"
 463 #undef GYRO_FILTER_FUNCTION_NAME
 464 #undef GYRO_FILTER_DEBUG_SET
 465 #undef GYRO_FILTER_AXIS_DEBUG_SET
 466
 467 FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
 468 {
 469     if (gyro.gyroDebugMode == DEBUG_NONE) {
 470         filterGyro();
 471     } else {
 472         filterGyroDebug();
 473     }
 474
 475 #ifdef USE_DYN_NOTCH_FILTER
 476     if (isDynNotchActive()) {
 477         dynNotchUpdate();
 478     }
 479 #endif
 480
 481     if (gyro.useDualGyroDebugging) {
 482         switch (gyro.gyroToUse) {
 483         case GYRO_CONFIG_USE_GYRO_1:
 484             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
 485             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
 486             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale));
 487             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale));
 488             break;
 489
 490 #ifdef USE_MULTI_GYRO
 491         case GYRO_CONFIG_USE_GYRO_2:
 492             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
 493             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
 494             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale));
 495             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale));
 496             break;
 497
 498     case GYRO_CONFIG_USE_GYRO_BOTH:
 499             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
 500             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
 501             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
 502             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
 503             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale));
 504             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale));
 505             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale));
 506             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale));
 507             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[X] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[X] * gyro.gyroSensor2.gyroDev.scale)));
 508             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[Y] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[Y] * gyro.gyroSensor2.gyroDev.scale)));
 509             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC[Z] * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC[Z] * gyro.gyroSensor2.gyroDev.scale)));
 510             break;
 511 #endif
 512         }
 513     }
 514
 515 #ifdef USE_GYRO_OVERFLOW_CHECK
 516     if (gyroConfig()->checkOverflow && !gyro.gyroHasOverflowProtection) {
 517         checkForOverflow(currentTimeUs);
 518     }
 519 #endif
 520
 521 #ifdef USE_YAW_SPIN_RECOVERY
 522     if (yawSpinRecoveryEnabled) {
 523         checkForYawSpin(currentTimeUs);
 524     }
 525 #endif
 526
 527     if (!overflowDetected) {
 528         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 529             // integrate using trapezium rule to avoid bias
 530             accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * gyro.targetLooptime;
 531             gyroPrevious[axis] = gyro.gyroADCf[axis];
 532         }
 533         accumulatedMeasurementCount++;
 534     }
 535
 536 #if !defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY)
 537     UNUSED(currentTimeUs);
 538 #endif
 539 }
 540
 541 bool gyroGetAccumulationAverage(float *accumulationAverage)
 542 {
 543     if (accumulatedMeasurementCount) {
 544         // If we have gyro data accumulated, calculate average rate that will yield the same rotation
 545         const timeUs_t accumulatedMeasurementTimeUs = accumulatedMeasurementCount * gyro.targetLooptime;
 546         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 547             accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
 548             accumulatedMeasurements[axis] = 0.0f;
 549         }
 550         accumulatedMeasurementCount = 0;
 551         return true;
 552     } else {
 553         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 554             accumulationAverage[axis] = 0.0f;
 555         }
 556         return false;
 557     }
 558 }
 559
 560 int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)
 561 {
 562     if (gyroSensor.gyroDev.temperatureFn) {
 563         gyroSensor.gyroDev.temperatureFn(&gyroSensor.gyroDev, &gyroSensor.gyroDev.temperature);
 564     }
 565     return gyroSensor.gyroDev.temperature;
 566 }
 567
 568 void gyroReadTemperature(void)
 569 {
 570     switch (gyro.gyroToUse) {
 571     case GYRO_CONFIG_USE_GYRO_1:
 572         gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor1);
 573         break;
 574
 575 #ifdef USE_MULTI_GYRO
 576     case GYRO_CONFIG_USE_GYRO_2:
 577         gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor2);
 578         break;
 579
 580     case GYRO_CONFIG_USE_GYRO_BOTH:
 581         gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyro.gyroSensor1), gyroReadSensorTemperature(gyro.gyroSensor2));
 582         break;
 583 #endif // USE_MULTI_GYRO
 584     }
 585 }
 586
 587 int16_t gyroGetTemperature(void)
 588 {
 589     return gyroSensorTemperature;
 590 }
 591
 592 bool gyroOverflowDetected(void)
 593 {
 594 #ifdef USE_GYRO_OVERFLOW_CHECK
 595     return overflowDetected;
 596 #else
 597     return false;
 598 #endif // USE_GYRO_OVERFLOW_CHECK
 599 }
 600
 601 #ifdef USE_YAW_SPIN_RECOVERY
 602 bool gyroYawSpinDetected(void)
 603 {
 604     return yawSpinDetected;
 605 }
 606 #endif // USE_YAW_SPIN_RECOVERY
 607
 608 uint16_t gyroAbsRateDps(int axis)
 609 {
 610     return fabsf(gyro.gyroADCf[axis]);
 611 }
 612
 613 #ifdef USE_DYN_LPF
 614
 615 float dynThrottle(float throttle) {
 616     return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
 617 }
 618
 619 void dynLpfGyroUpdate(float throttle)
 620 {
 621     if (gyro.dynLpfFilter != DYN_LPF_NONE) {
 622         float cutoffFreq;
 623         if (gyro.dynLpfCurveExpo > 0) {
 624             cutoffFreq = dynLpfCutoffFreq(throttle, gyro.dynLpfMin, gyro.dynLpfMax, gyro.dynLpfCurveExpo);
 625         } else {
 626             cutoffFreq = fmaxf(dynThrottle(throttle) * gyro.dynLpfMax, gyro.dynLpfMin);
 627         }
 628         DEBUG_SET(DEBUG_DYN_LPF, 2, lrintf(cutoffFreq));
 629         const float gyroDt = gyro.targetLooptime * 1e-6f;
 630         switch (gyro.dynLpfFilter) {
 631         case DYN_LPF_PT1:
 632             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 633                 pt1FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
 634             }
 635             break;
 636         case DYN_LPF_BIQUAD:
 637             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 638                 biquadFilterUpdateLPF(&gyro.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
 639             }
 640             break;
 641         case  DYN_LPF_PT2:
 642             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 643                 pt2FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt2FilterState, pt2FilterGain(cutoffFreq, gyroDt));
 644             }
 645             break;
 646         case DYN_LPF_PT3:
 647             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 648                 pt3FilterUpdateCutoff(&gyro.lowpassFilter[axis].pt3FilterState, pt3FilterGain(cutoffFreq, gyroDt));
 649             }
 650             break;
 651         }
 652     }
 653 }
 654 #endif
 655
 656 #ifdef USE_YAW_SPIN_RECOVERY
 657 void initYawSpinRecovery(int maxYawRate)
 658 {
 659     bool enabledFlag;
 660     int threshold;
 661
 662     switch (gyroConfig()->yaw_spin_recovery) {
 663     case YAW_SPIN_RECOVERY_ON:
 664         enabledFlag = true;
 665         threshold = gyroConfig()->yaw_spin_threshold;
 666         break;
 667     case YAW_SPIN_RECOVERY_AUTO:
 668         enabledFlag = true;
 669         const int overshootAllowance = MAX(maxYawRate / 4, 200); // Allow a 25% or minimum 200dps overshoot tolerance
 670         threshold = constrain(maxYawRate + overshootAllowance, YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX);
 671         break;
 672     case YAW_SPIN_RECOVERY_OFF:
 673     default:
 674         enabledFlag = false;
 675         threshold = YAW_SPIN_RECOVERY_THRESHOLD_MAX;
 676         break;
 677     }
 678
 679     yawSpinRecoveryEnabled = enabledFlag;
 680     yawSpinRecoveryThreshold = threshold;
 681 }
 682 #endif