src/main/sensors/gyro_init.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/config.h"
  36
  37 #include "drivers/accgyro/accgyro.h"
  38 #include "drivers/accgyro/accgyro_fake.h"
  39 #include "drivers/accgyro/accgyro_mpu.h"
  40 #include "drivers/accgyro/accgyro_mpu3050.h"
  41 #include "drivers/accgyro/accgyro_mpu6050.h"
  42 #include "drivers/accgyro/accgyro_mpu6500.h"
  43 #include "drivers/accgyro/accgyro_spi_bmi160.h"
  44 #include "drivers/accgyro/accgyro_spi_bmi270.h"
  45 #include "drivers/accgyro/accgyro_spi_icm20649.h"
  46 #include "drivers/accgyro/accgyro_spi_icm20689.h"
  47 #include "drivers/accgyro/accgyro_spi_icm20689.h"
  48 #include "drivers/accgyro/accgyro_spi_icm426xx.h"
  49 #include "drivers/accgyro/accgyro_spi_lsm6dso.h"
  50 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
  51 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
  52 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
  53
  54 #ifdef USE_GYRO_L3GD20
  55 #include "drivers/accgyro/accgyro_spi_l3gd20.h"
  56 #endif
  57
  58 #ifdef USE_GYRO_L3G4200D
  59 #include "drivers/accgyro_legacy/accgyro_l3g4200d.h"
  60 #endif
  61
  62 #include "drivers/accgyro/gyro_sync.h"
  63
  64 #include "fc/runtime_config.h"
  65
  66 #ifdef USE_DYN_NOTCH_FILTER
  67 #include "flight/dyn_notch_filter.h"
  68 #endif
  69
  70 #include "pg/gyrodev.h"
  71
  72 #include "sensors/gyro.h"
  73 #include "sensors/sensors.h"
  74
  75 #ifdef USE_MULTI_GYRO
  76 #define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1)
  77 #else
  78 #define ACTIVE_GYRO (&gyro.gyroSensor1)
  79 #endif
  80
  81 // The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
  82 // This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
  83 // gyro types are supported with SPI DMA.
  84 #define GYRO_BUF_SIZE 32
  85
  86 static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
  87
  88 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
  89 {
  90     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
  91     if (notchHz > gyroFrequencyNyquist) {
  92         if (notchCutoffHz < gyroFrequencyNyquist) {
  93             notchHz = gyroFrequencyNyquist;
  94         } else {
  95             notchHz = 0;
  96         }
  97     }
  98
  99     return notchHz;
 100 }
 101
 102 static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz)
 103 {
 104     gyro.notchFilter1ApplyFn = nullFilterApply;
 105
 106     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 107
 108     if (notchHz != 0 && notchCutoffHz != 0) {
 109         gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 110         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 111         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 112             biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
 113         }
 114     }
 115 }
 116
 117 static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
 118 {
 119     gyro.notchFilter2ApplyFn = nullFilterApply;
 120
 121     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 122
 123     if (notchHz != 0 && notchCutoffHz != 0) {
 124         gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 125         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 126         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 127             biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
 128         }
 129     }
 130 }
 131
 132 static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
 133 {
 134     filterApplyFnPtr *lowpassFilterApplyFn;
 135     gyroLowpassFilter_t *lowpassFilter = NULL;
 136
 137     switch (slot) {
 138     case FILTER_LPF1:
 139         lowpassFilterApplyFn = &gyro.lowpassFilterApplyFn;
 140         lowpassFilter = gyro.lowpassFilter;
 141         break;
 142
 143     case FILTER_LPF2:
 144         lowpassFilterApplyFn = &gyro.lowpass2FilterApplyFn;
 145         lowpassFilter = gyro.lowpass2Filter;
 146         break;
 147
 148     default:
 149         return false;
 150     }
 151
 152     bool ret = false;
 153
 154     // Establish some common constants
 155     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / looptime;
 156     const float gyroDt = looptime * 1e-6f;
 157
 158     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
 159     const float gain = pt1FilterGain(lpfHz, gyroDt);
 160
 161     // Dereference the pointer to null before checking valid cutoff and filter
 162     // type. It will be overridden for positive cases.
 163     *lowpassFilterApplyFn = nullFilterApply;
 164
 165     // If lowpass cutoff has been specified
 166     if (lpfHz) {
 167         switch (type) {
 168         case FILTER_PT1:
 169             *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
 170             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 171                 pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
 172             }
 173             ret = true;
 174             break;
 175         case FILTER_BIQUAD:
 176             if (lpfHz <= gyroFrequencyNyquist) {
 177 #ifdef USE_DYN_LPF
 178                 *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
 179 #else
 180                 *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 181 #endif
 182                 for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 183                     biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, looptime);
 184                 }
 185                 ret = true;
 186             }
 187             break;
 188         case FILTER_PT2:
 189             *lowpassFilterApplyFn = (filterApplyFnPtr) pt2FilterApply;
 190             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 191                 pt2FilterInit(&lowpassFilter[axis].pt2FilterState, gain);
 192             }
 193             ret = true;
 194             break;
 195         case FILTER_PT3:
 196             *lowpassFilterApplyFn = (filterApplyFnPtr) pt3FilterApply;
 197             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 198                 pt3FilterInit(&lowpassFilter[axis].pt3FilterState, gain);
 199             }
 200             ret = true;
 201             break;
 202         }
 203     }
 204     return ret;
 205 }
 206
 207 #ifdef USE_DYN_LPF
 208 static void dynLpfFilterInit()
 209 {
 210     if (gyroConfig()->gyro_lpf1_dyn_min_hz > 0) {
 211         switch (gyroConfig()->gyro_lpf1_type) {
 212         case FILTER_PT1:
 213             gyro.dynLpfFilter = DYN_LPF_PT1;
 214             break;
 215         case FILTER_BIQUAD:
 216             gyro.dynLpfFilter = DYN_LPF_BIQUAD;
 217             break;
 218         case FILTER_PT2:
 219             gyro.dynLpfFilter = DYN_LPF_PT2;
 220             break;
 221         case FILTER_PT3:
 222             gyro.dynLpfFilter = DYN_LPF_PT3;
 223             break;
 224         default:
 225             gyro.dynLpfFilter = DYN_LPF_NONE;
 226             break;
 227         }
 228     } else {
 229         gyro.dynLpfFilter = DYN_LPF_NONE;
 230     }
 231     gyro.dynLpfMin = gyroConfig()->gyro_lpf1_dyn_min_hz;
 232     gyro.dynLpfMax = gyroConfig()->gyro_lpf1_dyn_max_hz;
 233     gyro.dynLpfCurveExpo = gyroConfig()->gyro_lpf1_dyn_expo;
 234 }
 235 #endif
 236
 237 void gyroInitFilters(void)
 238 {
 239     uint16_t gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_static_hz;
 240
 241 #ifdef USE_DYN_LPF
 242     if (gyroConfig()->gyro_lpf1_dyn_min_hz > 0) {
 243         gyro_lpf1_init_hz = gyroConfig()->gyro_lpf1_dyn_min_hz;
 244     }
 245 #endif
 246
 247     gyroInitLowpassFilterLpf(
 248       FILTER_LPF1,
 249       gyroConfig()->gyro_lpf1_type,
 250       gyro_lpf1_init_hz,
 251       gyro.targetLooptime
 252     );
 253
 254     gyro.downsampleFilterEnabled = gyroInitLowpassFilterLpf(
 255       FILTER_LPF2,
 256       gyroConfig()->gyro_lpf2_type,
 257       gyroConfig()->gyro_lpf2_static_hz,
 258       gyro.sampleLooptime
 259     );
 260
 261     gyroInitFilterNotch1(gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
 262     gyroInitFilterNotch2(gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 263 #ifdef USE_DYN_LPF
 264     dynLpfFilterInit();
 265 #endif
 266 #ifdef USE_DYN_NOTCH_FILTER
 267     dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
 268 #endif
 269
 270     const float k = pt1FilterGain(GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ, gyro.targetLooptime);
 271     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 272         pt1FilterInit(&gyro.imuGyroFilter[axis], k);
 273     }
 274 }
 275
 276 #if defined(USE_GYRO_SLEW_LIMITER)
 277 void gyroInitSlewLimiter(gyroSensor_t *gyroSensor)
 278 {
 279
 280     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 281         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
 282     }
 283 }
 284 #endif
 285
 286 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 287 {
 288 #if defined(USE_GYRO_SLEW_LIMITER)
 289     gyroInitSlewLimiter(gyroSensor);
 290 #else
 291     UNUSED(gyroSensor);
 292 #endif
 293 }
 294
 295 void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 296 {
 297     gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
 298     gyroSensor->gyroDev.gyroAlign = config->alignment;
 299     buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
 300     gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
 301     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
 302
 303     // The targetLooptime gets set later based on the active sensor's gyroSampleRateHz and pid_process_denom
 304     gyroSensor->gyroDev.gyroSampleRateHz = gyroSetSampleRate(&gyroSensor->gyroDev);
 305     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 306
 307     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
 308     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
 309     switch (gyroSensor->gyroDev.gyroHardware) {
 310     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
 311     case GYRO_DEFAULT:
 312     case GYRO_FAKE:
 313     case GYRO_MPU6050:
 314     case GYRO_L3G4200D:
 315     case GYRO_MPU3050:
 316     case GYRO_L3GD20:
 317     case GYRO_BMI160:
 318     case GYRO_BMI270:
 319     case GYRO_MPU6000:
 320     case GYRO_MPU6500:
 321     case GYRO_MPU9250:
 322     case GYRO_LSM6DSO:
 323         gyroSensor->gyroDev.gyroHasOverflowProtection = true;
 324         break;
 325
 326     case GYRO_ICM20601:
 327     case GYRO_ICM20602:
 328     case GYRO_ICM20608G:
 329     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
 330     case GYRO_ICM20689:
 331         gyroSensor->gyroDev.gyroHasOverflowProtection = false;
 332         break;
 333
 334     default:
 335         gyroSensor->gyroDev.gyroHasOverflowProtection = false;  // default catch for newly added gyros until proven to be unaffected
 336         break;
 337     }
 338
 339     gyroInitSensorFilters(gyroSensor);
 340 }
 341
 342 STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
 343 {
 344     gyroHardware_e gyroHardware = GYRO_DEFAULT;
 345
 346     switch (gyroHardware) {
 347     case GYRO_DEFAULT:
 348         FALLTHROUGH;
 349
 350 #ifdef USE_GYRO_MPU6050
 351     case GYRO_MPU6050:
 352         if (mpu6050GyroDetect(dev)) {
 353             gyroHardware = GYRO_MPU6050;
 354             break;
 355         }
 356         FALLTHROUGH;
 357 #endif
 358
 359 #ifdef USE_GYRO_L3G4200D
 360     case GYRO_L3G4200D:
 361         if (l3g4200dDetect(dev)) {
 362             gyroHardware = GYRO_L3G4200D;
 363             break;
 364         }
 365         FALLTHROUGH;
 366 #endif
 367
 368 #ifdef USE_GYRO_MPU3050
 369     case GYRO_MPU3050:
 370         if (mpu3050Detect(dev)) {
 371             gyroHardware = GYRO_MPU3050;
 372             break;
 373         }
 374         FALLTHROUGH;
 375 #endif
 376
 377 #ifdef USE_GYRO_L3GD20
 378     case GYRO_L3GD20:
 379         if (l3gd20GyroDetect(dev)) {
 380             gyroHardware = GYRO_L3GD20;
 381             break;
 382         }
 383         FALLTHROUGH;
 384 #endif
 385
 386 #ifdef USE_GYRO_SPI_MPU6000
 387     case GYRO_MPU6000:
 388         if (mpu6000SpiGyroDetect(dev)) {
 389             gyroHardware = GYRO_MPU6000;
 390             break;
 391         }
 392         FALLTHROUGH;
 393 #endif
 394
 395 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 396     case GYRO_MPU6500:
 397     case GYRO_ICM20601:
 398     case GYRO_ICM20602:
 399     case GYRO_ICM20608G:
 400 #ifdef USE_GYRO_SPI_MPU6500
 401         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 402 #else
 403         if (mpu6500GyroDetect(dev)) {
 404 #endif
 405             switch (dev->mpuDetectionResult.sensor) {
 406             case MPU_9250_SPI:
 407                 gyroHardware = GYRO_MPU9250;
 408                 break;
 409             case ICM_20601_SPI:
 410                 gyroHardware = GYRO_ICM20601;
 411                 break;
 412             case ICM_20602_SPI:
 413                 gyroHardware = GYRO_ICM20602;
 414                 break;
 415             case ICM_20608_SPI:
 416                 gyroHardware = GYRO_ICM20608G;
 417                 break;
 418             default:
 419                 gyroHardware = GYRO_MPU6500;
 420             }
 421             break;
 422         }
 423         FALLTHROUGH;
 424 #endif
 425
 426 #ifdef USE_GYRO_SPI_MPU9250
 427     case GYRO_MPU9250:
 428         if (mpu9250SpiGyroDetect(dev)) {
 429             gyroHardware = GYRO_MPU9250;
 430             break;
 431         }
 432         FALLTHROUGH;
 433 #endif
 434
 435 #ifdef USE_GYRO_SPI_ICM20649
 436     case GYRO_ICM20649:
 437         if (icm20649SpiGyroDetect(dev)) {
 438             gyroHardware = GYRO_ICM20649;
 439             break;
 440         }
 441         FALLTHROUGH;
 442 #endif
 443
 444 #ifdef USE_GYRO_SPI_ICM20689
 445     case GYRO_ICM20689:
 446         if (icm20689SpiGyroDetect(dev)) {
 447             gyroHardware = GYRO_ICM20689;
 448             break;
 449         }
 450         FALLTHROUGH;
 451 #endif
 452
 453 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
 454     case GYRO_ICM42605:
 455     case GYRO_ICM42688P:
 456         if (icm426xxSpiGyroDetect(dev)) {
 457             switch (dev->mpuDetectionResult.sensor) {
 458             case ICM_42605_SPI:
 459                 gyroHardware = GYRO_ICM42605;
 460                 break;
 461             case ICM_42688P_SPI:
 462                 gyroHardware = GYRO_ICM42688P;
 463                 break;
 464             default:
 465                 gyroHardware = GYRO_NONE;
 466                 break;
 467             }
 468             break;
 469         }
 470         FALLTHROUGH;
 471 #endif
 472
 473 #ifdef USE_ACCGYRO_BMI160
 474     case GYRO_BMI160:
 475         if (bmi160SpiGyroDetect(dev)) {
 476             gyroHardware = GYRO_BMI160;
 477             break;
 478         }
 479         FALLTHROUGH;
 480 #endif
 481
 482 #ifdef USE_ACCGYRO_BMI270
 483     case GYRO_BMI270:
 484         if (bmi270SpiGyroDetect(dev)) {
 485             gyroHardware = GYRO_BMI270;
 486             break;
 487         }
 488         FALLTHROUGH;
 489 #endif
 490
 491 #ifdef USE_ACCGYRO_LSM6DSO
 492     case GYRO_LSM6DSO:
 493         if (lsm6dsoSpiGyroDetect(dev)) {
 494             gyroHardware = GYRO_LSM6DSO;
 495             break;
 496         }
 497         FALLTHROUGH;
 498 #endif
 499
 500 #ifdef USE_FAKE_GYRO
 501     case GYRO_FAKE:
 502         if (fakeGyroDetect(dev)) {
 503             gyroHardware = GYRO_FAKE;
 504             break;
 505         }
 506         FALLTHROUGH;
 507 #endif
 508
 509     default:
 510         gyroHardware = GYRO_NONE;
 511     }
 512
 513     if (gyroHardware != GYRO_NONE) {
 514         sensorsSet(SENSOR_GYRO);
 515     }
 516
 517
 518     return gyroHardware;
 519 }
 520
 521 static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 522 {
 523 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 524  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 525  || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
 526
 527     bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
 528
 529 #if !defined(USE_FAKE_GYRO) // Allow resorting to fake accgyro if defined
 530     if (!gyroFound) {
 531         return false;
 532     }
 533 #else
 534     UNUSED(gyroFound);
 535 #endif
 536 #else
 537     UNUSED(config);
 538 #endif
 539
 540     const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 541     gyroSensor->gyroDev.gyroHardware = gyroHardware;
 542
 543     return gyroHardware != GYRO_NONE;
 544 }
 545
 546 static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
 547 {
 548 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 549  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 550  || defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DSO)
 551     mpuPreInit(config);
 552 #else
 553     UNUSED(config);
 554 #endif
 555 }
 556
 557 void gyroPreInit(void)
 558 {
 559     gyroPreInitSensor(gyroDeviceConfig(0));
 560 #ifdef USE_MULTI_GYRO
 561     gyroPreInitSensor(gyroDeviceConfig(1));
 562 #endif
 563 }
 564
 565 bool gyroInit(void)
 566 {
 567 #ifdef USE_GYRO_OVERFLOW_CHECK
 568     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 569         gyro.overflowAxisMask = GYRO_OVERFLOW_Z;
 570     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 571         gyro.overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 572     } else {
 573         gyro.overflowAxisMask = 0;
 574     }
 575 #endif
 576
 577     gyro.gyroDebugMode = DEBUG_NONE;
 578     gyro.useDualGyroDebugging = false;
 579     gyro.gyroHasOverflowProtection = true;
 580
 581     switch (debugMode) {
 582     case DEBUG_FFT:
 583     case DEBUG_FFT_FREQ:
 584     case DEBUG_GYRO_RAW:
 585     case DEBUG_GYRO_SCALED:
 586     case DEBUG_GYRO_FILTERED:
 587     case DEBUG_DYN_LPF:
 588     case DEBUG_GYRO_SAMPLE:
 589         gyro.gyroDebugMode = debugMode;
 590         break;
 591     case DEBUG_DUAL_GYRO_DIFF:
 592     case DEBUG_DUAL_GYRO_RAW:
 593     case DEBUG_DUAL_GYRO_SCALED:
 594         gyro.useDualGyroDebugging = true;
 595         break;
 596     }
 597
 598     gyroDetectionFlags = GYRO_NONE_MASK;
 599     uint8_t gyrosToScan = gyroConfig()->gyrosDetected;
 600
 601     gyro.gyroToUse = gyroConfig()->gyro_to_use;
 602     gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
 603
 604     if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
 605         gyroDetectionFlags |= GYRO_1_MASK;
 606     }
 607
 608 #if defined(USE_MULTI_GYRO)
 609     if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
 610         gyroDetectionFlags |= GYRO_2_MASK;
 611     }
 612 #endif
 613
 614     if (gyroDetectionFlags == GYRO_NONE_MASK) {
 615         return false;
 616     }
 617
 618     bool eepromWriteRequired = false;
 619     if (!gyrosToScan) {
 620         gyroConfigMutable()->gyrosDetected = gyroDetectionFlags;
 621         eepromWriteRequired = true;
 622     }
 623
 624 #if defined(USE_MULTI_GYRO)
 625     if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
 626         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
 627         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
 628         if (gyroDetectionFlags & GYRO_1_MASK) {
 629             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 630         } else {
 631             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2;
 632         }
 633
 634         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 635         eepromWriteRequired = true;
 636     }
 637
 638     // Only allow using both gyros simultaneously if they are the same hardware type.
 639     if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
 640         gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
 641     } else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 642         // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
 643         gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 644         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 645         eepromWriteRequired = true;
 646     }
 647
 648     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 649         static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
 650         // SPI DMA buffer required per device
 651         gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
 652         gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
 653
 654         gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
 655         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
 656         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
 657     }
 658 #endif
 659
 660     if (eepromWriteRequired) {
 661         writeEEPROM();
 662     }
 663
 664     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 665         static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
 666         // SPI DMA buffer required per device
 667         gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
 668         gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
 669         gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
 670         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
 671         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;
 672     }
 673
 674     // Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode
 675     // then logic above requires both sensors to be the same so we'll use sensor1's scale.
 676     // This will need to be revised if we ever allow different sensor types to be used simultaneously.
 677     // Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW
 678     gyro.scale = gyro.gyroSensor1.gyroDev.scale;
 679     gyro.rawSensorDev = &gyro.gyroSensor1.gyroDev;
 680 #if defined(USE_MULTI_GYRO)
 681     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 682         gyro.scale = gyro.gyroSensor2.gyroDev.scale;
 683         gyro.rawSensorDev = &gyro.gyroSensor2.gyroDev;
 684     }
 685 #endif
 686
 687     if (gyro.rawSensorDev) {
 688         gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
 689         gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
 690     } else {
 691         gyro.sampleRateHz = 0;
 692         gyro.accSampleRateHz = 0;
 693     }
 694
 695     return true;
 696 }
 697
 698 gyroDetectionFlags_t getGyroDetectionFlags(void)
 699 {
 700     return gyroDetectionFlags;
 701 }
 702
 703 void gyroSetTargetLooptime(uint8_t pidDenom)
 704 {
 705     activePidLoopDenom = pidDenom;
 706     if (gyro.sampleRateHz) {
 707         gyro.sampleLooptime = 1e6 / gyro.sampleRateHz;
 708         gyro.targetLooptime = activePidLoopDenom * 1e6 / gyro.sampleRateHz;
 709     } else {
 710         gyro.sampleLooptime = 0;
 711         gyro.targetLooptime = 0;
 712     }
 713 }
 714
 715
 716 gyroDev_t *gyroActiveDev(void)
 717 {
 718     return &ACTIVE_GYRO->gyroDev;
 719 }
 720
 721 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 722 {
 723     return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
 724 }
 725
 726 int16_t gyroRateDps(int axis)
 727 {
 728     return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
 729 }
 730
 731 #ifdef USE_GYRO_REGISTER_DUMP
 732 static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor)
 733 {
 734 #ifdef USE_MULTI_GYRO
 735     if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
 736         return &gyro.gyroSensor2.gyroDev.dev;
 737     }
 738 #else
 739     UNUSED(whichSensor);
 740 #endif
 741     return &gyro.gyroSensor1.gyroDev.dev;
 742 }
 743
 744 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
 745 {
 746     return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg);
 747 }
 748 #endif // USE_GYRO_REGISTER_DUMP