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
  37 #include "pg/pg.h"
  38 #include "pg/pg_ids.h"
  39
  40 #include "drivers/accgyro/accgyro.h"
  41 #include "drivers/accgyro/accgyro_adxl345.h"
  42 #include "drivers/accgyro/accgyro_bma280.h"
  43 #include "drivers/accgyro/accgyro_fake.h"
  44 #include "drivers/accgyro/accgyro_l3g4200d.h"
  45 #include "drivers/accgyro/accgyro_l3gd20.h"
  46 #include "drivers/accgyro/accgyro_lsm303dlhc.h"
  47 #include "drivers/accgyro/accgyro_mma845x.h"
  48 #include "drivers/accgyro/accgyro_mpu.h"
  49 #include "drivers/accgyro/accgyro_mpu3050.h"
  50 #include "drivers/accgyro/accgyro_mpu6050.h"
  51 #include "drivers/accgyro/accgyro_mpu6500.h"
  52 #include "drivers/accgyro/accgyro_spi_bmi160.h"
  53 #include "drivers/accgyro/accgyro_spi_icm20649.h"
  54 #include "drivers/accgyro/accgyro_spi_icm20689.h"
  55 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
  56 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
  57 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
  58 #include "drivers/accgyro/gyro_sync.h"
  59 #include "drivers/bus_spi.h"
  60 #include "drivers/io.h"
  61
  62 #include "fc/config.h"
  63 #include "fc/runtime_config.h"
  64
  65 #include "io/beeper.h"
  66 #include "io/statusindicator.h"
  67
  68 #include "scheduler/scheduler.h"
  69
  70 #include "sensors/boardalignment.h"
  71 #include "sensors/gyro.h"
  72 #include "sensors/gyroanalyse.h"
  73 #include "sensors/sensors.h"
  74
  75 #ifdef USE_HARDWARE_REVISION_DETECTION
  76 #include "hardware_revision.h"
  77 #endif
  78
  79 #if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
  80 #define USE_GYRO_SLEW_LIMITER
  81 #endif
  82
  83 FAST_RAM gyro_t gyro;
  84 static FAST_RAM uint8_t gyroDebugMode;
  85
  86 static uint8_t gyroToUse = 0;
  87
  88 #ifdef USE_GYRO_OVERFLOW_CHECK
  89 static FAST_RAM uint8_t overflowAxisMask;
  90 #endif
  91 static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT];
  92 static FAST_RAM float gyroPrevious[XYZ_AXIS_COUNT];
  93 static FAST_RAM timeUs_t accumulatedMeasurementTimeUs;
  94 static FAST_RAM timeUs_t accumulationLastTimeSampledUs;
  95
  96 static bool gyroHasOverflowProtection = true;
  97
  98 typedef struct gyroCalibration_s {
  99     int32_t sum[XYZ_AXIS_COUNT];
 100     stdev_t var[XYZ_AXIS_COUNT];
 101     uint16_t calibratingG;
 102 } gyroCalibration_t;
 103
 104 bool firstArmingCalibrationWasStarted = false;
 105
 106 #if GYRO_LPF_ORDER_MAX > BIQUAD_LPF_ORDER_MAX
 107 #error "GYRO_LPF_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX"
 108 #endif
 109
 110 typedef union gyroLowpassFilter_u {
 111     pt1Filter_t pt1FilterState;
 112     biquadFilter_t biquadFilterState;
 113     biquadFilterCascade_t biquadFilterCascadeState;
 114     firFilterDenoise_t denoiseFilterState;
 115     fastKalman_t fastKalmanFilterState;
 116 } gyroLowpassFilter_t;
 117
 118 typedef struct gyroSensor_s {
 119     gyroDev_t gyroDev;
 120     gyroCalibration_t calibration;
 121
 122     // lowpass gyro soft filter
 123     filterApplyFnPtr lowpassFilterApplyFn;
 124     gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
 125
 126     // lowpass2 gyro soft filter
 127     filterApplyFnPtr lowpass2FilterApplyFn;
 128     gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
 129
 130     // lagged moving average smoothing
 131     filterApplyFnPtr lmaSmoothingApplyFn;
 132     laggedMovingAverage_t lmaSmoothingFilter[XYZ_AXIS_COUNT];
 133
 134     // notch filters
 135     filterApplyFnPtr notchFilter1ApplyFn;
 136     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
 137     filterApplyFnPtr notchFilter2ApplyFn;
 138     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 139     filterApplyFnPtr notchFilterDynApplyFn;
 140     biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
 141
 142     // overflow and recovery
 143     timeUs_t overflowTimeUs;
 144     bool overflowDetected;
 145 #ifdef USE_YAW_SPIN_RECOVERY
 146     timeUs_t yawSpinTimeUs;
 147     bool yawSpinDetected;
 148 #endif // USE_YAW_SPIN_RECOVERY
 149
 150 } gyroSensor_t;
 151
 152 STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1;
 153 #ifdef USE_DUAL_GYRO
 154 STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor2;
 155 #endif
 156
 157 #ifdef UNIT_TEST
 158 STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
 159 STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
 160 #endif
 161
 162 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 163 static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order);
 164
 165 #define DEBUG_GYRO_CALIBRATION 3
 166
 167 #ifdef STM32F10X
 168 #define GYRO_SYNC_DENOM_DEFAULT 8
 169 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 170    || defined(USE_GYRO_SPI_ICM20689)
 171 #define GYRO_SYNC_DENOM_DEFAULT 1
 172 #else
 173 #define GYRO_SYNC_DENOM_DEFAULT 4
 174 #endif
 175
 176 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 177 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 178
 179 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 2);
 180
 181 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 182 #ifdef USE_DUAL_GYRO
 183 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
 184 #else
 185 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
 186 #endif
 187 #endif
 188
 189 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
 190     .gyro_align = ALIGN_DEFAULT,
 191     .gyroMovementCalibrationThreshold = 48,
 192     .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
 193     .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
 194     .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
 195     .gyro_lowpass_type = FILTER_PT1,
 196     .gyro_lowpass_hz = 90,
 197     .gyro_lowpass_order = 1,
 198     .gyro_lowpass2_type = FILTER_PT1,
 199     .gyro_lowpass2_hz = 0,
 200     .gyro_lowpass2_order = 1,
 201     .gyro_high_fsr = false,
 202     .gyro_use_32khz = false,
 203     .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
 204     .gyro_soft_notch_hz_1 = 400,
 205     .gyro_soft_notch_cutoff_1 = 300,
 206     .gyro_soft_notch_hz_2 = 200,
 207     .gyro_soft_notch_cutoff_2 = 100,
 208     .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
 209     .gyro_offset_yaw = 0,
 210     .gyro_lma_depth = 0,
 211     .gyro_lma_weight = 100,
 212     .yaw_spin_recovery = true,
 213     .yaw_spin_threshold = 1950,
 214 );
 215
 216
 217 const busDevice_t *gyroSensorBus(void)
 218 {
 219 #ifdef USE_DUAL_GYRO
 220     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 221         return &gyroSensor2.gyroDev.bus;
 222     } else {
 223         return &gyroSensor1.gyroDev.bus;
 224     }
 225 #else
 226     return &gyroSensor1.gyroDev.bus;
 227 #endif
 228 }
 229
 230 const mpuConfiguration_t *gyroMpuConfiguration(void)
 231 {
 232 #ifdef USE_DUAL_GYRO
 233     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 234         return &gyroSensor2.gyroDev.mpuConfiguration;
 235     } else {
 236         return &gyroSensor1.gyroDev.mpuConfiguration;
 237     }
 238 #else
 239     return &gyroSensor1.gyroDev.mpuConfiguration;
 240 #endif
 241 }
 242
 243 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 244 {
 245 #ifdef USE_DUAL_GYRO
 246     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 247         return &gyroSensor2.gyroDev.mpuDetectionResult;
 248     } else {
 249         return &gyroSensor1.gyroDev.mpuDetectionResult;
 250     }
 251 #else
 252     return &gyroSensor1.gyroDev.mpuDetectionResult;
 253 #endif
 254 }
 255
 256 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
 257 {
 258     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 259
 260     switch (gyroHardware) {
 261     case GYRO_DEFAULT:
 262         FALLTHROUGH;
 263
 264 #ifdef USE_GYRO_MPU6050
 265     case GYRO_MPU6050:
 266         if (mpu6050GyroDetect(dev)) {
 267             gyroHardware = GYRO_MPU6050;
 268 #ifdef GYRO_MPU6050_ALIGN
 269             dev->gyroAlign = GYRO_MPU6050_ALIGN;
 270 #endif
 271             break;
 272         }
 273         FALLTHROUGH;
 274 #endif
 275
 276 #ifdef USE_GYRO_L3G4200D
 277     case GYRO_L3G4200D:
 278         if (l3g4200dDetect(dev)) {
 279             gyroHardware = GYRO_L3G4200D;
 280 #ifdef GYRO_L3G4200D_ALIGN
 281             dev->gyroAlign = GYRO_L3G4200D_ALIGN;
 282 #endif
 283             break;
 284         }
 285         FALLTHROUGH;
 286 #endif
 287
 288 #ifdef USE_GYRO_MPU3050
 289     case GYRO_MPU3050:
 290         if (mpu3050Detect(dev)) {
 291             gyroHardware = GYRO_MPU3050;
 292 #ifdef GYRO_MPU3050_ALIGN
 293             dev->gyroAlign = GYRO_MPU3050_ALIGN;
 294 #endif
 295             break;
 296         }
 297         FALLTHROUGH;
 298 #endif
 299
 300 #ifdef USE_GYRO_L3GD20
 301     case GYRO_L3GD20:
 302         if (l3gd20Detect(dev)) {
 303             gyroHardware = GYRO_L3GD20;
 304 #ifdef GYRO_L3GD20_ALIGN
 305             dev->gyroAlign = GYRO_L3GD20_ALIGN;
 306 #endif
 307             break;
 308         }
 309         FALLTHROUGH;
 310 #endif
 311
 312 #ifdef USE_GYRO_SPI_MPU6000
 313     case GYRO_MPU6000:
 314         if (mpu6000SpiGyroDetect(dev)) {
 315             gyroHardware = GYRO_MPU6000;
 316 #ifdef GYRO_MPU6000_ALIGN
 317             dev->gyroAlign = GYRO_MPU6000_ALIGN;
 318 #endif
 319             break;
 320         }
 321         FALLTHROUGH;
 322 #endif
 323
 324 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 325     case GYRO_MPU6500:
 326     case GYRO_ICM20601:
 327     case GYRO_ICM20602:
 328     case GYRO_ICM20608G:
 329 #ifdef USE_GYRO_SPI_MPU6500
 330         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 331 #else
 332         if (mpu6500GyroDetect(dev)) {
 333 #endif
 334             switch (dev->mpuDetectionResult.sensor) {
 335             case MPU_9250_SPI:
 336                 gyroHardware = GYRO_MPU9250;
 337                 break;
 338             case ICM_20601_SPI:
 339                 gyroHardware = GYRO_ICM20601;
 340                 break;
 341             case ICM_20602_SPI:
 342                 gyroHardware = GYRO_ICM20602;
 343                 break;
 344             case ICM_20608_SPI:
 345                 gyroHardware = GYRO_ICM20608G;
 346                 break;
 347             default:
 348                 gyroHardware = GYRO_MPU6500;
 349             }
 350 #ifdef GYRO_MPU6500_ALIGN
 351             dev->gyroAlign = GYRO_MPU6500_ALIGN;
 352 #endif
 353             break;
 354         }
 355         FALLTHROUGH;
 356 #endif
 357
 358 #ifdef USE_GYRO_SPI_MPU9250
 359     case GYRO_MPU9250:
 360         if (mpu9250SpiGyroDetect(dev)) {
 361             gyroHardware = GYRO_MPU9250;
 362 #ifdef GYRO_MPU9250_ALIGN
 363             dev->gyroAlign = GYRO_MPU9250_ALIGN;
 364 #endif
 365             break;
 366         }
 367         FALLTHROUGH;
 368 #endif
 369
 370 #ifdef USE_GYRO_SPI_ICM20649
 371     case GYRO_ICM20649:
 372         if (icm20649SpiGyroDetect(dev)) {
 373             gyroHardware = GYRO_ICM20649;
 374 #ifdef GYRO_ICM20649_ALIGN
 375             dev->gyroAlign = GYRO_ICM20649_ALIGN;
 376 #endif
 377             break;
 378         }
 379         FALLTHROUGH;
 380 #endif
 381
 382 #ifdef USE_GYRO_SPI_ICM20689
 383     case GYRO_ICM20689:
 384         if (icm20689SpiGyroDetect(dev)) {
 385             gyroHardware = GYRO_ICM20689;
 386 #ifdef GYRO_ICM20689_ALIGN
 387             dev->gyroAlign = GYRO_ICM20689_ALIGN;
 388 #endif
 389             break;
 390         }
 391         FALLTHROUGH;
 392 #endif
 393
 394 #ifdef USE_ACCGYRO_BMI160
 395     case GYRO_BMI160:
 396         if (bmi160SpiGyroDetect(dev)) {
 397             gyroHardware = GYRO_BMI160;
 398 #ifdef GYRO_BMI160_ALIGN
 399             dev->gyroAlign = GYRO_BMI160_ALIGN;
 400 #endif
 401             break;
 402         }
 403         FALLTHROUGH;
 404 #endif
 405
 406 #ifdef USE_FAKE_GYRO
 407     case GYRO_FAKE:
 408         if (fakeGyroDetect(dev)) {
 409             gyroHardware = GYRO_FAKE;
 410             break;
 411         }
 412         FALLTHROUGH;
 413 #endif
 414
 415     default:
 416         gyroHardware = GYRO_NONE;
 417     }
 418
 419     if (gyroHardware != GYRO_NONE) {
 420         detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 421         sensorsSet(SENSOR_GYRO);
 422     }
 423
 424
 425     return gyroHardware;
 426 }
 427
 428 static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 429 {
 430     gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
 431
 432 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 433  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
 434
 435     mpuDetect(&gyroSensor->gyroDev);
 436     mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
 437 #endif
 438
 439     const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 440     if (gyroHardware == GYRO_NONE) {
 441         return false;
 442     }
 443
 444     switch (gyroHardware) {
 445     case GYRO_MPU6500:
 446     case GYRO_MPU9250:
 447     case GYRO_ICM20601:
 448     case GYRO_ICM20602:
 449     case GYRO_ICM20608G:
 450     case GYRO_ICM20689:
 451         // do nothing, as gyro supports 32kHz
 452         break;
 453     default:
 454         // gyro does not support 32kHz
 455         gyroConfigMutable()->gyro_use_32khz = false;
 456         break;
 457     }
 458
 459     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
 460     gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
 461     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
 462     gyroSensor->gyroDev.hardware_32khz_lpf = gyroConfig()->gyro_32khz_hardware_lpf;
 463     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 464     if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
 465         gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align;
 466     }
 467
 468     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
 469     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
 470     switch (gyroHardware) {
 471     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
 472     case GYRO_DEFAULT:
 473     case GYRO_FAKE:
 474     case GYRO_MPU6050:
 475     case GYRO_L3G4200D:
 476     case GYRO_MPU3050:
 477     case GYRO_L3GD20:
 478     case GYRO_BMI160:
 479     case GYRO_MPU6000:
 480     case GYRO_MPU6500:
 481     case GYRO_MPU9250:
 482         gyroSensor->gyroDev.gyroHasOverflowProtection = true;
 483         break;
 484
 485     case GYRO_ICM20601:
 486     case GYRO_ICM20602:
 487     case GYRO_ICM20608G:
 488     case GYRO_ICM20649:  // we don't actually know if this is affected, but as there are currently no flight controllers using it we err on the side of caution
 489     case GYRO_ICM20689:
 490         gyroSensor->gyroDev.gyroHasOverflowProtection = false;
 491         break;
 492
 493     default:
 494         gyroSensor->gyroDev.gyroHasOverflowProtection = false;  // default catch for newly added gyros until proven to be unaffected
 495         break;
 496     }
 497
 498     gyroInitSensorFilters(gyroSensor);
 499
 500 #ifdef USE_GYRO_DATA_ANALYSE
 501     gyroDataAnalyseInit(gyro.targetLooptime);
 502 #endif
 503     return true;
 504 }
 505
 506 bool gyroInit(void)
 507 {
 508 #ifdef USE_GYRO_OVERFLOW_CHECK
 509     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 510         overflowAxisMask = GYRO_OVERFLOW_Z;
 511     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 512         overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 513     } else {
 514         overflowAxisMask = 0;
 515     }
 516 #endif
 517
 518     switch (debugMode) {
 519     case DEBUG_FFT:
 520     case DEBUG_GYRO_NOTCH:
 521     case DEBUG_GYRO:
 522     case DEBUG_GYRO_RAW:
 523         gyroDebugMode = debugMode;
 524         break;
 525     default:
 526         // debugMode is not gyro-related
 527         gyroDebugMode = DEBUG_NONE;
 528         break;
 529     }
 530     firstArmingCalibrationWasStarted = false;
 531
 532     bool ret = false;
 533     memset(&gyro, 0, sizeof(gyro));
 534     gyroToUse = gyroConfig()->gyro_to_use;
 535
 536 #if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN)
 537     if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 538         gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
 539         IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(0));
 540         IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
 541         IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
 542     }
 543 #endif
 544
 545 #if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN)
 546     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 547         gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN));
 548         IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(1));
 549         IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
 550         IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
 551     }
 552 #endif
 553
 554     gyroSensor1.gyroDev.gyroAlign = ALIGN_DEFAULT;
 555
 556 #if defined(GYRO_1_EXTI_PIN)
 557     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG(GYRO_1_EXTI_PIN);
 558 #elif defined(MPU_INT_EXTI)
 559     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG(MPU_INT_EXTI);
 560 #elif defined(USE_HARDWARE_REVISION_DETECTION)
 561     gyroSensor1.gyroDev.mpuIntExtiTag =  selectMPUIntExtiConfigByHardwareRevision();
 562 #else
 563     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG_NONE;
 564 #endif // GYRO_1_EXTI_PIN
 565 #ifdef USE_DUAL_GYRO
 566 #ifdef GYRO_1_ALIGN
 567     gyroSensor1.gyroDev.gyroAlign = GYRO_1_ALIGN;
 568 #endif
 569     gyroSensor1.gyroDev.bus.bustype = BUSTYPE_SPI;
 570     spiBusSetInstance(&gyroSensor1.gyroDev.bus, GYRO_1_SPI_INSTANCE);
 571     if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 572         ret = gyroInitSensor(&gyroSensor1);
 573         if (!ret) {
 574             return false; // TODO handle failure of first gyro detection better. - Perhaps update the config to use second gyro then indicate a new failure mode and reboot.
 575         }
 576         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
 577     }
 578 #else // USE_DUAL_GYRO
 579     ret = gyroInitSensor(&gyroSensor1);
 580     gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
 581 #endif // USE_DUAL_GYRO
 582
 583 #ifdef USE_DUAL_GYRO
 584
 585     gyroSensor2.gyroDev.gyroAlign = ALIGN_DEFAULT;
 586
 587 #if defined(GYRO_2_EXTI_PIN)
 588     gyroSensor2.gyroDev.mpuIntExtiTag =  IO_TAG(GYRO_2_EXTI_PIN);
 589 #elif defined(USE_HARDWARE_REVISION_DETECTION)
 590     gyroSensor2.gyroDev.mpuIntExtiTag =  selectMPUIntExtiConfigByHardwareRevision();
 591 #else
 592     gyroSensor2.gyroDev.mpuIntExtiTag =  IO_TAG_NONE;
 593 #endif // GYRO_2_EXTI_PIN
 594 #ifdef GYRO_2_ALIGN
 595     gyroSensor2.gyroDev.gyroAlign = GYRO_2_ALIGN;
 596 #endif
 597     gyroSensor2.gyroDev.bus.bustype = BUSTYPE_SPI;
 598     spiBusSetInstance(&gyroSensor2.gyroDev.bus, GYRO_2_SPI_INSTANCE);
 599     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 600         ret = gyroInitSensor(&gyroSensor2);
 601         if (!ret) {
 602             return false; // TODO handle failure of second gyro detection better. - Perhaps update the config to use first gyro then indicate a new failure mode and reboot.
 603         }
 604         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
 605     }
 606 #endif
 607     return ret;
 608 }
 609
 610 void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz, int order)
 611 {
 612     filterApplyFnPtr *lowpassFilterApplyFn;
 613     gyroLowpassFilter_t *lowpassFilter = NULL;
 614
 615     switch (slot) {
 616     case FILTER_LOWPASS:
 617         lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
 618         lowpassFilter = gyroSensor->lowpassFilter;
 619         break;
 620
 621     case FILTER_LOWPASS2:
 622         lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
 623         lowpassFilter = gyroSensor->lowpass2Filter;
 624         break;
 625
 626     default:
 627         return;
 628     }
 629
 630     // Establish some common constants
 631     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 632     const float gyroDt = gyro.targetLooptime * 1e-6f;
 633
 634     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
 635     const float gain = pt1FilterGain(lpfHz, gyroDt);
 636
 637     // Dereference the pointer to null before checking valid cutoff and filter
 638     // type. It will be overridden for positive cases.
 639     *lowpassFilterApplyFn = &nullFilterApply;
 640
 641     // If lowpass cutoff has been specified and is less than the Nyquist frequency
 642     if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
 643         switch (type) {
 644         case FILTER_PT1:
 645             *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
 646             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 647                 pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
 648             }
 649             break;
 650         case FILTER_BIQUAD:
 651             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 652             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 653                 biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
 654             }
 655             break;
 656         case FILTER_BIQUAD_RC_FIR2:
 657             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 658             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 659                 biquadRCFIR2FilterInit(&lowpassFilter[axis].biquadFilterState, gain);
 660             }
 661             break;
 662         case FILTER_BUTTERWORTH:
 663             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadCascadeFilterApply;
 664             if (order <= GYRO_LPF_ORDER_MAX) {
 665                 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 666                     int *sections = &lowpassFilter[axis].biquadFilterCascadeState.sections;
 667                     biquadFilter_t *biquadSections = lowpassFilter[axis].biquadFilterCascadeState.biquad;
 668                     *sections = biquadFilterLpfCascadeInit(biquadSections, order, lpfHz, gyro.targetLooptime);
 669                 }
 670             }
 671             break;
 672         case FILTER_FAST_KALMAN:
 673             *lowpassFilterApplyFn = (filterApplyFnPtr) fastKalmanUpdate;
 674             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 675                 fastKalmanInit(&lowpassFilter[axis].fastKalmanFilterState, gain / 2);
 676             }
 677             break;
 678 #if defined(USE_FIR_FILTER_DENOISE)
 679         case FILTER_FIR:
 680             *lowpassFilterApplyFn = (filterApplyFnPtr) firFilterDenoiseUpdate;
 681             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 682                 firFilterDenoiseInit(&lowpassFilter[axis].denoiseFilterState, lpfHz, gyro.targetLooptime);
 683             }
 684             break;
 685
 686 #endif
 687         }
 688     }
 689 }
 690
 691 static void gyroInitLmaSmoothing(gyroSensor_t *gyroSensor, uint8_t depth, uint8_t weight)
 692 {
 693     gyroSensor->lmaSmoothingApplyFn = nullFilterApply;
 694
 695     if (depth && weight) {
 696         const uint8_t windowSize = depth + 1;
 697         const float lmaWeight = weight * 0.01f;
 698         gyroSensor->lmaSmoothingApplyFn = (filterApplyFnPtr)lmaSmoothingUpdate;
 699         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 700             lmaSmoothingInit(&gyroSensor->lmaSmoothingFilter[axis], windowSize, lmaWeight);
 701         }
 702     }
 703 }
 704
 705 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
 706 {
 707     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 708     if (notchHz > gyroFrequencyNyquist) {
 709         if (notchCutoffHz < gyroFrequencyNyquist) {
 710             notchHz = gyroFrequencyNyquist;
 711         } else {
 712             notchHz = 0;
 713         }
 714     }
 715
 716     return notchHz;
 717 }
 718
 719 #if defined(USE_GYRO_SLEW_LIMITER)
 720 void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
 721
 722     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 723         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
 724     }
 725 }
 726 #endif
 727
 728 static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 729 {
 730     gyroSensor->notchFilter1ApplyFn = nullFilterApply;
 731
 732     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 733
 734     if (notchHz != 0 && notchCutoffHz != 0) {
 735         gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 736         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 737         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 738             biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 739         }
 740     }
 741 }
 742
 743 static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 744 {
 745     gyroSensor->notchFilter2ApplyFn = nullFilterApply;
 746
 747     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 748
 749     if (notchHz != 0 && notchCutoffHz != 0) {
 750         gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 751         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 752         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 753             biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 754         }
 755     }
 756 }
 757
 758 #ifdef USE_GYRO_DATA_ANALYSE
 759 static bool isDynamicFilterActive(void)
 760 {
 761     return feature(FEATURE_DYNAMIC_FILTER);
 762 }
 763
 764 static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
 765 {
 766     gyroSensor->notchFilterDynApplyFn = nullFilterApply;
 767
 768     if (isDynamicFilterActive()) {
 769         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
 770         const float notchQ = filterGetNotchQ(400, 390); //just any init value
 771         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 772             biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 773         }
 774     }
 775 }
 776 #endif
 777
 778
 779 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 780 {
 781 #if defined(USE_GYRO_SLEW_LIMITER)
 782     gyroInitSlewLimiter(gyroSensor);
 783 #endif
 784
 785     gyroInitLowpassFilterLpf(
 786       gyroSensor,
 787       FILTER_LOWPASS,
 788       gyroConfig()->gyro_lowpass_type,
 789       gyroConfig()->gyro_lowpass_hz,
 790       gyroConfig()->gyro_lowpass_order
 791     );
 792
 793     gyroInitLowpassFilterLpf(
 794       gyroSensor,
 795       FILTER_LOWPASS2,
 796       gyroConfig()->gyro_lowpass2_type,
 797       gyroConfig()->gyro_lowpass2_hz,
 798       gyroConfig()->gyro_lowpass2_order
 799     );
 800
 801     gyroInitLmaSmoothing(gyroSensor, gyroConfig()->gyro_lma_depth, gyroConfig()->gyro_lma_weight);
 802
 803     gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
 804     gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 805 #ifdef USE_GYRO_DATA_ANALYSE
 806     gyroInitFilterDynamicNotch(gyroSensor);
 807 #endif
 808 }
 809
 810 void gyroInitFilters(void)
 811 {
 812     gyroInitSensorFilters(&gyroSensor1);
 813 #ifdef USE_DUAL_GYRO
 814     gyroInitSensorFilters(&gyroSensor2);
 815 #endif
 816 }
 817
 818 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
 819 {
 820     return gyroSensor->calibration.calibratingG == 0;
 821 }
 822
 823 FAST_CODE bool isGyroCalibrationComplete(void)
 824 {
 825 #ifdef USE_DUAL_GYRO
 826     switch (gyroToUse) {
 827         default:
 828         case GYRO_CONFIG_USE_GYRO_1: {
 829             return isGyroSensorCalibrationComplete(&gyroSensor1);
 830         }
 831         case GYRO_CONFIG_USE_GYRO_2: {
 832             return isGyroSensorCalibrationComplete(&gyroSensor2);
 833         }
 834         case GYRO_CONFIG_USE_GYRO_BOTH: {
 835             return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2);
 836         }
 837     }
 838 #else
 839     return isGyroSensorCalibrationComplete(&gyroSensor1);
 840 #endif
 841 }
 842
 843 static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 844 {
 845     return gyroCalibration->calibratingG == 1;
 846 }
 847
 848 static uint16_t gyroCalculateCalibratingCycles(void)
 849 {
 850     return (CALIBRATING_GYRO_TIME_US / gyro.targetLooptime);
 851 }
 852
 853 static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 854 {
 855     return gyroCalibration->calibratingG == gyroCalculateCalibratingCycles();
 856 }
 857
 858 static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
 859 {
 860     gyroSensor->calibration.calibratingG = gyroCalculateCalibratingCycles();
 861 }
 862
 863 void gyroStartCalibration(bool isFirstArmingCalibration)
 864 {
 865     if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) {
 866         gyroSetCalibrationCycles(&gyroSensor1);
 867 #ifdef USE_DUAL_GYRO
 868         gyroSetCalibrationCycles(&gyroSensor2);
 869 #endif
 870
 871         if (isFirstArmingCalibration) {
 872             firstArmingCalibrationWasStarted = true;
 873         }
 874     }
 875 }
 876
 877 bool isFirstArmingGyroCalibrationRunning(void)
 878 {
 879     return firstArmingCalibrationWasStarted && !isGyroCalibrationComplete();
 880 }
 881
 882 STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
 883 {
 884     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 885         // Reset g[axis] at start of calibration
 886         if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
 887             gyroSensor->calibration.sum[axis] = 0;
 888             devClear(&gyroSensor->calibration.var[axis]);
 889             // gyroZero is set to zero until calibration complete
 890             gyroSensor->gyroDev.gyroZero[axis] = 0;
 891         }
 892
 893         // Sum up CALIBRATING_GYRO_TIME_US readings
 894         gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
 895         devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
 896
 897         if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 898             const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
 899
 900             DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
 901
 902             // check deviation and startover in case the model was moved
 903             if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
 904                 gyroSetCalibrationCycles(gyroSensor);
 905                 return;
 906             }
 907
 908             // please take care with exotic boardalignment !!
 909             gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
 910             if (axis == Z) {
 911               gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
 912             }
 913         }
 914     }
 915
 916     if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 917         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
 918         if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
 919             beeper(BEEPER_GYRO_CALIBRATED);
 920         }
 921     }
 922     --gyroSensor->calibration.calibratingG;
 923
 924 }
 925
 926 #if defined(USE_GYRO_SLEW_LIMITER)
 927 FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 928 {
 929     int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
 930     if (gyroConfig()->checkOverflow || gyroHasOverflowProtection) {
 931         // don't use the slew limiter if overflow checking is on or gyro is not subject to overflow bug
 932         return ret;
 933     }
 934     if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
 935         // there has been a large change in value, so assume overflow has occurred and return the previous value
 936         ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
 937     } else {
 938         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
 939     }
 940     return ret;
 941 }
 942 #endif
 943
 944 static void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 945 {
 946 #ifdef USE_GYRO_OVERFLOW_CHECK
 947     // check for overflow to handle Yaw Spin To The Moon (YSTTM)
 948     // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
 949     // This can cause an overflow and sign reversal in the output.
 950     // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
 951     if (gyroSensor->overflowDetected) {
 952         const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
 953         if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
 954               && (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
 955               && (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
 956             // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
 957             // reset requires good OK values on all axes
 958             if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
 959                 gyroSensor->overflowDetected = false;
 960             }
 961         } else {
 962             // not a consecutive OK value, so reset the overflow time
 963             gyroSensor->overflowTimeUs = currentTimeUs;
 964         }
 965     } else {
 966 #ifndef SIMULATOR_BUILD
 967         // check for overflow in the axes set in overflowAxisMask
 968         gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
 969         const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale;
 970         if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) {
 971             overflowCheck |= GYRO_OVERFLOW_X;
 972         }
 973         if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) {
 974             overflowCheck |= GYRO_OVERFLOW_Y;
 975         }
 976         if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) {
 977             overflowCheck |= GYRO_OVERFLOW_Z;
 978         }
 979         if (overflowCheck & overflowAxisMask) {
 980             gyroSensor->overflowDetected = true;
 981             gyroSensor->overflowTimeUs = currentTimeUs;
 982 #ifdef USE_YAW_SPIN_RECOVERY
 983             gyroSensor->yawSpinDetected = false;
 984 #endif // USE_YAW_SPIN_RECOVERY
 985         }
 986 #endif // SIMULATOR_BUILD
 987     }
 988 #else
 989     UNUSED(gyroSensor);
 990     UNUSED(currentTimeUs);
 991 #endif // USE_GYRO_OVERFLOW_CHECK
 992 }
 993
 994 static void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 995 {
 996 #ifdef USE_YAW_SPIN_RECOVERY
 997     // if not in overflow mode, handle yaw spins above threshold
 998 #ifdef USE_GYRO_OVERFLOW_CHECK
 999     if (gyroSensor->overflowDetected) {
1000         gyroSensor->yawSpinDetected = false;
1001         return;
1002     }
1003 #endif // USE_GYRO_OVERFLOW_CHECK
1004     if (gyroSensor->yawSpinDetected) {
1005         const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
1006         if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
1007             // testing whether 20ms of consecutive OK gyro yaw values is enough
1008             if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
1009                 gyroSensor->yawSpinDetected = false;
1010             }
1011         } else {
1012             // reset the yaw spin time
1013             gyroSensor->yawSpinTimeUs = currentTimeUs;
1014         }
1015     } else {
1016 #ifndef SIMULATOR_BUILD
1017         // check for spin on yaw axis only
1018          if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
1019             gyroSensor->yawSpinDetected = true;
1020             gyroSensor->yawSpinTimeUs = currentTimeUs;
1021         }
1022 #endif // SIMULATOR_BUILD
1023     }
1024 #else
1025     UNUSED(gyroSensor);
1026     UNUSED(currentTimeUs);
1027 #endif // USE_YAW_SPIN_RECOVERY
1028 }
1029
1030 static FAST_CODE NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
1031 {
1032     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
1033         return;
1034     }
1035     gyroSensor->gyroDev.dataReady = false;
1036
1037     if (isGyroSensorCalibrationComplete(gyroSensor)) {
1038         // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
1039
1040 #if defined(USE_GYRO_SLEW_LIMITER)
1041         gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
1042         gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
1043         gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
1044 #else
1045         gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
1046         gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
1047         gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
1048 #endif
1049
1050         alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
1051     } else {
1052         performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
1053         // still calibrating, so no need to further process gyro data
1054         return;
1055     }
1056
1057 #ifdef USE_GYRO_DATA_ANALYSE
1058     if (isDynamicFilterActive()) {
1059         gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn);
1060     }
1061 #endif
1062
1063     const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
1064     accumulationLastTimeSampledUs = currentTimeUs;
1065     accumulatedMeasurementTimeUs += sampleDeltaUs;
1066
1067     if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
1068         checkForOverflow(gyroSensor, currentTimeUs);
1069     }
1070
1071     if (gyroConfig()->yaw_spin_recovery) {
1072         checkForYawSpin(gyroSensor, currentTimeUs);
1073     }
1074
1075     if (gyroDebugMode == DEBUG_NONE) {
1076         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1077             // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch
1078             float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
1079
1080             gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
1081             gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
1082
1083 #ifdef USE_GYRO_DATA_ANALYSE
1084             gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
1085 #endif
1086             gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
1087             gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
1088             gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
1089             gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
1090
1091             if (!gyroSensor->overflowDetected) {
1092                 // integrate using trapezium rule to avoid bias
1093                 accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
1094                 gyroPrevious[axis] = gyroADCf;
1095             }
1096         }
1097     } else {
1098         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1099             DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]);
1100             // scale gyro output to degrees per second
1101             float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale;
1102             // DEBUG_GYRO_NOTCH records the unfiltered gyro output
1103             DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf));
1104
1105             // apply lowpass2 filter
1106             gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf);
1107             gyroADCf = gyroSensor->lmaSmoothingApplyFn((filter_t *)&gyroSensor->lmaSmoothingFilter[axis], gyroADCf);
1108
1109 #ifdef USE_GYRO_DATA_ANALYSE
1110             // apply dynamic notch filter
1111             if (isDynamicFilterActive()) {
1112                 if (axis == 0) {
1113                     DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data
1114                 }
1115                 gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf);
1116                 if (axis == 0) {
1117                     DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch
1118                 }
1119             }
1120 #endif
1121
1122             // apply static notch filters
1123             gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
1124             gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
1125
1126             // apply lowpass2 filter
1127             DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
1128             gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf);
1129
1130             gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf;
1131             if (!gyroSensor->overflowDetected) {
1132                 // integrate using trapezium rule to avoid bias
1133                 accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyroADCf) * sampleDeltaUs;
1134                 gyroPrevious[axis] = gyroADCf;
1135             }
1136         }
1137     }
1138 }
1139
1140 FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
1141 {
1142 #ifdef USE_DUAL_GYRO
1143     switch (gyroToUse) {
1144     case GYRO_CONFIG_USE_GYRO_1:
1145         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1146         if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
1147             gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
1148             gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
1149             gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
1150         }
1151         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1152         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1153         DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1154         DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1155         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
1156         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
1157         break;
1158     case GYRO_CONFIG_USE_GYRO_2:
1159         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1160         if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
1161             gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X];
1162             gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
1163             gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z];
1164         }
1165         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1166         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1167         DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1168         DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1169         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X]));
1170         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y]));
1171         break;
1172     case GYRO_CONFIG_USE_GYRO_BOTH:
1173         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1174         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1175         if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) {
1176             gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f;
1177             gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f;
1178             gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f;
1179         }
1180         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1181         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1182         DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1183         DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1184         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1185         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1186         DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1187         DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1188         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X]));
1189         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y]));
1190         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X]));
1191         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
1192         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
1193         break;
1194     }
1195 #else
1196     gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1197     gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
1198     gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
1199     gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
1200 #endif
1201 }
1202
1203 bool gyroGetAccumulationAverage(float *accumulationAverage)
1204 {
1205     if (accumulatedMeasurementTimeUs > 0) {
1206         // If we have gyro data accumulated, calculate average rate that will yield the same rotation
1207         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1208             accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
1209             accumulatedMeasurements[axis] = 0.0f;
1210         }
1211         accumulatedMeasurementTimeUs = 0;
1212         return true;
1213     } else {
1214         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1215             accumulationAverage[axis] = 0.0f;
1216         }
1217         return false;
1218     }
1219 }
1220
1221 void gyroReadTemperature(void)
1222 {
1223     if (gyroSensor1.gyroDev.temperatureFn) {
1224         gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature);
1225     }
1226 }
1227
1228 int16_t gyroGetTemperature(void)
1229 {
1230     return gyroSensor1.gyroDev.temperature;
1231 }
1232
1233 int16_t gyroRateDps(int axis)
1234 {
1235 #ifdef USE_DUAL_GYRO
1236     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
1237         return lrintf(gyro.gyroADCf[axis] / gyroSensor2.gyroDev.scale);
1238     } else {
1239         return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
1240     }
1241 #else
1242     return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
1243 #endif
1244 }
1245
1246 bool gyroOverflowDetected(void)
1247 {
1248     return gyroSensor1.overflowDetected;
1249 }
1250
1251 #ifdef USE_YAW_SPIN_RECOVERY
1252 bool gyroYawSpinDetected(void)
1253 {
1254     return gyroSensor1.yawSpinDetected;
1255 }
1256 #endif // USE_YAW_SPIN_RECOVERY
1257
1258 uint16_t gyroAbsRateDps(int axis)
1259 {
1260     return fabsf(gyro.gyroADCf[axis]);
1261 }
1262
1263 #ifdef USE_GYRO_REGISTER_DUMP
1264 uint8_t gyroReadRegister(uint8_t reg)
1265 {
1266     return mpuGyroReadRegister(gyroSensorBus(), reg);
1267 }
1268 #endif