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
 271 #if defined(USE_GYRO_SLEW_LIMITER)
 272 void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
 273
 274     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 275         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
 276     }
 277 }
 278 #endif
 279
 280 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 281 {
 282 #if defined(USE_GYRO_SLEW_LIMITER)
 283     gyroInitSlewLimiter(gyroSensor);
 284 #else
 285     UNUSED(gyroSensor);
 286 #endif
 287 }
 288
 289 void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 290 {
 291     gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
 292     gyroSensor->gyroDev.gyroAlign = config->alignment;
 293     buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
 294     gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
 295     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
 296
 297     // The targetLooptime gets set later based on the active sensor's gyroSampleRateHz and pid_process_denom
 298     gyroSensor->gyroDev.gyroSampleRateHz = gyroSetSampleRate(&gyroSensor->gyroDev);
 299     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 300
 301     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
 302     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
 303     switch (gyroSensor->gyroDev.gyroHardware) {
 304     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
 305     case GYRO_DEFAULT:
 306     case GYRO_FAKE:
 307     case GYRO_MPU6050:
 308     case GYRO_L3G4200D:
 309     case GYRO_MPU3050:
 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         gyroSensor->gyroDev.gyroHasOverflowProtection = true;
 318         break;
 319
 320     case GYRO_ICM20601:
 321     case GYRO_ICM20602:
 322     case GYRO_ICM20608G:
 323     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
 324     case GYRO_ICM20689:
 325         gyroSensor->gyroDev.gyroHasOverflowProtection = false;
 326         break;
 327
 328     default:
 329         gyroSensor->gyroDev.gyroHasOverflowProtection = false;  // default catch for newly added gyros until proven to be unaffected
 330         break;
 331     }
 332
 333     gyroInitSensorFilters(gyroSensor);
 334 }
 335
 336 STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
 337 {
 338     gyroHardware_e gyroHardware = GYRO_DEFAULT;
 339
 340     switch (gyroHardware) {
 341     case GYRO_DEFAULT:
 342         FALLTHROUGH;
 343
 344 #ifdef USE_GYRO_MPU6050
 345     case GYRO_MPU6050:
 346         if (mpu6050GyroDetect(dev)) {
 347             gyroHardware = GYRO_MPU6050;
 348             break;
 349         }
 350         FALLTHROUGH;
 351 #endif
 352
 353 #ifdef USE_GYRO_L3G4200D
 354     case GYRO_L3G4200D:
 355         if (l3g4200dDetect(dev)) {
 356             gyroHardware = GYRO_L3G4200D;
 357             break;
 358         }
 359         FALLTHROUGH;
 360 #endif
 361
 362 #ifdef USE_GYRO_MPU3050
 363     case GYRO_MPU3050:
 364         if (mpu3050Detect(dev)) {
 365             gyroHardware = GYRO_MPU3050;
 366             break;
 367         }
 368         FALLTHROUGH;
 369 #endif
 370
 371 #ifdef USE_GYRO_L3GD20
 372     case GYRO_L3GD20:
 373         if (l3gd20GyroDetect(dev)) {
 374             gyroHardware = GYRO_L3GD20;
 375             break;
 376         }
 377         FALLTHROUGH;
 378 #endif
 379
 380 #ifdef USE_GYRO_SPI_MPU6000
 381     case GYRO_MPU6000:
 382         if (mpu6000SpiGyroDetect(dev)) {
 383             gyroHardware = GYRO_MPU6000;
 384             break;
 385         }
 386         FALLTHROUGH;
 387 #endif
 388
 389 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 390     case GYRO_MPU6500:
 391     case GYRO_ICM20601:
 392     case GYRO_ICM20602:
 393     case GYRO_ICM20608G:
 394 #ifdef USE_GYRO_SPI_MPU6500
 395         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 396 #else
 397         if (mpu6500GyroDetect(dev)) {
 398 #endif
 399             switch (dev->mpuDetectionResult.sensor) {
 400             case MPU_9250_SPI:
 401                 gyroHardware = GYRO_MPU9250;
 402                 break;
 403             case ICM_20601_SPI:
 404                 gyroHardware = GYRO_ICM20601;
 405                 break;
 406             case ICM_20602_SPI:
 407                 gyroHardware = GYRO_ICM20602;
 408                 break;
 409             case ICM_20608_SPI:
 410                 gyroHardware = GYRO_ICM20608G;
 411                 break;
 412             default:
 413                 gyroHardware = GYRO_MPU6500;
 414             }
 415             break;
 416         }
 417         FALLTHROUGH;
 418 #endif
 419
 420 #ifdef USE_GYRO_SPI_MPU9250
 421     case GYRO_MPU9250:
 422         if (mpu9250SpiGyroDetect(dev)) {
 423             gyroHardware = GYRO_MPU9250;
 424             break;
 425         }
 426         FALLTHROUGH;
 427 #endif
 428
 429 #ifdef USE_GYRO_SPI_ICM20649
 430     case GYRO_ICM20649:
 431         if (icm20649SpiGyroDetect(dev)) {
 432             gyroHardware = GYRO_ICM20649;
 433             break;
 434         }
 435         FALLTHROUGH;
 436 #endif
 437
 438 #ifdef USE_GYRO_SPI_ICM20689
 439     case GYRO_ICM20689:
 440         if (icm20689SpiGyroDetect(dev)) {
 441             gyroHardware = GYRO_ICM20689;
 442             break;
 443         }
 444         FALLTHROUGH;
 445 #endif
 446
 447 #if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
 448     case GYRO_ICM42605:
 449     case GYRO_ICM42688P:
 450         if (icm426xxSpiGyroDetect(dev)) {
 451             switch (dev->mpuDetectionResult.sensor) {
 452             case ICM_42605_SPI:
 453                 gyroHardware = GYRO_ICM42605;
 454                 break;
 455             case ICM_42688P_SPI:
 456                 gyroHardware = GYRO_ICM42688P;
 457                 break;
 458             default:
 459                 gyroHardware = GYRO_NONE;
 460                 break;
 461             }
 462             break;
 463         }
 464         FALLTHROUGH;
 465 #endif
 466
 467 #ifdef USE_ACCGYRO_BMI160
 468     case GYRO_BMI160:
 469         if (bmi160SpiGyroDetect(dev)) {
 470             gyroHardware = GYRO_BMI160;
 471             break;
 472         }
 473         FALLTHROUGH;
 474 #endif
 475
 476 #ifdef USE_ACCGYRO_BMI270
 477     case GYRO_BMI270:
 478         if (bmi270SpiGyroDetect(dev)) {
 479             gyroHardware = GYRO_BMI270;
 480             break;
 481         }
 482         FALLTHROUGH;
 483 #endif
 484
 485 #ifdef USE_ACCGYRO_LSM6DSO
 486     case GYRO_LSM6DSO:
 487         if (lsm6dsoSpiGyroDetect(dev)) {
 488             gyroHardware = GYRO_LSM6DSO;
 489             break;
 490         }
 491         FALLTHROUGH;
 492 #endif
 493
 494 #ifdef USE_FAKE_GYRO
 495     case GYRO_FAKE:
 496         if (fakeGyroDetect(dev)) {
 497             gyroHardware = GYRO_FAKE;
 498             break;
 499         }
 500         FALLTHROUGH;
 501 #endif
 502
 503     default:
 504         gyroHardware = GYRO_NONE;
 505     }
 506
 507     if (gyroHardware != GYRO_NONE) {
 508         sensorsSet(SENSOR_GYRO);
 509     }
 510
 511
 512     return gyroHardware;
 513 }
 514
 515 static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 516 {
 517 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 518  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 519  || 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)
 520
 521     bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
 522
 523 #if !defined(USE_FAKE_GYRO) // Allow resorting to fake accgyro if defined
 524     if (!gyroFound) {
 525         return false;
 526     }
 527 #else
 528     UNUSED(gyroFound);
 529 #endif
 530 #else
 531     UNUSED(config);
 532 #endif
 533
 534     const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 535     gyroSensor->gyroDev.gyroHardware = gyroHardware;
 536
 537     return gyroHardware != GYRO_NONE;
 538 }
 539
 540 static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
 541 {
 542 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 543  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 544  || defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DSO)
 545     mpuPreInit(config);
 546 #else
 547     UNUSED(config);
 548 #endif
 549 }
 550
 551 void gyroPreInit(void)
 552 {
 553     gyroPreInitSensor(gyroDeviceConfig(0));
 554 #ifdef USE_MULTI_GYRO
 555     gyroPreInitSensor(gyroDeviceConfig(1));
 556 #endif
 557 }
 558
 559 bool gyroInit(void)
 560 {
 561 #ifdef USE_GYRO_OVERFLOW_CHECK
 562     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 563         gyro.overflowAxisMask = GYRO_OVERFLOW_Z;
 564     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 565         gyro.overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 566     } else {
 567         gyro.overflowAxisMask = 0;
 568     }
 569 #endif
 570
 571     gyro.gyroDebugMode = DEBUG_NONE;
 572     gyro.useDualGyroDebugging = false;
 573     gyro.gyroHasOverflowProtection = true;
 574
 575     switch (debugMode) {
 576     case DEBUG_FFT:
 577     case DEBUG_FFT_FREQ:
 578     case DEBUG_GYRO_RAW:
 579     case DEBUG_GYRO_SCALED:
 580     case DEBUG_GYRO_FILTERED:
 581     case DEBUG_DYN_LPF:
 582     case DEBUG_GYRO_SAMPLE:
 583         gyro.gyroDebugMode = debugMode;
 584         break;
 585     case DEBUG_DUAL_GYRO_DIFF:
 586     case DEBUG_DUAL_GYRO_RAW:
 587     case DEBUG_DUAL_GYRO_SCALED:
 588         gyro.useDualGyroDebugging = true;
 589         break;
 590     }
 591
 592     gyroDetectionFlags = GYRO_NONE_MASK;
 593     uint8_t gyrosToScan = gyroConfig()->gyrosDetected;
 594
 595     gyro.gyroToUse = gyroConfig()->gyro_to_use;
 596     gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
 597
 598     if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
 599         gyroDetectionFlags |= GYRO_1_MASK;
 600     }
 601
 602 #if defined(USE_MULTI_GYRO)
 603     if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
 604         gyroDetectionFlags |= GYRO_2_MASK;
 605     }
 606 #endif
 607
 608     if (gyroDetectionFlags == GYRO_NONE_MASK) {
 609         return false;
 610     }
 611
 612     bool eepromWriteRequired = false;
 613     if (!gyrosToScan) {
 614         gyroConfigMutable()->gyrosDetected = gyroDetectionFlags;
 615         eepromWriteRequired = true;
 616     }
 617
 618 #if defined(USE_MULTI_GYRO)
 619     if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
 620         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
 621         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
 622         if (gyroDetectionFlags & GYRO_1_MASK) {
 623             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 624         } else {
 625             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2;
 626         }
 627
 628         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 629         eepromWriteRequired = true;
 630     }
 631
 632     // Only allow using both gyros simultaneously if they are the same hardware type.
 633     if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
 634         gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
 635     } else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 636         // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
 637         gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 638         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 639         eepromWriteRequired = true;
 640     }
 641
 642     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 643         static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
 644         // SPI DMA buffer required per device
 645         gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
 646         gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
 647
 648         gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
 649         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
 650         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
 651     }
 652 #endif
 653
 654     if (eepromWriteRequired) {
 655         writeEEPROM();
 656     }
 657
 658     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 659         static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
 660         // SPI DMA buffer required per device
 661         gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
 662         gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
 663         gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
 664         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
 665         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;
 666     }
 667
 668     // Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode
 669     // then logic above requires both sensors to be the same so we'll use sensor1's scale.
 670     // This will need to be revised if we ever allow different sensor types to be used simultaneously.
 671     // Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW
 672     gyro.scale = gyro.gyroSensor1.gyroDev.scale;
 673     gyro.rawSensorDev = &gyro.gyroSensor1.gyroDev;
 674 #if defined(USE_MULTI_GYRO)
 675     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 676         gyro.scale = gyro.gyroSensor2.gyroDev.scale;
 677         gyro.rawSensorDev = &gyro.gyroSensor2.gyroDev;
 678     }
 679 #endif
 680
 681     if (gyro.rawSensorDev) {
 682         gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
 683         gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
 684     } else {
 685         gyro.sampleRateHz = 0;
 686         gyro.accSampleRateHz = 0;
 687     }
 688
 689     return true;
 690 }
 691
 692 gyroDetectionFlags_t getGyroDetectionFlags(void)
 693 {
 694     return gyroDetectionFlags;
 695 }
 696
 697 void gyroSetTargetLooptime(uint8_t pidDenom)
 698 {
 699     activePidLoopDenom = pidDenom;
 700     if (gyro.sampleRateHz) {
 701         gyro.sampleLooptime = 1e6 / gyro.sampleRateHz;
 702         gyro.targetLooptime = activePidLoopDenom * 1e6 / gyro.sampleRateHz;
 703     } else {
 704         gyro.sampleLooptime = 0;
 705         gyro.targetLooptime = 0;
 706     }
 707 }
 708
 709
 710 gyroDev_t *gyroActiveDev(void)
 711 {
 712     return &ACTIVE_GYRO->gyroDev;
 713 }
 714
 715 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 716 {
 717     return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
 718 }
 719
 720 int16_t gyroRateDps(int axis)
 721 {
 722     return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
 723 }
 724
 725 #ifdef USE_GYRO_REGISTER_DUMP
 726 static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor)
 727 {
 728 #ifdef USE_MULTI_GYRO
 729     if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
 730         return &gyro.gyroSensor2.gyroDev.dev;
 731     }
 732 #else
 733     UNUSED(whichSensor);
 734 #endif
 735     return &gyro.gyroSensor1.gyroDev.dev;
 736 }
 737
 738 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
 739 {
 740     return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg);
 741 }
 742 #endif // USE_GYRO_REGISTER_DUMP