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