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