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