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_MPU3050) && !defined(USE_GYRO_MPU6050) && \
  76     !defined(USE_GYRO_MPU6500) && !defined(USE_GYRO_SPI_ICM20689) && !defined(USE_GYRO_SPI_MPU6000) && \
  77     !defined(USE_GYRO_SPI_MPU6500) && !defined(USE_GYRO_SPI_MPU9250) && !defined(USE_GYRO_L3GD20) && \
  78     !defined(USE_GYRO_SPI_ICM42605) && !defined(USE_GYRO_SPI_ICM42688P) && !defined(USE_ACCGYRO_BMI270) && \
  79     !defined(USE_ACCGYRO_LSM6DSV16X) && !defined(USE_ACCGYRO_LSM6DSO) && !defined(USE_VIRTUAL_GYRO)
  80 #error At least one USE_GYRO device definition required
  81 #endif
  82
  83 #ifdef USE_MULTI_GYRO
  84 #define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1)
  85 #else
  86 #define ACTIVE_GYRO (&gyro.gyroSensor1)
  87 #endif
  88
  89 // The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
  90 // This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
  91 // gyro types are supported with SPI DMA.
  92 #define GYRO_BUF_SIZE 32
  93
  94 static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
  95
  96 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
  97 {
  98     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
  99     if (notchHz > gyroFrequencyNyquist) {
 100         if (notchCutoffHz < gyroFrequencyNyquist) {
 101             notchHz = gyroFrequencyNyquist;
 102         } else {
 103             notchHz = 0;
 104         }
 105     }
 106
 107     return notchHz;
 108 }
 109
 110 static void gyroInitFilterNotch1(uint16_t notchHz, uint16_t notchCutoffHz)
 111 {
 112     gyro.notchFilter1ApplyFn = nullFilterApply;
 113
 114     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 115
 116     if (notchHz != 0 && notchCutoffHz != 0) {
 117         gyro.notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 118         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 119         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 120             biquadFilterInit(&gyro.notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
 121         }
 122     }
 123 }
 124
 125 static void gyroInitFilterNotch2(uint16_t notchHz, uint16_t notchCutoffHz)
 126 {
 127     gyro.notchFilter2ApplyFn = nullFilterApply;
 128
 129     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 130
 131     if (notchHz != 0 && notchCutoffHz != 0) {
 132         gyro.notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 133         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 134         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 135             biquadFilterInit(&gyro.notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH, 1.0f);
 136         }
 137     }
 138 }
 139
 140 static bool gyroInitLowpassFilterLpf(int slot, int type, uint16_t lpfHz, uint32_t looptime)
 141 {
 142     filterApplyFnPtr *lowpassFilterApplyFn;
 143     gyroLowpassFilter_t *lowpassFilter = NULL;
 144
 145     switch (slot) {
 146     case FILTER_LPF1:
 147         lowpassFilterApplyFn = &gyro.lowpassFilterApplyFn;
 148         lowpassFilter = gyro.lowpassFilter;
 149         break;
 150
 151     case FILTER_LPF2:
 152         lowpassFilterApplyFn = &gyro.lowpass2FilterApplyFn;
 153         lowpassFilter = gyro.lowpass2Filter;
 154         break;
 155
 156     default:
 157         return false;
 158     }
 159
 160     bool ret = false;
 161
 162     // Establish some common constants
 163     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / looptime;
 164     const float gyroDt = looptime * 1e-6f;
 165
 166     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
 167     const float gain = pt1FilterGain(lpfHz, gyroDt);
 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, gain);
 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, gain);
 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, gain);
 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);
 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     buildRotationMatrixFromAlignment(&config->customAlignment, &gyroSensor->gyroDev.rotationMatrix);
 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 (mpu6500GyroDetect(dev) || 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
 538     return gyroHardware;
 539 }
 540
 541 static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 542 {
 543 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 544  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 545  || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) \
 546  || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_LSM6DSV16X)
 547
 548     bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
 549
 550 #if !defined(USE_VIRTUAL_GYRO) // Allow resorting to virtual accgyro if defined
 551     if (!gyroFound) {
 552         return false;
 553     }
 554 #else
 555     UNUSED(gyroFound);
 556 #endif
 557 #else
 558     UNUSED(config);
 559 #endif
 560
 561     const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 562     gyroSensor->gyroDev.gyroHardware = gyroHardware;
 563
 564     return gyroHardware != GYRO_NONE;
 565 }
 566
 567 static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
 568 {
 569 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 570  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 571  || defined(USE_GYRO_SPI_ICM20689) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGRYO_LSM6DSO)
 572     mpuPreInit(config);
 573 #else
 574     UNUSED(config);
 575 #endif
 576 }
 577
 578 void gyroPreInit(void)
 579 {
 580     gyroPreInitSensor(gyroDeviceConfig(0));
 581 #ifdef USE_MULTI_GYRO
 582     gyroPreInitSensor(gyroDeviceConfig(1));
 583 #endif
 584 }
 585
 586 bool gyroInit(void)
 587 {
 588 #ifdef USE_GYRO_OVERFLOW_CHECK
 589     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 590         gyro.overflowAxisMask = GYRO_OVERFLOW_Z;
 591     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 592         gyro.overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 593     } else {
 594         gyro.overflowAxisMask = 0;
 595     }
 596 #endif
 597
 598     gyro.gyroDebugMode = DEBUG_NONE;
 599     gyro.useDualGyroDebugging = false;
 600     gyro.gyroHasOverflowProtection = true;
 601
 602     switch (debugMode) {
 603     case DEBUG_FFT:
 604     case DEBUG_FFT_FREQ:
 605     case DEBUG_GYRO_RAW:
 606     case DEBUG_GYRO_SCALED:
 607     case DEBUG_GYRO_FILTERED:
 608     case DEBUG_DYN_LPF:
 609     case DEBUG_GYRO_SAMPLE:
 610         gyro.gyroDebugMode = debugMode;
 611         break;
 612     case DEBUG_DUAL_GYRO_DIFF:
 613     case DEBUG_DUAL_GYRO_RAW:
 614     case DEBUG_DUAL_GYRO_SCALED:
 615         gyro.useDualGyroDebugging = true;
 616         break;
 617     }
 618
 619     gyroDetectionFlags = GYRO_NONE_MASK;
 620     uint8_t gyrosToScan = gyroConfig()->gyrosDetected;
 621
 622     gyro.gyroToUse = gyroConfig()->gyro_to_use;
 623     gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
 624
 625     if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
 626         gyroDetectionFlags |= GYRO_1_MASK;
 627     }
 628
 629 #if defined(USE_MULTI_GYRO)
 630     if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
 631         gyroDetectionFlags |= GYRO_2_MASK;
 632     }
 633 #endif
 634
 635     if (gyroDetectionFlags == GYRO_NONE_MASK) {
 636         return false;
 637     }
 638
 639     bool eepromWriteRequired = false;
 640     if (!gyrosToScan) {
 641         gyroConfigMutable()->gyrosDetected = gyroDetectionFlags;
 642         eepromWriteRequired = true;
 643     }
 644
 645 #if defined(USE_MULTI_GYRO)
 646     if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
 647         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
 648         || (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
 649         if (gyroDetectionFlags & GYRO_1_MASK) {
 650             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 651         } else {
 652             gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2;
 653         }
 654
 655         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 656         eepromWriteRequired = true;
 657     }
 658
 659     // Only allow using both gyros simultaneously if they are the same hardware type.
 660     if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
 661         gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
 662     } else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 663         // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
 664         gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 665         gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
 666         eepromWriteRequired = true;
 667     }
 668
 669     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 670         static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
 671         // SPI DMA buffer required per device
 672         gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
 673         gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
 674
 675         gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
 676         gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
 677         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
 678     }
 679 #endif
 680
 681     if (eepromWriteRequired) {
 682         writeEEPROM();
 683     }
 684
 685     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 686         static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
 687         // SPI DMA buffer required per device
 688         gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
 689         gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
 690         gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
 691         gyro.gyroHasOverflowProtection =  gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
 692         detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;
 693     }
 694
 695     // Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode
 696     // then logic above requires both sensors to be the same so we'll use sensor1's scale.
 697     // This will need to be revised if we ever allow different sensor types to be used simultaneously.
 698     // Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW
 699     gyro.scale = gyro.gyroSensor1.gyroDev.scale;
 700     gyro.rawSensorDev = &gyro.gyroSensor1.gyroDev;
 701 #if defined(USE_MULTI_GYRO)
 702     if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 703         gyro.scale = gyro.gyroSensor2.gyroDev.scale;
 704         gyro.rawSensorDev = &gyro.gyroSensor2.gyroDev;
 705     }
 706 #endif
 707
 708     if (gyro.rawSensorDev) {
 709         gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
 710         gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
 711     } else {
 712         gyro.sampleRateHz = 0;
 713         gyro.accSampleRateHz = 0;
 714     }
 715
 716     return true;
 717 }
 718
 719 gyroDetectionFlags_t getGyroDetectionFlags(void)
 720 {
 721     return gyroDetectionFlags;
 722 }
 723
 724 void gyroSetTargetLooptime(uint8_t pidDenom)
 725 {
 726     activePidLoopDenom = pidDenom;
 727     if (gyro.sampleRateHz) {
 728         gyro.sampleLooptime = 1e6f / gyro.sampleRateHz;
 729         gyro.targetLooptime = activePidLoopDenom * 1e6f / gyro.sampleRateHz;
 730     } else {
 731         gyro.sampleLooptime = 0;
 732         gyro.targetLooptime = 0;
 733     }
 734 }
 735
 736
 737 gyroDev_t *gyroActiveDev(void)
 738 {
 739     return &ACTIVE_GYRO->gyroDev;
 740 }
 741
 742 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 743 {
 744     return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
 745 }
 746
 747 int16_t gyroRateDps(int axis)
 748 {
 749     return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
 750 }
 751
 752 #ifdef USE_GYRO_REGISTER_DUMP
 753 static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor)
 754 {
 755 #ifdef USE_MULTI_GYRO
 756     if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
 757         return &gyro.gyroSensor2.gyroDev.dev;
 758     }
 759 #else
 760     UNUSED(whichSensor);
 761 #endif
 762     return &gyro.gyroSensor1.gyroDev.dev;
 763 }
 764
 765 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
 766 {
 767     return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg);
 768 }
 769 #endif // USE_GYRO_REGISTER_DUMP