src/main/sensors/gyro.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/feature.h"
  36
  37 #include "pg/pg.h"
  38 #include "pg/pg_ids.h"
  39
  40 #include "drivers/accgyro/accgyro.h"
  41 #include "drivers/accgyro/accgyro_fake.h"
  42 #include "drivers/accgyro/accgyro_mpu.h"
  43 #include "drivers/accgyro/accgyro_mpu3050.h"
  44 #include "drivers/accgyro/accgyro_mpu6050.h"
  45 #include "drivers/accgyro/accgyro_mpu6500.h"
  46 #include "drivers/accgyro/accgyro_spi_bmi160.h"
  47 #include "drivers/accgyro/accgyro_spi_icm20649.h"
  48 #include "drivers/accgyro/accgyro_spi_icm20689.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_L3G4200D
  54 #include "drivers/accgyro_legacy/accgyro_l3g4200d.h"
  55 #endif
  56
  57 #ifdef USE_GYRO_L3GD20
  58 #include "drivers/accgyro_legacy/accgyro_l3gd20.h"
  59 #endif
  60
  61 #include "drivers/accgyro/gyro_sync.h"
  62 #include "drivers/bus_spi.h"
  63 #include "drivers/io.h"
  64
  65 #include "fc/config.h"
  66 #include "fc/runtime_config.h"
  67
  68 #include "io/beeper.h"
  69 #include "io/statusindicator.h"
  70
  71 #include "scheduler/scheduler.h"
  72
  73 #include "sensors/boardalignment.h"
  74 #include "sensors/gyro.h"
  75 #include "sensors/gyroanalyse.h"
  76 #include "sensors/sensors.h"
  77
  78 #ifdef USE_HARDWARE_REVISION_DETECTION
  79 #include "hardware_revision.h"
  80 #endif
  81
  82 #if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
  83 #define USE_GYRO_SLEW_LIMITER
  84 #endif
  85
  86 FAST_RAM_ZERO_INIT gyro_t gyro;
  87 static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode;
  88
  89 static uint8_t gyroToUse = 0;
  90
  91 #ifdef USE_GYRO_OVERFLOW_CHECK
  92 static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
  93 #endif
  94 static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
  95 static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
  96 static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs;
  97 static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs;
  98
  99 static bool gyroHasOverflowProtection = true;
 100
 101 typedef struct gyroCalibration_s {
 102     float sum[XYZ_AXIS_COUNT];
 103     stdev_t var[XYZ_AXIS_COUNT];
 104     int32_t cyclesRemaining;
 105 } gyroCalibration_t;
 106
 107 bool firstArmingCalibrationWasStarted = false;
 108
 109 typedef union gyroLowpassFilter_u {
 110     pt1Filter_t pt1FilterState;
 111     biquadFilter_t biquadFilterState;
 112 } gyroLowpassFilter_t;
 113
 114 typedef struct gyroSensor_s {
 115     gyroDev_t gyroDev;
 116     gyroCalibration_t calibration;
 117
 118     // lowpass gyro soft filter
 119     filterApplyFnPtr lowpassFilterApplyFn;
 120     gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
 121
 122     // lowpass2 gyro soft filter
 123     filterApplyFnPtr lowpass2FilterApplyFn;
 124     gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
 125
 126     // notch filters
 127     filterApplyFnPtr notchFilter1ApplyFn;
 128     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
 129
 130     filterApplyFnPtr notchFilter2ApplyFn;
 131     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 132
 133     filterApplyFnPtr notchFilterDynApplyFn;
 134     biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
 135
 136     // overflow and recovery
 137     timeUs_t overflowTimeUs;
 138     bool overflowDetected;
 139 #ifdef USE_YAW_SPIN_RECOVERY
 140     timeUs_t yawSpinTimeUs;
 141     bool yawSpinDetected;
 142 #endif // USE_YAW_SPIN_RECOVERY
 143 } gyroSensor_t;
 144
 145 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
 146 #ifdef USE_DUAL_GYRO
 147 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2;
 148 #endif
 149
 150 #ifdef UNIT_TEST
 151 STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
 152 STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
 153 #endif
 154
 155 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 156 static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz);
 157
 158 #define DEBUG_GYRO_CALIBRATION 3
 159
 160 #ifdef STM32F10X
 161 #define GYRO_SYNC_DENOM_DEFAULT 8
 162 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 163    || defined(USE_GYRO_SPI_ICM20689)
 164 #define GYRO_SYNC_DENOM_DEFAULT 1
 165 #else
 166 #define GYRO_SYNC_DENOM_DEFAULT 4
 167 #endif
 168
 169 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 170 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 171
 172 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 4);
 173
 174 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 175 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
 176 #endif
 177
 178 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
 179     .gyro_align = ALIGN_DEFAULT,
 180     .gyroCalibrationDuration = 125,        // 1.25 seconds
 181     .gyroMovementCalibrationThreshold = 48,
 182     .gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT,
 183     .gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL,
 184     .gyro_32khz_hardware_lpf = GYRO_32KHZ_HARDWARE_LPF_NORMAL,
 185     .gyro_lowpass_type = FILTER_PT1,
 186     .gyro_lowpass_hz = 100,
 187     .gyro_lowpass2_type = FILTER_PT1,
 188     .gyro_lowpass2_hz = 300,
 189     .gyro_high_fsr = false,
 190     .gyro_use_32khz = false,
 191     .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT,
 192     .gyro_soft_notch_hz_1 = 0,
 193     .gyro_soft_notch_cutoff_1 = 0,
 194     .gyro_soft_notch_hz_2 = 0,
 195     .gyro_soft_notch_cutoff_2 = 0,
 196     .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES,
 197     .gyro_offset_yaw = 0,
 198     .yaw_spin_recovery = true,
 199     .yaw_spin_threshold = 1950,
 200 );
 201
 202
 203 const busDevice_t *gyroSensorBus(void)
 204 {
 205 #ifdef USE_DUAL_GYRO
 206     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 207         return &gyroSensor2.gyroDev.bus;
 208     } else {
 209         return &gyroSensor1.gyroDev.bus;
 210     }
 211 #else
 212     return &gyroSensor1.gyroDev.bus;
 213 #endif
 214 }
 215
 216 #ifdef USE_GYRO_REGISTER_DUMP
 217 const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
 218 {
 219 #ifdef USE_DUAL_GYRO
 220     if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
 221         return &gyroSensor2.gyroDev.bus;
 222     } else {
 223         return &gyroSensor1.gyroDev.bus;
 224     }
 225 #else
 226     UNUSED(whichSensor);
 227     return &gyroSensor1.gyroDev.bus;
 228 #endif
 229 }
 230 #endif // USE_GYRO_REGISTER_DUMP
 231
 232 const mpuConfiguration_t *gyroMpuConfiguration(void)
 233 {
 234 #ifdef USE_DUAL_GYRO
 235     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 236         return &gyroSensor2.gyroDev.mpuConfiguration;
 237     } else {
 238         return &gyroSensor1.gyroDev.mpuConfiguration;
 239     }
 240 #else
 241     return &gyroSensor1.gyroDev.mpuConfiguration;
 242 #endif
 243 }
 244
 245 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 246 {
 247 #ifdef USE_DUAL_GYRO
 248     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
 249         return &gyroSensor2.gyroDev.mpuDetectionResult;
 250     } else {
 251         return &gyroSensor1.gyroDev.mpuDetectionResult;
 252     }
 253 #else
 254     return &gyroSensor1.gyroDev.mpuDetectionResult;
 255 #endif
 256 }
 257
 258 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev)
 259 {
 260     gyroSensor_e gyroHardware = GYRO_DEFAULT;
 261
 262     switch (gyroHardware) {
 263     case GYRO_DEFAULT:
 264         FALLTHROUGH;
 265
 266 #ifdef USE_GYRO_MPU6050
 267     case GYRO_MPU6050:
 268         if (mpu6050GyroDetect(dev)) {
 269             gyroHardware = GYRO_MPU6050;
 270 #ifdef GYRO_MPU6050_ALIGN
 271             dev->gyroAlign = GYRO_MPU6050_ALIGN;
 272 #endif
 273             break;
 274         }
 275         FALLTHROUGH;
 276 #endif
 277
 278 #ifdef USE_GYRO_L3G4200D
 279     case GYRO_L3G4200D:
 280         if (l3g4200dDetect(dev)) {
 281             gyroHardware = GYRO_L3G4200D;
 282 #ifdef GYRO_L3G4200D_ALIGN
 283             dev->gyroAlign = GYRO_L3G4200D_ALIGN;
 284 #endif
 285             break;
 286         }
 287         FALLTHROUGH;
 288 #endif
 289
 290 #ifdef USE_GYRO_MPU3050
 291     case GYRO_MPU3050:
 292         if (mpu3050Detect(dev)) {
 293             gyroHardware = GYRO_MPU3050;
 294 #ifdef GYRO_MPU3050_ALIGN
 295             dev->gyroAlign = GYRO_MPU3050_ALIGN;
 296 #endif
 297             break;
 298         }
 299         FALLTHROUGH;
 300 #endif
 301
 302 #ifdef USE_GYRO_L3GD20
 303     case GYRO_L3GD20:
 304         if (l3gd20Detect(dev)) {
 305             gyroHardware = GYRO_L3GD20;
 306 #ifdef GYRO_L3GD20_ALIGN
 307             dev->gyroAlign = GYRO_L3GD20_ALIGN;
 308 #endif
 309             break;
 310         }
 311         FALLTHROUGH;
 312 #endif
 313
 314 #ifdef USE_GYRO_SPI_MPU6000
 315     case GYRO_MPU6000:
 316         if (mpu6000SpiGyroDetect(dev)) {
 317             gyroHardware = GYRO_MPU6000;
 318 #ifdef GYRO_MPU6000_ALIGN
 319             dev->gyroAlign = GYRO_MPU6000_ALIGN;
 320 #endif
 321             break;
 322         }
 323         FALLTHROUGH;
 324 #endif
 325
 326 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 327     case GYRO_MPU6500:
 328     case GYRO_ICM20601:
 329     case GYRO_ICM20602:
 330     case GYRO_ICM20608G:
 331 #ifdef USE_GYRO_SPI_MPU6500
 332         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 333 #else
 334         if (mpu6500GyroDetect(dev)) {
 335 #endif
 336             switch (dev->mpuDetectionResult.sensor) {
 337             case MPU_9250_SPI:
 338                 gyroHardware = GYRO_MPU9250;
 339                 break;
 340             case ICM_20601_SPI:
 341                 gyroHardware = GYRO_ICM20601;
 342                 break;
 343             case ICM_20602_SPI:
 344                 gyroHardware = GYRO_ICM20602;
 345                 break;
 346             case ICM_20608_SPI:
 347                 gyroHardware = GYRO_ICM20608G;
 348                 break;
 349             default:
 350                 gyroHardware = GYRO_MPU6500;
 351             }
 352 #ifdef GYRO_MPU6500_ALIGN
 353             dev->gyroAlign = GYRO_MPU6500_ALIGN;
 354 #endif
 355             break;
 356         }
 357         FALLTHROUGH;
 358 #endif
 359
 360 #ifdef USE_GYRO_SPI_MPU9250
 361     case GYRO_MPU9250:
 362         if (mpu9250SpiGyroDetect(dev)) {
 363             gyroHardware = GYRO_MPU9250;
 364 #ifdef GYRO_MPU9250_ALIGN
 365             dev->gyroAlign = GYRO_MPU9250_ALIGN;
 366 #endif
 367             break;
 368         }
 369         FALLTHROUGH;
 370 #endif
 371
 372 #ifdef USE_GYRO_SPI_ICM20649
 373     case GYRO_ICM20649:
 374         if (icm20649SpiGyroDetect(dev)) {
 375             gyroHardware = GYRO_ICM20649;
 376 #ifdef GYRO_ICM20649_ALIGN
 377             dev->gyroAlign = GYRO_ICM20649_ALIGN;
 378 #endif
 379             break;
 380         }
 381         FALLTHROUGH;
 382 #endif
 383
 384 #ifdef USE_GYRO_SPI_ICM20689
 385     case GYRO_ICM20689:
 386         if (icm20689SpiGyroDetect(dev)) {
 387             gyroHardware = GYRO_ICM20689;
 388 #ifdef GYRO_ICM20689_ALIGN
 389             dev->gyroAlign = GYRO_ICM20689_ALIGN;
 390 #endif
 391             break;
 392         }
 393         FALLTHROUGH;
 394 #endif
 395
 396 #ifdef USE_ACCGYRO_BMI160
 397     case GYRO_BMI160:
 398         if (bmi160SpiGyroDetect(dev)) {
 399             gyroHardware = GYRO_BMI160;
 400 #ifdef GYRO_BMI160_ALIGN
 401             dev->gyroAlign = GYRO_BMI160_ALIGN;
 402 #endif
 403             break;
 404         }
 405         FALLTHROUGH;
 406 #endif
 407
 408 #ifdef USE_FAKE_GYRO
 409     case GYRO_FAKE:
 410         if (fakeGyroDetect(dev)) {
 411             gyroHardware = GYRO_FAKE;
 412             break;
 413         }
 414         FALLTHROUGH;
 415 #endif
 416
 417     default:
 418         gyroHardware = GYRO_NONE;
 419     }
 420
 421     if (gyroHardware != GYRO_NONE) {
 422         detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 423         sensorsSet(SENSOR_GYRO);
 424     }
 425
 426
 427     return gyroHardware;
 428 }
 429
 430 static bool gyroInitSensor(gyroSensor_t *gyroSensor)
 431 {
 432     gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
 433
 434 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 435  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
 436
 437     mpuDetect(&gyroSensor->gyroDev);
 438     mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect
 439 #endif
 440
 441     const gyroSensor_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 442     gyroSensor->gyroDev.gyroHardware = gyroHardware;
 443     if (gyroHardware == GYRO_NONE) {
 444         return false;
 445     }
 446
 447     switch (gyroHardware) {
 448     case GYRO_MPU6500:
 449     case GYRO_MPU9250:
 450     case GYRO_ICM20601:
 451     case GYRO_ICM20602:
 452     case GYRO_ICM20608G:
 453     case GYRO_ICM20689:
 454         // do nothing, as gyro supports 32kHz
 455         break;
 456     default:
 457         // gyro does not support 32kHz
 458         gyroConfigMutable()->gyro_use_32khz = false;
 459         break;
 460     }
 461
 462     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
 463     gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom, gyroConfig()->gyro_use_32khz);
 464     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
 465     gyroSensor->gyroDev.hardware_32khz_lpf = gyroConfig()->gyro_32khz_hardware_lpf;
 466     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 467     if (gyroConfig()->gyro_align != ALIGN_DEFAULT) {
 468         gyroSensor->gyroDev.gyroAlign = gyroConfig()->gyro_align;
 469     }
 470
 471     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
 472     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
 473     switch (gyroHardware) {
 474     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
 475     case GYRO_DEFAULT:
 476     case GYRO_FAKE:
 477     case GYRO_MPU6050:
 478     case GYRO_L3G4200D:
 479     case GYRO_MPU3050:
 480     case GYRO_L3GD20:
 481     case GYRO_BMI160:
 482     case GYRO_MPU6000:
 483     case GYRO_MPU6500:
 484     case GYRO_MPU9250:
 485         gyroSensor->gyroDev.gyroHasOverflowProtection = true;
 486         break;
 487
 488     case GYRO_ICM20601:
 489     case GYRO_ICM20602:
 490     case GYRO_ICM20608G:
 491     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
 492     case GYRO_ICM20689:
 493         gyroSensor->gyroDev.gyroHasOverflowProtection = false;
 494         break;
 495
 496     default:
 497         gyroSensor->gyroDev.gyroHasOverflowProtection = false;  // default catch for newly added gyros until proven to be unaffected
 498         break;
 499     }
 500
 501     gyroInitSensorFilters(gyroSensor);
 502
 503 #ifdef USE_GYRO_DATA_ANALYSE
 504     gyroDataAnalyseInit(gyro.targetLooptime);
 505 #endif
 506     return true;
 507 }
 508
 509 bool gyroInit(void)
 510 {
 511 #ifdef USE_GYRO_OVERFLOW_CHECK
 512     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 513         overflowAxisMask = GYRO_OVERFLOW_Z;
 514     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 515         overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 516     } else {
 517         overflowAxisMask = 0;
 518     }
 519 #endif
 520
 521     switch (debugMode) {
 522     case DEBUG_FFT:
 523     case DEBUG_FFT_FREQ:
 524     case DEBUG_GYRO_RAW:
 525     case DEBUG_GYRO_SCALED:
 526     case DEBUG_GYRO_FILTERED:
 527         gyroDebugMode = debugMode;
 528         break;
 529     default:
 530         // debugMode is not gyro-related
 531         gyroDebugMode = DEBUG_NONE;
 532         break;
 533     }
 534     firstArmingCalibrationWasStarted = false;
 535
 536     bool ret = false;
 537     memset(&gyro, 0, sizeof(gyro));
 538     gyroToUse = gyroConfig()->gyro_to_use;
 539
 540 #if defined(USE_DUAL_GYRO) && defined(GYRO_1_CS_PIN)
 541     if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 542         gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_1_CS_PIN));
 543         IOInit(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(0));
 544         IOHi(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
 545         IOConfigGPIO(gyroSensor1.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
 546     }
 547 #endif
 548
 549 #if defined(USE_DUAL_GYRO) && defined(GYRO_2_CS_PIN)
 550     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 551         gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin = IOGetByTag(IO_TAG(GYRO_2_CS_PIN));
 552         IOInit(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, OWNER_MPU_CS, RESOURCE_INDEX(1));
 553         IOHi(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin); // Ensure device is disabled, important when two devices are on the same bus.
 554         IOConfigGPIO(gyroSensor2.gyroDev.bus.busdev_u.spi.csnPin, SPI_IO_CS_CFG);
 555     }
 556 #endif
 557
 558     gyroSensor1.gyroDev.gyroAlign = ALIGN_DEFAULT;
 559
 560 #if defined(GYRO_1_EXTI_PIN)
 561     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG(GYRO_1_EXTI_PIN);
 562 #elif defined(MPU_INT_EXTI)
 563     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG(MPU_INT_EXTI);
 564 #elif defined(USE_HARDWARE_REVISION_DETECTION)
 565     gyroSensor1.gyroDev.mpuIntExtiTag =  selectMPUIntExtiConfigByHardwareRevision();
 566 #else
 567     gyroSensor1.gyroDev.mpuIntExtiTag =  IO_TAG_NONE;
 568 #endif // GYRO_1_EXTI_PIN
 569 #ifdef USE_DUAL_GYRO
 570 #ifdef GYRO_1_ALIGN
 571     gyroSensor1.gyroDev.gyroAlign = GYRO_1_ALIGN;
 572 #endif
 573     gyroSensor1.gyroDev.bus.bustype = BUSTYPE_SPI;
 574     spiBusSetInstance(&gyroSensor1.gyroDev.bus, GYRO_1_SPI_INSTANCE);
 575     if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 576         ret = gyroInitSensor(&gyroSensor1);
 577         if (!ret) {
 578             return false; // TODO handle failure of first gyro detection better. - Perhaps update the config to use second gyro then indicate a new failure mode and reboot.
 579         }
 580         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
 581     }
 582 #else // USE_DUAL_GYRO
 583     ret = gyroInitSensor(&gyroSensor1);
 584     gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
 585 #endif // USE_DUAL_GYRO
 586
 587 #ifdef USE_DUAL_GYRO
 588
 589     gyroSensor2.gyroDev.gyroAlign = ALIGN_DEFAULT;
 590
 591 #if defined(GYRO_2_EXTI_PIN)
 592     gyroSensor2.gyroDev.mpuIntExtiTag =  IO_TAG(GYRO_2_EXTI_PIN);
 593 #elif defined(USE_HARDWARE_REVISION_DETECTION)
 594     gyroSensor2.gyroDev.mpuIntExtiTag =  selectMPUIntExtiConfigByHardwareRevision();
 595 #else
 596     gyroSensor2.gyroDev.mpuIntExtiTag =  IO_TAG_NONE;
 597 #endif // GYRO_2_EXTI_PIN
 598 #ifdef GYRO_2_ALIGN
 599     gyroSensor2.gyroDev.gyroAlign = GYRO_2_ALIGN;
 600 #endif
 601     gyroSensor2.gyroDev.bus.bustype = BUSTYPE_SPI;
 602     spiBusSetInstance(&gyroSensor2.gyroDev.bus, GYRO_2_SPI_INSTANCE);
 603     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 604         ret = gyroInitSensor(&gyroSensor2);
 605         if (!ret) {
 606             return false; // TODO handle failure of second gyro detection better. - Perhaps update the config to use first gyro then indicate a new failure mode and reboot.
 607         }
 608         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
 609     }
 610 #endif // USE_DUAL_GYRO
 611
 612 #ifdef USE_DUAL_GYRO
 613     // Only allow using both gyros simultaneously if they are the same hardware type.
 614     // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
 615     if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 616         if (gyroSensor1.gyroDev.gyroHardware != gyroSensor2.gyroDev.gyroHardware) {
 617             gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 618             gyroConfigMutable()->gyro_to_use = GYRO_CONFIG_USE_GYRO_1;
 619             detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
 620             sensorsSet(SENSOR_GYRO);
 621
 622         }
 623     }
 624 #endif // USE_DUAL_GYRO
 625     return ret;
 626 }
 627
 628 void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
 629 {
 630     filterApplyFnPtr *lowpassFilterApplyFn;
 631     gyroLowpassFilter_t *lowpassFilter = NULL;
 632
 633     switch (slot) {
 634     case FILTER_LOWPASS:
 635         lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
 636         lowpassFilter = gyroSensor->lowpassFilter;
 637         break;
 638
 639     case FILTER_LOWPASS2:
 640         lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
 641         lowpassFilter = gyroSensor->lowpass2Filter;
 642         break;
 643
 644     default:
 645         return;
 646     }
 647
 648     // Establish some common constants
 649     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 650     const float gyroDt = gyro.targetLooptime * 1e-6f;
 651
 652     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
 653     const float gain = pt1FilterGain(lpfHz, gyroDt);
 654
 655     // Dereference the pointer to null before checking valid cutoff and filter
 656     // type. It will be overridden for positive cases.
 657     *lowpassFilterApplyFn = &nullFilterApply;
 658
 659     // If lowpass cutoff has been specified and is less than the Nyquist frequency
 660     if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
 661         switch (type) {
 662         case FILTER_PT1:
 663             *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
 664             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 665                 pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
 666             }
 667             break;
 668         case FILTER_BIQUAD:
 669             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 670             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 671                 biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
 672             }
 673             break;
 674         }
 675     }
 676 }
 677
 678 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
 679 {
 680     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 681     if (notchHz > gyroFrequencyNyquist) {
 682         if (notchCutoffHz < gyroFrequencyNyquist) {
 683             notchHz = gyroFrequencyNyquist;
 684         } else {
 685             notchHz = 0;
 686         }
 687     }
 688
 689     return notchHz;
 690 }
 691
 692 #if defined(USE_GYRO_SLEW_LIMITER)
 693 void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
 694
 695     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 696         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
 697     }
 698 }
 699 #endif
 700
 701 static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 702 {
 703     gyroSensor->notchFilter1ApplyFn = nullFilterApply;
 704
 705     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 706
 707     if (notchHz != 0 && notchCutoffHz != 0) {
 708         gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 709         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 710         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 711             biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 712         }
 713     }
 714 }
 715
 716 static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 717 {
 718     gyroSensor->notchFilter2ApplyFn = nullFilterApply;
 719
 720     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 721
 722     if (notchHz != 0 && notchCutoffHz != 0) {
 723         gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 724         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 725         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 726             biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 727         }
 728     }
 729 }
 730
 731 #ifdef USE_GYRO_DATA_ANALYSE
 732 static bool isDynamicFilterActive(void)
 733 {
 734     return feature(FEATURE_DYNAMIC_FILTER);
 735 }
 736
 737 static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
 738 {
 739     gyroSensor->notchFilterDynApplyFn = nullFilterApply;
 740
 741     if (isDynamicFilterActive()) {
 742         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
 743         const float notchQ = filterGetNotchQ(400, 390); //just any init value
 744         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 745             biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 746         }
 747     }
 748 }
 749 #endif
 750
 751
 752 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 753 {
 754 #if defined(USE_GYRO_SLEW_LIMITER)
 755     gyroInitSlewLimiter(gyroSensor);
 756 #endif
 757
 758     gyroInitLowpassFilterLpf(
 759       gyroSensor,
 760       FILTER_LOWPASS,
 761       gyroConfig()->gyro_lowpass_type,
 762       gyroConfig()->gyro_lowpass_hz
 763     );
 764
 765     gyroInitLowpassFilterLpf(
 766       gyroSensor,
 767       FILTER_LOWPASS2,
 768       gyroConfig()->gyro_lowpass2_type,
 769       gyroConfig()->gyro_lowpass2_hz
 770     );
 771
 772     gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
 773     gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 774 #ifdef USE_GYRO_DATA_ANALYSE
 775     gyroInitFilterDynamicNotch(gyroSensor);
 776 #endif
 777 }
 778
 779 void gyroInitFilters(void)
 780 {
 781     gyroInitSensorFilters(&gyroSensor1);
 782 #ifdef USE_DUAL_GYRO
 783     gyroInitSensorFilters(&gyroSensor2);
 784 #endif
 785 }
 786
 787 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
 788 {
 789     return gyroSensor->calibration.cyclesRemaining == 0;
 790 }
 791
 792 FAST_CODE bool isGyroCalibrationComplete(void)
 793 {
 794 #ifdef USE_DUAL_GYRO
 795     switch (gyroToUse) {
 796         default:
 797         case GYRO_CONFIG_USE_GYRO_1: {
 798             return isGyroSensorCalibrationComplete(&gyroSensor1);
 799         }
 800         case GYRO_CONFIG_USE_GYRO_2: {
 801             return isGyroSensorCalibrationComplete(&gyroSensor2);
 802         }
 803         case GYRO_CONFIG_USE_GYRO_BOTH: {
 804             return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2);
 805         }
 806     }
 807 #else
 808     return isGyroSensorCalibrationComplete(&gyroSensor1);
 809 #endif
 810 }
 811
 812 static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 813 {
 814     return gyroCalibration->cyclesRemaining == 1;
 815 }
 816
 817 static int32_t gyroCalculateCalibratingCycles(void)
 818 {
 819     return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.targetLooptime;
 820 }
 821
 822 static bool isOnFirstGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
 823 {
 824     return gyroCalibration->cyclesRemaining == gyroCalculateCalibratingCycles();
 825 }
 826
 827 static void gyroSetCalibrationCycles(gyroSensor_t *gyroSensor)
 828 {
 829     gyroSensor->calibration.cyclesRemaining = gyroCalculateCalibratingCycles();
 830 }
 831
 832 void gyroStartCalibration(bool isFirstArmingCalibration)
 833 {
 834     if (!(isFirstArmingCalibration && firstArmingCalibrationWasStarted)) {
 835         gyroSetCalibrationCycles(&gyroSensor1);
 836 #ifdef USE_DUAL_GYRO
 837         gyroSetCalibrationCycles(&gyroSensor2);
 838 #endif
 839
 840         if (isFirstArmingCalibration) {
 841             firstArmingCalibrationWasStarted = true;
 842         }
 843     }
 844 }
 845
 846 bool isFirstArmingGyroCalibrationRunning(void)
 847 {
 848     return firstArmingCalibrationWasStarted && !isGyroCalibrationComplete();
 849 }
 850
 851 STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
 852 {
 853     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 854         // Reset g[axis] at start of calibration
 855         if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
 856             gyroSensor->calibration.sum[axis] = 0.0f;
 857             devClear(&gyroSensor->calibration.var[axis]);
 858             // gyroZero is set to zero until calibration complete
 859             gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
 860         }
 861
 862         // Sum up CALIBRATING_GYRO_TIME_US readings
 863         gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
 864         devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
 865
 866         if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 867             const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
 868             // DEBUG_GYRO_CALIBRATION records the standard deviation of roll
 869             // into the spare field - debug[3], in DEBUG_GYRO_RAW
 870             if (axis == X) {
 871                 DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
 872             }
 873
 874             // check deviation and startover in case the model was moved
 875             if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
 876                 gyroSetCalibrationCycles(gyroSensor);
 877                 return;
 878             }
 879
 880             // please take care with exotic boardalignment !!
 881             gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
 882             if (axis == Z) {
 883               gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
 884             }
 885         }
 886     }
 887
 888     if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
 889         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
 890         if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
 891             beeper(BEEPER_GYRO_CALIBRATED);
 892         }
 893     }
 894     --gyroSensor->calibration.cyclesRemaining;
 895
 896 }
 897
 898 #if defined(USE_GYRO_SLEW_LIMITER)
 899 FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
 900 {
 901     int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
 902     if (gyroConfig()->checkOverflow || gyroHasOverflowProtection) {
 903         // don't use the slew limiter if overflow checking is on or gyro is not subject to overflow bug
 904         return ret;
 905     }
 906     if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
 907         // there has been a large change in value, so assume overflow has occurred and return the previous value
 908         ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
 909     } else {
 910         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
 911     }
 912     return ret;
 913 }
 914 #endif
 915
 916 #ifdef USE_GYRO_OVERFLOW_CHECK
 917 static FAST_CODE_NOINLINE void handleOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 918 {
 919     const float gyroOverflowResetRate = GYRO_OVERFLOW_RESET_THRESHOLD * gyroSensor->gyroDev.scale;
 920     if ((abs(gyro.gyroADCf[X]) < gyroOverflowResetRate)
 921           && (abs(gyro.gyroADCf[Y]) < gyroOverflowResetRate)
 922           && (abs(gyro.gyroADCf[Z]) < gyroOverflowResetRate)) {
 923         // if we have 50ms of consecutive OK gyro vales, then assume yaw readings are OK again and reset overflowDetected
 924         // reset requires good OK values on all axes
 925         if (cmpTimeUs(currentTimeUs, gyroSensor->overflowTimeUs) > 50000) {
 926             gyroSensor->overflowDetected = false;
 927         }
 928     } else {
 929         // not a consecutive OK value, so reset the overflow time
 930         gyroSensor->overflowTimeUs = currentTimeUs;
 931     }
 932 }
 933
 934 static FAST_CODE void checkForOverflow(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 935 {
 936     // check for overflow to handle Yaw Spin To The Moon (YSTTM)
 937     // ICM gyros are specified to +/- 2000 deg/sec, in a crash they can go out of spec.
 938     // This can cause an overflow and sign reversal in the output.
 939     // Overflow and sign reversal seems to result in a gyro value of +1996 or -1996.
 940     if (gyroSensor->overflowDetected) {
 941         handleOverflow(gyroSensor, currentTimeUs);
 942     } else {
 943 #ifndef SIMULATOR_BUILD
 944         // check for overflow in the axes set in overflowAxisMask
 945         gyroOverflow_e overflowCheck = GYRO_OVERFLOW_NONE;
 946         const float gyroOverflowTriggerRate = GYRO_OVERFLOW_TRIGGER_THRESHOLD * gyroSensor->gyroDev.scale;
 947         if (abs(gyro.gyroADCf[X]) > gyroOverflowTriggerRate) {
 948             overflowCheck |= GYRO_OVERFLOW_X;
 949         }
 950         if (abs(gyro.gyroADCf[Y]) > gyroOverflowTriggerRate) {
 951             overflowCheck |= GYRO_OVERFLOW_Y;
 952         }
 953         if (abs(gyro.gyroADCf[Z]) > gyroOverflowTriggerRate) {
 954             overflowCheck |= GYRO_OVERFLOW_Z;
 955         }
 956         if (overflowCheck & overflowAxisMask) {
 957             gyroSensor->overflowDetected = true;
 958             gyroSensor->overflowTimeUs = currentTimeUs;
 959 #ifdef USE_YAW_SPIN_RECOVERY
 960             gyroSensor->yawSpinDetected = false;
 961 #endif // USE_YAW_SPIN_RECOVERY
 962         }
 963 #endif // SIMULATOR_BUILD
 964     }
 965 }
 966 #endif // USE_GYRO_OVERFLOW_CHECK
 967
 968 #ifdef USE_YAW_SPIN_RECOVERY
 969 static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 970 {
 971     const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
 972     if (abs(gyro.gyroADCf[Z]) < yawSpinResetRate) {
 973         // testing whether 20ms of consecutive OK gyro yaw values is enough
 974         if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
 975             gyroSensor->yawSpinDetected = false;
 976         }
 977     } else {
 978         // reset the yaw spin time
 979         gyroSensor->yawSpinTimeUs = currentTimeUs;
 980     }
 981 }
 982
 983 static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
 984 {
 985     // if not in overflow mode, handle yaw spins above threshold
 986 #ifdef USE_GYRO_OVERFLOW_CHECK
 987     if (gyroSensor->overflowDetected) {
 988         gyroSensor->yawSpinDetected = false;
 989         return;
 990     }
 991 #endif // USE_GYRO_OVERFLOW_CHECK
 992
 993     if (gyroSensor->yawSpinDetected) {
 994         handleYawSpin(gyroSensor, currentTimeUs);
 995     } else {
 996 #ifndef SIMULATOR_BUILD
 997         // check for spin on yaw axis only
 998          if (abs(gyro.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
 999             gyroSensor->yawSpinDetected = true;
1000             gyroSensor->yawSpinTimeUs = currentTimeUs;
1001         }
1002 #endif // SIMULATOR_BUILD
1003     }
1004 }
1005 #endif // USE_YAW_SPIN_RECOVERY
1006
1007 #define GYRO_FILTER_FUNCTION_NAME filterGyro
1008 #define GYRO_FILTER_DEBUG_SET(...)
1009 #include "gyro_filter_impl.h"
1010 #undef GYRO_FILTER_FUNCTION_NAME
1011 #undef GYRO_FILTER_DEBUG_SET
1012
1013 #define GYRO_FILTER_FUNCTION_NAME filterGyroDebug
1014 #define GYRO_FILTER_DEBUG_SET DEBUG_SET
1015 #include "gyro_filter_impl.h"
1016 #undef GYRO_FILTER_FUNCTION_NAME
1017 #undef GYRO_FILTER_DEBUG_SET
1018
1019 static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
1020 {
1021     if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
1022         return;
1023     }
1024     gyroSensor->gyroDev.dataReady = false;
1025
1026     if (isGyroSensorCalibrationComplete(gyroSensor)) {
1027         // move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
1028
1029 #if defined(USE_GYRO_SLEW_LIMITER)
1030         gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
1031         gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
1032         gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
1033 #else
1034         gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
1035         gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
1036         gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
1037 #endif
1038
1039         alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
1040     } else {
1041         performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
1042         // still calibrating, so no need to further process gyro data
1043         return;
1044     }
1045
1046     const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
1047     accumulationLastTimeSampledUs = currentTimeUs;
1048     accumulatedMeasurementTimeUs += sampleDeltaUs;
1049
1050 #ifdef USE_GYRO_OVERFLOW_CHECK
1051     if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
1052         checkForOverflow(gyroSensor, currentTimeUs);
1053     }
1054 #endif
1055
1056 #ifdef USE_YAW_SPIN_RECOVERY
1057     if (gyroConfig()->yaw_spin_recovery) {
1058         checkForYawSpin(gyroSensor, currentTimeUs);
1059     }
1060 #endif
1061
1062     if (gyroDebugMode == DEBUG_NONE) {
1063         filterGyro(gyroSensor, sampleDeltaUs);
1064     } else {
1065         filterGyroDebug(gyroSensor, sampleDeltaUs);
1066     }
1067 #ifdef USE_GYRO_DATA_ANALYSE
1068     if (isDynamicFilterActive()) {
1069         gyroDataAnalyse(gyroSensor->notchFilterDyn);
1070     }
1071 #endif
1072 }
1073
1074 FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
1075 {
1076 #ifdef USE_DUAL_GYRO
1077     switch (gyroToUse) {
1078     case GYRO_CONFIG_USE_GYRO_1:
1079         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1080         if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
1081             gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
1082             gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
1083             gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
1084         }
1085         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1086         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1087         DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1088         DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1089         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
1090         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
1091         break;
1092     case GYRO_CONFIG_USE_GYRO_2:
1093         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1094         if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
1095             gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X];
1096             gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
1097             gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z];
1098         }
1099         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1100         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1101         DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1102         DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1103         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X]));
1104         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y]));
1105         break;
1106     case GYRO_CONFIG_USE_GYRO_BOTH:
1107         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1108         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1109         if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) {
1110             gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f;
1111             gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f;
1112             gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f;
1113         }
1114         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1115         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1116         DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1117         DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1118         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1119         DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1120         DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1121         DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1122         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X]));
1123         DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y]));
1124         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X]));
1125         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
1126         DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
1127         break;
1128     }
1129 #else
1130     gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1131     gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
1132     gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
1133     gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
1134 #endif
1135 }
1136
1137 bool gyroGetAccumulationAverage(float *accumulationAverage)
1138 {
1139     if (accumulatedMeasurementTimeUs > 0) {
1140         // If we have gyro data accumulated, calculate average rate that will yield the same rotation
1141         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1142             accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
1143             accumulatedMeasurements[axis] = 0.0f;
1144         }
1145         accumulatedMeasurementTimeUs = 0;
1146         return true;
1147     } else {
1148         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1149             accumulationAverage[axis] = 0.0f;
1150         }
1151         return false;
1152     }
1153 }
1154
1155 void gyroReadTemperature(void)
1156 {
1157     if (gyroSensor1.gyroDev.temperatureFn) {
1158         gyroSensor1.gyroDev.temperatureFn(&gyroSensor1.gyroDev, &gyroSensor1.gyroDev.temperature);
1159     }
1160 }
1161
1162 int16_t gyroGetTemperature(void)
1163 {
1164     return gyroSensor1.gyroDev.temperature;
1165 }
1166
1167 int16_t gyroRateDps(int axis)
1168 {
1169 #ifdef USE_DUAL_GYRO
1170     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
1171         return lrintf(gyro.gyroADCf[axis] / gyroSensor2.gyroDev.scale);
1172     } else {
1173         return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
1174     }
1175 #else
1176     return lrintf(gyro.gyroADCf[axis] / gyroSensor1.gyroDev.scale);
1177 #endif
1178 }
1179
1180 bool gyroOverflowDetected(void)
1181 {
1182     return gyroSensor1.overflowDetected;
1183 }
1184
1185 #ifdef USE_YAW_SPIN_RECOVERY
1186 bool gyroYawSpinDetected(void)
1187 {
1188     return gyroSensor1.yawSpinDetected;
1189 }
1190 #endif // USE_YAW_SPIN_RECOVERY
1191
1192 uint16_t gyroAbsRateDps(int axis)
1193 {
1194     return fabsf(gyro.gyroADCf[axis]);
1195 }
1196
1197 #ifdef USE_GYRO_REGISTER_DUMP
1198 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
1199 {
1200     return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
1201 }
1202 #endif // USE_GYRO_REGISTER_DUMP