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