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 #include "io/beeper.h"
  70 #include "io/statusindicator.h"
  71
  72 #include "scheduler/scheduler.h"
  73
  74 #include "sensors/boardalignment.h"
  75 #include "sensors/gyro.h"
  76 #ifdef USE_GYRO_DATA_ANALYSE
  77 #include "sensors/gyroanalyse.h"
  78 #endif
  79 #include "sensors/rpm_filter.h"
  80 #include "sensors/sensors.h"
  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 FAST_RAM_ZERO_INIT uint8_t gyroToUse;
  90 static FAST_RAM_ZERO_INIT bool overflowDetected;
  91
  92 #ifdef USE_GYRO_OVERFLOW_CHECK
  93 static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask;
  94 #endif
  95
  96 #ifdef USE_YAW_SPIN_RECOVERY
  97 static FAST_RAM_ZERO_INIT bool yawSpinDetected;
  98 #endif
  99
 100 static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT];
 101 static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT];
 102 static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs;
 103 static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs;
 104
 105 static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature;
 106
 107 static bool gyroHasOverflowProtection = true;
 108
 109 static FAST_RAM_ZERO_INIT bool useDualGyroDebugging;
 110
 111 typedef struct gyroCalibration_s {
 112     float sum[XYZ_AXIS_COUNT];
 113     stdev_t var[XYZ_AXIS_COUNT];
 114     int32_t cyclesRemaining;
 115 } gyroCalibration_t;
 116
 117 bool firstArmingCalibrationWasStarted = false;
 118
 119 typedef union gyroLowpassFilter_u {
 120     pt1Filter_t pt1FilterState;
 121     biquadFilter_t biquadFilterState;
 122 } gyroLowpassFilter_t;
 123
 124 typedef struct gyroSensor_s {
 125     gyroDev_t gyroDev;
 126     gyroCalibration_t calibration;
 127
 128     // lowpass gyro soft filter
 129     filterApplyFnPtr lowpassFilterApplyFn;
 130     gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT];
 131
 132     // lowpass2 gyro soft filter
 133     filterApplyFnPtr lowpass2FilterApplyFn;
 134     gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT];
 135
 136     // notch filters
 137     filterApplyFnPtr notchFilter1ApplyFn;
 138     biquadFilter_t notchFilter1[XYZ_AXIS_COUNT];
 139
 140     filterApplyFnPtr notchFilter2ApplyFn;
 141     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 142
 143     filterApplyFnPtr notchFilterDynApplyFn;
 144     filterApplyFnPtr notchFilterDynApplyFn2;
 145     biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT];
 146     biquadFilter_t notchFilterDyn2[XYZ_AXIS_COUNT];
 147
 148     // overflow and recovery
 149     timeUs_t overflowTimeUs;
 150     bool overflowDetected;
 151 #ifdef USE_YAW_SPIN_RECOVERY
 152     timeUs_t yawSpinTimeUs;
 153     bool yawSpinDetected;
 154 #endif // USE_YAW_SPIN_RECOVERY
 155
 156 #ifdef USE_GYRO_DATA_ANALYSE
 157 #define DYNAMIC_NOTCH_DEFAULT_CENTER_HZ 350
 158 #define DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ 300
 159     gyroAnalyseState_t gyroAnalyseState;
 160 #endif
 161
 162     flight_dynamics_index_t gyroDebugAxis;
 163 } gyroSensor_t;
 164
 165 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1;
 166 #ifdef USE_MULTI_GYRO
 167 STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2;
 168 #endif
 169
 170 static gyroDetectionFlags_t gyroDetectionFlags = NO_GYROS_DETECTED;
 171
 172 #ifdef UNIT_TEST
 173 STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyroSensor1;
 174 STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev;
 175 #endif
 176
 177 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor);
 178 static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz);
 179
 180 #define DEBUG_GYRO_CALIBRATION 3
 181
 182 #ifdef STM32F10X
 183 #define GYRO_SYNC_DENOM_DEFAULT 8
 184 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
 185    || defined(USE_GYRO_SPI_ICM20689)
 186 #define GYRO_SYNC_DENOM_DEFAULT 1
 187 #else
 188 #define GYRO_SYNC_DENOM_DEFAULT 3
 189 #endif
 190
 191 #define GYRO_OVERFLOW_TRIGGER_THRESHOLD 31980  // 97.5% full scale (1950dps for 2000dps gyro)
 192 #define GYRO_OVERFLOW_RESET_THRESHOLD 30340    // 92.5% full scale (1850dps for 2000dps gyro)
 193
 194 PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 7);
 195
 196 #ifndef GYRO_CONFIG_USE_GYRO_DEFAULT
 197 #define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1
 198 #endif
 199
 200 void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
 201 {
 202     gyroConfig->gyroCalibrationDuration = 125;        // 1.25 seconds
 203     gyroConfig->gyroMovementCalibrationThreshold = 48;
 204     gyroConfig->gyro_sync_denom = GYRO_SYNC_DENOM_DEFAULT;
 205     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
 206     gyroConfig->gyro_lowpass_type = FILTER_BIQUAD;
 207     gyroConfig->gyro_lowpass_hz = 150;  // NOTE: dynamic lpf is enabled by default so this setting is actually
 208                                         // overridden and the static lowpass 1 is disabled. We can't set this
 209                                         // value to 0 otherwise Configurator versions 10.4 and earlier will also
 210                                         // reset the lowpass filter type to PT1 overriding the desired BIQUAD setting.
 211     gyroConfig->gyro_lowpass2_type = FILTER_PT1;
 212     gyroConfig->gyro_lowpass2_hz = 150;
 213     gyroConfig->gyro_high_fsr = false;
 214     gyroConfig->gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT;
 215     gyroConfig->gyro_soft_notch_hz_1 = 0;
 216     gyroConfig->gyro_soft_notch_cutoff_1 = 0;
 217     gyroConfig->gyro_soft_notch_hz_2 = 0;
 218     gyroConfig->gyro_soft_notch_cutoff_2 = 0;
 219     gyroConfig->checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES;
 220     gyroConfig->gyro_offset_yaw = 0;
 221     gyroConfig->yaw_spin_recovery = true;
 222     gyroConfig->yaw_spin_threshold = 1950;
 223     gyroConfig->dyn_lpf_gyro_min_hz = 150;
 224     gyroConfig->dyn_lpf_gyro_max_hz = 450;
 225     gyroConfig->dyn_notch_range = DYN_NOTCH_RANGE_AUTO;
 226     gyroConfig->dyn_notch_width_percent = 8;
 227     gyroConfig->dyn_notch_q = 120;
 228     gyroConfig->dyn_notch_min_hz = 150;
 229     gyroConfig->gyro_filter_debug_axis = FD_ROLL;
 230 }
 231
 232 #ifdef USE_MULTI_GYRO
 233 #define ACTIVE_GYRO ((gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyroSensor2 : &gyroSensor1)
 234 #else
 235 #define ACTIVE_GYRO (&gyroSensor1)
 236 #endif
 237
 238 const busDevice_t *gyroSensorBus(void)
 239 {
 240     return &ACTIVE_GYRO->gyroDev.bus;
 241 }
 242
 243 #ifdef USE_GYRO_REGISTER_DUMP
 244 const busDevice_t *gyroSensorBusByDevice(uint8_t whichSensor)
 245 {
 246 #ifdef USE_MULTI_GYRO
 247     if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
 248         return &gyroSensor2.gyroDev.bus;
 249     }
 250 #else
 251     UNUSED(whichSensor);
 252 #endif
 253     return &gyroSensor1.gyroDev.bus;
 254 }
 255 #endif // USE_GYRO_REGISTER_DUMP
 256
 257 const mpuDetectionResult_t *gyroMpuDetectionResult(void)
 258 {
 259     return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
 260 }
 261
 262 STATIC_UNIT_TESTED gyroHardware_e gyroDetect(gyroDev_t *dev)
 263 {
 264     gyroHardware_e gyroHardware = GYRO_DEFAULT;
 265
 266     switch (gyroHardware) {
 267     case GYRO_DEFAULT:
 268         FALLTHROUGH;
 269
 270 #ifdef USE_GYRO_MPU6050
 271     case GYRO_MPU6050:
 272         if (mpu6050GyroDetect(dev)) {
 273             gyroHardware = GYRO_MPU6050;
 274             break;
 275         }
 276         FALLTHROUGH;
 277 #endif
 278
 279 #ifdef USE_GYRO_L3G4200D
 280     case GYRO_L3G4200D:
 281         if (l3g4200dDetect(dev)) {
 282             gyroHardware = GYRO_L3G4200D;
 283             break;
 284         }
 285         FALLTHROUGH;
 286 #endif
 287
 288 #ifdef USE_GYRO_MPU3050
 289     case GYRO_MPU3050:
 290         if (mpu3050Detect(dev)) {
 291             gyroHardware = GYRO_MPU3050;
 292             break;
 293         }
 294         FALLTHROUGH;
 295 #endif
 296
 297 #ifdef USE_GYRO_L3GD20
 298     case GYRO_L3GD20:
 299         if (l3gd20GyroDetect(dev)) {
 300             gyroHardware = GYRO_L3GD20;
 301             break;
 302         }
 303         FALLTHROUGH;
 304 #endif
 305
 306 #ifdef USE_GYRO_SPI_MPU6000
 307     case GYRO_MPU6000:
 308         if (mpu6000SpiGyroDetect(dev)) {
 309             gyroHardware = GYRO_MPU6000;
 310             break;
 311         }
 312         FALLTHROUGH;
 313 #endif
 314
 315 #if defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500)
 316     case GYRO_MPU6500:
 317     case GYRO_ICM20601:
 318     case GYRO_ICM20602:
 319     case GYRO_ICM20608G:
 320 #ifdef USE_GYRO_SPI_MPU6500
 321         if (mpu6500GyroDetect(dev) || mpu6500SpiGyroDetect(dev)) {
 322 #else
 323         if (mpu6500GyroDetect(dev)) {
 324 #endif
 325             switch (dev->mpuDetectionResult.sensor) {
 326             case MPU_9250_SPI:
 327                 gyroHardware = GYRO_MPU9250;
 328                 break;
 329             case ICM_20601_SPI:
 330                 gyroHardware = GYRO_ICM20601;
 331                 break;
 332             case ICM_20602_SPI:
 333                 gyroHardware = GYRO_ICM20602;
 334                 break;
 335             case ICM_20608_SPI:
 336                 gyroHardware = GYRO_ICM20608G;
 337                 break;
 338             default:
 339                 gyroHardware = GYRO_MPU6500;
 340             }
 341             break;
 342         }
 343         FALLTHROUGH;
 344 #endif
 345
 346 #ifdef USE_GYRO_SPI_MPU9250
 347     case GYRO_MPU9250:
 348         if (mpu9250SpiGyroDetect(dev)) {
 349             gyroHardware = GYRO_MPU9250;
 350             break;
 351         }
 352         FALLTHROUGH;
 353 #endif
 354
 355 #ifdef USE_GYRO_SPI_ICM20649
 356     case GYRO_ICM20649:
 357         if (icm20649SpiGyroDetect(dev)) {
 358             gyroHardware = GYRO_ICM20649;
 359             break;
 360         }
 361         FALLTHROUGH;
 362 #endif
 363
 364 #ifdef USE_GYRO_SPI_ICM20689
 365     case GYRO_ICM20689:
 366         if (icm20689SpiGyroDetect(dev)) {
 367             gyroHardware = GYRO_ICM20689;
 368             break;
 369         }
 370         FALLTHROUGH;
 371 #endif
 372
 373 #ifdef USE_ACCGYRO_BMI160
 374     case GYRO_BMI160:
 375         if (bmi160SpiGyroDetect(dev)) {
 376             gyroHardware = GYRO_BMI160;
 377             break;
 378         }
 379         FALLTHROUGH;
 380 #endif
 381
 382 #ifdef USE_FAKE_GYRO
 383     case GYRO_FAKE:
 384         if (fakeGyroDetect(dev)) {
 385             gyroHardware = GYRO_FAKE;
 386             break;
 387         }
 388         FALLTHROUGH;
 389 #endif
 390
 391     default:
 392         gyroHardware = GYRO_NONE;
 393     }
 394
 395     if (gyroHardware != GYRO_NONE) {
 396         sensorsSet(SENSOR_GYRO);
 397     }
 398
 399
 400     return gyroHardware;
 401 }
 402
 403 static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
 404 {
 405 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 406  || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689)
 407     mpuPreInit(config);
 408 #else
 409     UNUSED(config);
 410 #endif
 411 }
 412
 413 static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 414 {
 415 #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
 416  || 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)
 417
 418     if (!mpuDetect(&gyroSensor->gyroDev, config)) {
 419         return false;
 420     }
 421 #else
 422     UNUSED(config);
 423 #endif
 424
 425     const gyroHardware_e gyroHardware = gyroDetect(&gyroSensor->gyroDev);
 426     gyroSensor->gyroDev.gyroHardware = gyroHardware;
 427
 428     return gyroHardware != GYRO_NONE;
 429 }
 430
 431 static void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
 432 {
 433     gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
 434     gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
 435     gyroSensor->gyroDev.gyroAlign = config->align;
 436     gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
 437
 438     // Must set gyro targetLooptime before gyroDev.init and initialisation of filters
 439     gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
 440     gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
 441     gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
 442
 443     // As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
 444     // Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
 445     switch (gyroSensor->gyroDev.gyroHardware) {
 446     case GYRO_NONE:    // Won't ever actually get here, but included to account for all gyro types
 447     case GYRO_DEFAULT:
 448     case GYRO_FAKE:
 449     case GYRO_MPU6050:
 450     case GYRO_L3G4200D:
 451     case GYRO_MPU3050:
 452     case GYRO_L3GD20:
 453     case GYRO_BMI160:
 454     case GYRO_MPU6000:
 455     case GYRO_MPU6500:
 456     case GYRO_MPU9250:
 457         gyroSensor->gyroDev.gyroHasOverflowProtection = true;
 458         break;
 459
 460     case GYRO_ICM20601:
 461     case GYRO_ICM20602:
 462     case GYRO_ICM20608G:
 463     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
 464     case GYRO_ICM20689:
 465         gyroSensor->gyroDev.gyroHasOverflowProtection = false;
 466         break;
 467
 468     default:
 469         gyroSensor->gyroDev.gyroHasOverflowProtection = false;  // default catch for newly added gyros until proven to be unaffected
 470         break;
 471     }
 472
 473     gyroInitSensorFilters(gyroSensor);
 474
 475 #ifdef USE_GYRO_DATA_ANALYSE
 476     gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
 477
 478 #endif
 479 }
 480
 481 void gyroPreInit(void)
 482 {
 483     gyroPreInitSensor(gyroDeviceConfig(0));
 484 #ifdef USE_MULTI_GYRO
 485     gyroPreInitSensor(gyroDeviceConfig(1));
 486 #endif
 487 }
 488
 489 bool gyroInit(void)
 490 {
 491 #ifdef USE_GYRO_OVERFLOW_CHECK
 492     if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
 493         overflowAxisMask = GYRO_OVERFLOW_Z;
 494     } else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
 495         overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
 496     } else {
 497         overflowAxisMask = 0;
 498     }
 499 #endif
 500
 501     gyroDebugMode = DEBUG_NONE;
 502     useDualGyroDebugging = false;
 503
 504     switch (debugMode) {
 505     case DEBUG_FFT:
 506     case DEBUG_FFT_FREQ:
 507     case DEBUG_GYRO_RAW:
 508     case DEBUG_GYRO_SCALED:
 509     case DEBUG_GYRO_FILTERED:
 510     case DEBUG_DYN_LPF:
 511         gyroDebugMode = debugMode;
 512         break;
 513     case DEBUG_DUAL_GYRO:
 514     case DEBUG_DUAL_GYRO_COMBINE:
 515     case DEBUG_DUAL_GYRO_DIFF:
 516     case DEBUG_DUAL_GYRO_RAW:
 517     case DEBUG_DUAL_GYRO_SCALED:
 518         useDualGyroDebugging = true;
 519         break;
 520     }
 521     firstArmingCalibrationWasStarted = false;
 522
 523     gyroDetectionFlags = NO_GYROS_DETECTED;
 524
 525     gyroToUse = gyroConfig()->gyro_to_use;
 526
 527     if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
 528         gyroDetectionFlags |= DETECTED_GYRO_1;
 529     }
 530
 531 #if defined(USE_MULTI_GYRO)
 532     if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) {
 533         gyroDetectionFlags |= DETECTED_GYRO_2;
 534     }
 535 #endif
 536
 537     if (gyroDetectionFlags == NO_GYROS_DETECTED) {
 538         return false;
 539     }
 540
 541 #if defined(USE_MULTI_GYRO)
 542     if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS))
 543         || (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
 544         || (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
 545         if (gyroDetectionFlags & DETECTED_GYRO_1) {
 546             gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 547         } else {
 548             gyroToUse = GYRO_CONFIG_USE_GYRO_2;
 549         }
 550
 551         gyroConfigMutable()->gyro_to_use = gyroToUse;
 552     }
 553
 554     // Only allow using both gyros simultaneously if they are the same hardware type.
 555     if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) {
 556         gyroDetectionFlags |= DETECTED_DUAL_GYROS;
 557     } else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 558         // If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
 559         gyroToUse = GYRO_CONFIG_USE_GYRO_1;
 560         gyroConfigMutable()->gyro_to_use = gyroToUse;
 561     }
 562
 563     if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 564         gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
 565         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
 566         detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
 567     }
 568 #endif
 569
 570     if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
 571         gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
 572         gyroHasOverflowProtection =  gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
 573         detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
 574     }
 575
 576     return true;
 577 }
 578
 579 gyroDetectionFlags_t getGyroDetectionFlags(void)
 580 {
 581     return gyroDetectionFlags;
 582 }
 583
 584 #ifdef USE_DYN_LPF
 585 static FAST_RAM uint8_t dynLpfFilter = DYN_LPF_NONE;
 586 static FAST_RAM_ZERO_INIT uint16_t dynLpfMin;
 587 static FAST_RAM_ZERO_INIT uint16_t dynLpfMax;
 588
 589 static void dynLpfFilterInit()
 590 {
 591     if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
 592         switch (gyroConfig()->gyro_lowpass_type) {
 593         case FILTER_PT1:
 594             dynLpfFilter = DYN_LPF_PT1;
 595             break;
 596         case FILTER_BIQUAD:
 597             dynLpfFilter = DYN_LPF_BIQUAD;
 598             break;
 599         default:
 600             dynLpfFilter = DYN_LPF_NONE;
 601             break;
 602         }
 603     } else {
 604         dynLpfFilter = DYN_LPF_NONE;
 605     }
 606     dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
 607     dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
 608 }
 609 #endif
 610
 611 void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type, uint16_t lpfHz)
 612 {
 613     filterApplyFnPtr *lowpassFilterApplyFn;
 614     gyroLowpassFilter_t *lowpassFilter = NULL;
 615
 616     switch (slot) {
 617     case FILTER_LOWPASS:
 618         lowpassFilterApplyFn = &gyroSensor->lowpassFilterApplyFn;
 619         lowpassFilter = gyroSensor->lowpassFilter;
 620         break;
 621
 622     case FILTER_LOWPASS2:
 623         lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn;
 624         lowpassFilter = gyroSensor->lowpass2Filter;
 625         break;
 626
 627     default:
 628         return;
 629     }
 630
 631     // Establish some common constants
 632     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 633     const float gyroDt = gyro.targetLooptime * 1e-6f;
 634
 635     // Gain could be calculated a little later as it is specific to the pt1/bqrcf2/fkf branches
 636     const float gain = pt1FilterGain(lpfHz, gyroDt);
 637
 638     // Dereference the pointer to null before checking valid cutoff and filter
 639     // type. It will be overridden for positive cases.
 640     *lowpassFilterApplyFn = nullFilterApply;
 641
 642     // If lowpass cutoff has been specified and is less than the Nyquist frequency
 643     if (lpfHz && lpfHz <= gyroFrequencyNyquist) {
 644         switch (type) {
 645         case FILTER_PT1:
 646             *lowpassFilterApplyFn = (filterApplyFnPtr) pt1FilterApply;
 647             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 648                 pt1FilterInit(&lowpassFilter[axis].pt1FilterState, gain);
 649             }
 650             break;
 651         case FILTER_BIQUAD:
 652 #ifdef USE_DYN_LPF
 653             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApplyDF1;
 654 #else
 655             *lowpassFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
 656 #endif
 657             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 658                 biquadFilterInitLPF(&lowpassFilter[axis].biquadFilterState, lpfHz, gyro.targetLooptime);
 659             }
 660             break;
 661         }
 662     }
 663 }
 664
 665 static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
 666 {
 667     const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
 668     if (notchHz > gyroFrequencyNyquist) {
 669         if (notchCutoffHz < gyroFrequencyNyquist) {
 670             notchHz = gyroFrequencyNyquist;
 671         } else {
 672             notchHz = 0;
 673         }
 674     }
 675
 676     return notchHz;
 677 }
 678
 679 #if defined(USE_GYRO_SLEW_LIMITER)
 680 void gyroInitSlewLimiter(gyroSensor_t *gyroSensor) {
 681
 682     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 683         gyroSensor->gyroDev.gyroADCRawPrevious[axis] = 0;
 684     }
 685 }
 686 #endif
 687
 688 static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 689 {
 690     gyroSensor->notchFilter1ApplyFn = nullFilterApply;
 691
 692     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 693
 694     if (notchHz != 0 && notchCutoffHz != 0) {
 695         gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 696         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 697         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 698             biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 699         }
 700     }
 701 }
 702
 703 static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uint16_t notchCutoffHz)
 704 {
 705     gyroSensor->notchFilter2ApplyFn = nullFilterApply;
 706
 707     notchHz = calculateNyquistAdjustedNotchHz(notchHz, notchCutoffHz);
 708
 709     if (notchHz != 0 && notchCutoffHz != 0) {
 710         gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
 711         const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz);
 712         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 713             biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 714         }
 715     }
 716 }
 717
 718 #ifdef USE_GYRO_DATA_ANALYSE
 719 static bool isDynamicFilterActive(void)
 720 {
 721     return featureIsEnabled(FEATURE_DYNAMIC_FILTER);
 722 }
 723
 724 static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
 725 {
 726     gyroSensor->notchFilterDynApplyFn = nullFilterApply;
 727     gyroSensor->notchFilterDynApplyFn2 = nullFilterApply;
 728
 729     if (isDynamicFilterActive()) {
 730         gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
 731         if(gyroConfig()->dyn_notch_width_percent != 0) {
 732             gyroSensor->notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
 733         }
 734         const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
 735         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 736             biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 737             biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 738         }
 739     }
 740 }
 741 #endif
 742
 743 static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
 744 {
 745 #if defined(USE_GYRO_SLEW_LIMITER)
 746     gyroInitSlewLimiter(gyroSensor);
 747 #endif
 748
 749     uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
 750
 751 #ifdef USE_DYN_LPF
 752     if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
 753         gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
 754     }
 755 #endif
 756
 757     gyroInitLowpassFilterLpf(
 758       gyroSensor,
 759       FILTER_LOWPASS,
 760       gyroConfig()->gyro_lowpass_type,
 761       gyro_lowpass_hz
 762     );
 763
 764     gyroInitLowpassFilterLpf(
 765       gyroSensor,
 766       FILTER_LOWPASS2,
 767       gyroConfig()->gyro_lowpass2_type,
 768       gyroConfig()->gyro_lowpass2_hz
 769     );
 770
 771     gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
 772     gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
 773 #ifdef USE_GYRO_DATA_ANALYSE
 774     gyroInitFilterDynamicNotch(gyroSensor);
 775 #endif
 776 #ifdef USE_DYN_LPF
 777     dynLpfFilterInit();
 778 #endif
 779 }
 780
 781 void gyroInitFilters(void)
 782 {
 783     gyroInitSensorFilters(&gyroSensor1);
 784 #ifdef USE_MULTI_GYRO
 785     gyroInitSensorFilters(&gyroSensor2);
 786 #endif
 787 }
 788
 789 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
 790 {
 791     return gyroSensor->calibration.cyclesRemaining == 0;
 792 }
 793
 794 FAST_CODE bool isGyroCalibrationComplete(void)
 795 {
 796     switch (gyroToUse) {
 797         default:
 798         case GYRO_CONFIG_USE_GYRO_1: {
 799             return isGyroSensorCalibrationComplete(&gyroSensor1);
 800         }
 801 #ifdef USE_MULTI_GYRO
 802         case GYRO_CONFIG_USE_GYRO_2: {
 803             return isGyroSensorCalibrationComplete(&gyroSensor2);
 804         }
 805         case GYRO_CONFIG_USE_GYRO_BOTH: {
 806             return isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2);
 807         }
 808 #endif
 809     }
 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_MULTI_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(gyroSensor->gyroDev.gyroADCf[X]) < gyroOverflowResetRate)
 921           && (abs(gyroSensor->gyroDev.gyroADCf[Y]) < gyroOverflowResetRate)
 922           && (abs(gyroSensor->gyroDev.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(gyroSensor->gyroDev.gyroADCf[X]) > gyroOverflowTriggerRate) {
 948             overflowCheck |= GYRO_OVERFLOW_X;
 949         }
 950         if (abs(gyroSensor->gyroDev.gyroADCf[Y]) > gyroOverflowTriggerRate) {
 951             overflowCheck |= GYRO_OVERFLOW_Y;
 952         }
 953         if (abs(gyroSensor->gyroDev.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(gyroSensor->gyroDev.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(gyroSensor->gyroDev.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(mode, index, value) { UNUSED(mode); UNUSED(index); UNUSED(value); }
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     if (gyroDebugMode == DEBUG_NONE) {
1047         filterGyro(gyroSensor);
1048     } else {
1049         filterGyroDebug(gyroSensor);
1050     }
1051
1052 #ifdef USE_GYRO_OVERFLOW_CHECK
1053     if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
1054         checkForOverflow(gyroSensor, currentTimeUs);
1055     }
1056 #endif
1057
1058 #ifdef USE_YAW_SPIN_RECOVERY
1059     if (gyroConfig()->yaw_spin_recovery) {
1060         checkForYawSpin(gyroSensor, currentTimeUs);
1061     }
1062 #endif
1063
1064 #ifdef USE_GYRO_DATA_ANALYSE
1065     if (isDynamicFilterActive()) {
1066         gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2);
1067     }
1068 #endif
1069
1070 #if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
1071     UNUSED(currentTimeUs);
1072 #endif
1073 }
1074
1075 FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
1076 {
1077     const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs;
1078     accumulationLastTimeSampledUs = currentTimeUs;
1079     accumulatedMeasurementTimeUs += sampleDeltaUs;
1080
1081     switch (gyroToUse) {
1082     case GYRO_CONFIG_USE_GYRO_1:
1083         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1084         if (isGyroSensorCalibrationComplete(&gyroSensor1)) {
1085             gyro.gyroADCf[X] = gyroSensor1.gyroDev.gyroADCf[X];
1086             gyro.gyroADCf[Y] = gyroSensor1.gyroDev.gyroADCf[Y];
1087             gyro.gyroADCf[Z] = gyroSensor1.gyroDev.gyroADCf[Z];
1088 #ifdef USE_GYRO_OVERFLOW_CHECK
1089             overflowDetected = gyroSensor1.overflowDetected;
1090 #endif
1091 #ifdef USE_YAW_SPIN_RECOVERY
1092             yawSpinDetected = gyroSensor1.yawSpinDetected;
1093 #endif
1094         }
1095         if (useDualGyroDebugging) {
1096             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1097             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1098             DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1099             DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1100             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X]));
1101             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y]));
1102             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
1103             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
1104         }
1105         break;
1106 #ifdef USE_MULTI_GYRO
1107     case GYRO_CONFIG_USE_GYRO_2:
1108         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1109         if (isGyroSensorCalibrationComplete(&gyroSensor2)) {
1110             gyro.gyroADCf[X] = gyroSensor2.gyroDev.gyroADCf[X];
1111             gyro.gyroADCf[Y] = gyroSensor2.gyroDev.gyroADCf[Y];
1112             gyro.gyroADCf[Z] = gyroSensor2.gyroDev.gyroADCf[Z];
1113 #ifdef USE_GYRO_OVERFLOW_CHECK
1114             overflowDetected = gyroSensor2.overflowDetected;
1115 #endif
1116 #ifdef USE_YAW_SPIN_RECOVERY
1117             yawSpinDetected = gyroSensor2.yawSpinDetected;
1118 #endif
1119         }
1120         if (useDualGyroDebugging) {
1121             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1122             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1123             DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1124             DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1125             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X]));
1126             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y]));
1127             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
1128             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
1129         }
1130         break;
1131     case GYRO_CONFIG_USE_GYRO_BOTH:
1132         gyroUpdateSensor(&gyroSensor1, currentTimeUs);
1133         gyroUpdateSensor(&gyroSensor2, currentTimeUs);
1134         if (isGyroSensorCalibrationComplete(&gyroSensor1) && isGyroSensorCalibrationComplete(&gyroSensor2)) {
1135             gyro.gyroADCf[X] = (gyroSensor1.gyroDev.gyroADCf[X] + gyroSensor2.gyroDev.gyroADCf[X]) / 2.0f;
1136             gyro.gyroADCf[Y] = (gyroSensor1.gyroDev.gyroADCf[Y] + gyroSensor2.gyroDev.gyroADCf[Y]) / 2.0f;
1137             gyro.gyroADCf[Z] = (gyroSensor1.gyroDev.gyroADCf[Z] + gyroSensor2.gyroDev.gyroADCf[Z]) / 2.0f;
1138 #ifdef USE_GYRO_OVERFLOW_CHECK
1139             overflowDetected = gyroSensor1.overflowDetected || gyroSensor2.overflowDetected;
1140 #endif
1141 #ifdef USE_YAW_SPIN_RECOVERY
1142             yawSpinDetected = gyroSensor1.yawSpinDetected || gyroSensor2.yawSpinDetected;
1143 #endif
1144         }
1145
1146         if (useDualGyroDebugging) {
1147             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]);
1148             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]);
1149             DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X]));
1150             DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y]));
1151             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]);
1152             DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]);
1153             DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X]));
1154             DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y]));
1155             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X]));
1156             DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y]));
1157             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X]));
1158             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y]));
1159             DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z]));
1160             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyroSensor1.gyroDev.gyroADC[X] * gyroSensor1.gyroDev.scale));
1161             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyroSensor1.gyroDev.gyroADC[Y] * gyroSensor1.gyroDev.scale));
1162             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyroSensor2.gyroDev.gyroADC[X] * gyroSensor2.gyroDev.scale));
1163             DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyroSensor2.gyroDev.gyroADC[Y] * gyroSensor2.gyroDev.scale));
1164         }
1165         break;
1166 #endif
1167     }
1168
1169     if (!overflowDetected) {
1170         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1171             // integrate using trapezium rule to avoid bias
1172             accumulatedMeasurements[axis] += 0.5f * (gyroPrevious[axis] + gyro.gyroADCf[axis]) * sampleDeltaUs;
1173             gyroPrevious[axis] = gyro.gyroADCf[axis];
1174         }
1175     }
1176
1177 }
1178
1179 bool gyroGetAccumulationAverage(float *accumulationAverage)
1180 {
1181     if (accumulatedMeasurementTimeUs > 0) {
1182         // If we have gyro data accumulated, calculate average rate that will yield the same rotation
1183         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1184             accumulationAverage[axis] = accumulatedMeasurements[axis] / accumulatedMeasurementTimeUs;
1185             accumulatedMeasurements[axis] = 0.0f;
1186         }
1187         accumulatedMeasurementTimeUs = 0;
1188         return true;
1189     } else {
1190         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1191             accumulationAverage[axis] = 0.0f;
1192         }
1193         return false;
1194     }
1195 }
1196
1197 int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)
1198 {
1199     if (gyroSensor.gyroDev.temperatureFn) {
1200         gyroSensor.gyroDev.temperatureFn(&gyroSensor.gyroDev, &gyroSensor.gyroDev.temperature);
1201     }
1202     return gyroSensor.gyroDev.temperature;
1203 }
1204
1205 void gyroReadTemperature(void)
1206 {
1207     switch (gyroToUse) {
1208     case GYRO_CONFIG_USE_GYRO_1:
1209         gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor1);
1210         break;
1211
1212 #ifdef USE_MULTI_GYRO
1213     case GYRO_CONFIG_USE_GYRO_2:
1214         gyroSensorTemperature = gyroReadSensorTemperature(gyroSensor2);
1215         break;
1216
1217     case GYRO_CONFIG_USE_GYRO_BOTH:
1218         gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyroSensor1), gyroReadSensorTemperature(gyroSensor2));
1219         break;
1220 #endif // USE_MULTI_GYRO
1221     }
1222 }
1223
1224 int16_t gyroGetTemperature(void)
1225 {
1226     return gyroSensorTemperature;
1227 }
1228
1229 int16_t gyroRateDps(int axis)
1230 {
1231     return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
1232 }
1233
1234 bool gyroOverflowDetected(void)
1235 {
1236 #ifdef USE_GYRO_OVERFLOW_CHECK
1237     return overflowDetected;
1238 #else
1239     return false;
1240 #endif // USE_GYRO_OVERFLOW_CHECK
1241 }
1242
1243 #ifdef USE_YAW_SPIN_RECOVERY
1244 bool gyroYawSpinDetected(void)
1245 {
1246     return yawSpinDetected;
1247 }
1248 #endif // USE_YAW_SPIN_RECOVERY
1249
1250 uint16_t gyroAbsRateDps(int axis)
1251 {
1252     return fabsf(gyro.gyroADCf[axis]);
1253 }
1254
1255 #ifdef USE_GYRO_REGISTER_DUMP
1256 uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
1257 {
1258     return mpuGyroReadRegister(gyroSensorBusByDevice(whichSensor), reg);
1259 }
1260 #endif // USE_GYRO_REGISTER_DUMP
1261
1262 #ifdef USE_DYN_LPF
1263
1264 float dynThrottle(float throttle) {
1265     return throttle * (1 - (throttle * throttle) / 3.0f) * 1.5f;
1266 }
1267
1268 void dynLpfGyroUpdate(float throttle)
1269 {
1270     if (dynLpfFilter != DYN_LPF_NONE) {
1271         const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
1272
1273         if (dynLpfFilter == DYN_LPF_PT1) {
1274             DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
1275             const float gyroDt = gyro.targetLooptime * 1e-6f;
1276             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1277 #ifdef USE_MULTI_GYRO
1278                 if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
1279                     pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
1280                 }
1281                 if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
1282                     pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
1283                 }
1284 #else
1285                 pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
1286 #endif
1287             }
1288         } else if (dynLpfFilter == DYN_LPF_BIQUAD) {
1289             DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
1290             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
1291 #ifdef USE_MULTI_GYRO
1292                 if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
1293                     biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
1294                 }
1295                 if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
1296                     biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
1297                 }
1298 #else
1299                 biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
1300 #endif
1301             }
1302         }
1303     }
1304 }
1305 #endif