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