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