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