src/main/sensors/gyro.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/build_config.h"
  26 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29 #include "common/calibration.h"
  30 #include "common/filter.h"
  31 #include "common/log.h"
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34
  35 #include "config/parameter_group.h"
  36 #include "config/parameter_group_ids.h"
  37 #include "config/feature.h"
  38
  39 #include "drivers/accgyro/accgyro.h"
  40 #include "drivers/accgyro/accgyro_mpu.h"
  41 #include "drivers/accgyro/accgyro_mpu6000.h"
  42 #include "drivers/accgyro/accgyro_mpu6500.h"
  43 #include "drivers/accgyro/accgyro_mpu9250.h"
  44
  45 #include "drivers/accgyro/accgyro_bmi088.h"
  46 #include "drivers/accgyro/accgyro_bmi160.h"
  47 #include "drivers/accgyro/accgyro_bmi270.h"
  48 #include "drivers/accgyro/accgyro_icm20689.h"
  49 #include "drivers/accgyro/accgyro_icm42605.h"
  50 #include "drivers/accgyro/accgyro_lsm6dxx.h"
  51 #include "drivers/accgyro/accgyro_fake.h"
  52 #include "drivers/io.h"
  53
  54 #include "fc/config.h"
  55 #include "fc/runtime_config.h"
  56 #include "fc/rc_controls.h"
  57 #include "fc/settings.h"
  58
  59 #include "io/beeper.h"
  60 #include "io/statusindicator.h"
  61
  62 #include "scheduler/scheduler.h"
  63
  64 #include "sensors/boardalignment.h"
  65 #include "sensors/gyro.h"
  66 #include "sensors/sensors.h"
  67
  68 #include "flight/gyroanalyse.h"
  69 #include "flight/rpm_filter.h"
  70 #include "flight/kalman.h"
  71 #include "flight/adaptive_filter.h"
  72
  73 #ifdef USE_HARDWARE_REVISION_DETECTION
  74 #include "hardware_revision.h"
  75 #endif
  76
  77 FASTRAM gyro_t gyro; // gyro sensor object
  78
  79 #define MAX_GYRO_COUNT 1
  80
  81 STATIC_UNIT_TESTED gyroDev_t gyroDev[MAX_GYRO_COUNT];  // Not in FASTRAM since it may hold DMA buffers
  82 STATIC_FASTRAM int16_t gyroTemperature[MAX_GYRO_COUNT];
  83 STATIC_FASTRAM_UNIT_TESTED zeroCalibrationVector_t gyroCalibration[MAX_GYRO_COUNT];
  84
  85 STATIC_FASTRAM filterApplyFnPtr gyroLpfApplyFn;
  86 STATIC_FASTRAM filter_t gyroLpfState[XYZ_AXIS_COUNT];
  87
  88 STATIC_FASTRAM filterApplyFnPtr gyroLpf2ApplyFn;
  89 STATIC_FASTRAM filter_t gyroLpf2State[XYZ_AXIS_COUNT];
  90
  91 STATIC_FASTRAM filterApplyFnPtr gyroLuluApplyFn;
  92 STATIC_FASTRAM filter_t gyroLuluState[XYZ_AXIS_COUNT];
  93
  94 #ifdef USE_DYNAMIC_FILTERS
  95
  96 EXTENDED_FASTRAM gyroAnalyseState_t gyroAnalyseState;
  97 EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
  98 EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState;
  99
 100 #endif
 101
 102 PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
 103
 104 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
 105     .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT,
 106     .looptime = SETTING_LOOPTIME_DEFAULT,
 107 #ifdef USE_DUAL_GYRO
 108     .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT,
 109 #endif
 110     .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT,
 111     .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT,
 112     .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT,
 113     .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT,
 114 #ifdef USE_DYNAMIC_FILTERS
 115     .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT,
 116     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
 117     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
 118     .dynamicGyroNotchMode = SETTING_DYNAMIC_GYRO_NOTCH_MODE_DEFAULT,
 119     .dynamicGyroNotch3dQ = SETTING_DYNAMIC_GYRO_NOTCH_3D_Q_DEFAULT,
 120 #endif
 121 #ifdef USE_GYRO_KALMAN
 122     .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
 123     .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
 124 #endif
 125     .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT,
 126     .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT},
 127     .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT,
 128 #ifdef USE_ADAPTIVE_FILTER
 129     .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT,
 130     .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT,
 131     .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT,
 132     .adaptiveFilterStdLpfHz = SETTING_GYRO_ADAPTIVE_FILTER_STD_LPF_HZ_DEFAULT,
 133     .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT,
 134     .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT,
 135     .adaptiveFilterIntegratorThresholdLow  = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT,
 136 #endif
 137     .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT,
 138     .gyroLuluSampleCount = SETTING_GYRO_LULU_SAMPLE_COUNT_DEFAULT,
 139     .gyroLuluEnabled = SETTING_GYRO_LULU_ENABLED_DEFAULT
 140 );
 141
 142 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
 143 {
 144     dev->gyroAlign = ALIGN_DEFAULT;
 145
 146     switch (gyroHardware) {
 147     case GYRO_AUTODETECT:
 148         FALLTHROUGH;
 149
 150 #ifdef USE_IMU_MPU6000
 151     case GYRO_MPU6000:
 152         if (mpu6000GyroDetect(dev)) {
 153             gyroHardware = GYRO_MPU6000;
 154             break;
 155         }
 156         FALLTHROUGH;
 157 #endif
 158
 159 #if defined(USE_IMU_MPU6500)
 160     case GYRO_MPU6500:
 161         if (mpu6500GyroDetect(dev)) {
 162             gyroHardware = GYRO_MPU6500;
 163             break;
 164         }
 165         FALLTHROUGH;
 166 #endif
 167
 168 #ifdef USE_IMU_MPU9250
 169     case GYRO_MPU9250:
 170         if (mpu9250GyroDetect(dev)) {
 171             gyroHardware = GYRO_MPU9250;
 172             break;
 173         }
 174         FALLTHROUGH;
 175 #endif
 176
 177 #ifdef USE_IMU_BMI160
 178     case GYRO_BMI160:
 179         if (bmi160GyroDetect(dev)) {
 180             gyroHardware = GYRO_BMI160;
 181             break;
 182         }
 183         FALLTHROUGH;
 184 #endif
 185
 186 #ifdef USE_IMU_BMI088
 187     case GYRO_BMI088:
 188         if (bmi088GyroDetect(dev)) {
 189             gyroHardware = GYRO_BMI088;
 190             break;
 191         }
 192         FALLTHROUGH;
 193 #endif
 194
 195 #ifdef USE_IMU_ICM20689
 196     case GYRO_ICM20689:
 197         if (icm20689GyroDetect(dev)) {
 198             gyroHardware = GYRO_ICM20689;
 199             break;
 200         }
 201         FALLTHROUGH;
 202 #endif
 203
 204 #ifdef USE_IMU_ICM42605
 205     case GYRO_ICM42605:
 206         if (icm42605GyroDetect(dev)) {
 207             gyroHardware = GYRO_ICM42605;
 208             break;
 209         }
 210         FALLTHROUGH;
 211 #endif
 212
 213 #ifdef USE_IMU_BMI270
 214     case GYRO_BMI270:
 215         if (bmi270GyroDetect(dev)) {
 216             gyroHardware = GYRO_BMI270;
 217             break;
 218         }
 219         FALLTHROUGH;
 220 #endif
 221
 222 #ifdef USE_IMU_LSM6DXX
 223     case GYRO_LSM6DXX:
 224         if (lsm6dGyroDetect(dev)) {
 225             gyroHardware = GYRO_LSM6DXX;
 226             break;
 227         }
 228         FALLTHROUGH;
 229 #endif
 230
 231 #ifdef USE_IMU_FAKE
 232     case GYRO_FAKE:
 233         if (fakeGyroDetect(dev)) {
 234             gyroHardware = GYRO_FAKE;
 235             break;
 236         }
 237         FALLTHROUGH;
 238 #endif
 239
 240     default:
 241     case GYRO_NONE:
 242         gyroHardware = GYRO_NONE;
 243     }
 244
 245     return gyroHardware;
 246 }
 247
 248 static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint16_t cutoff, uint32_t looptime)
 249 {
 250     *applyFn = nullFilterApply;
 251     if (cutoff > 0) {
 252         *applyFn = (filterApplyFnPtr)pt1FilterApply;
 253         for (int axis = 0; axis < 3; axis++) {
 254             pt1FilterInit(&state[axis].pt1, cutoff, US2S(looptime));
 255         }
 256     }
 257 }
 258
 259 static void gyroInitFilters(void)
 260 {
 261     //First gyro LPF running at full gyro frequency 8kHz
 262     initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime());
 263
 264     if (gyroConfig()->gyroLuluEnabled && gyroConfig()->gyroLuluSampleCount > 0) {
 265         gyroLuluApplyFn = (filterApplyFnPtr)luluFilterApply;
 266
 267         for (int axis = 0; axis < 3; axis++) {
 268             luluFilterInit(&gyroLuluState[axis].lulu, gyroConfig()->gyroLuluSampleCount);
 269         }
 270     } else {
 271         gyroLuluApplyFn = nullFilterApply;
 272     }
 273
 274     if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_OFF) {
 275         initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime());
 276     } else {
 277         gyroLpf2ApplyFn = nullFilterApply;
 278     }
 279
 280 #ifdef USE_ADAPTIVE_FILTER
 281     if (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE) {
 282         adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz);
 283     }
 284 #endif
 285
 286 #ifdef USE_GYRO_KALMAN
 287     if (gyroConfig()->kalmanEnabled) {
 288         gyroKalmanInitialize(gyroConfig()->kalman_q);
 289     }
 290 #endif
 291 }
 292
 293 bool gyroInit(void)
 294 {
 295     memset(&gyro, 0, sizeof(gyro));
 296
 297     // Set inertial sensor tag (for dual-gyro selection)
 298 #ifdef USE_DUAL_GYRO
 299     gyroDev[0].imuSensorToUse = gyroConfig()->gyro_to_use;
 300 #else
 301     gyroDev[0].imuSensorToUse = 0;
 302 #endif
 303
 304     // Detecting gyro0
 305     gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
 306     if (gyroHardware == GYRO_NONE) {
 307         gyro.initialized = false;
 308         detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
 309         return true;
 310     }
 311
 312     // Gyro is initialized
 313     gyro.initialized = true;
 314     detectedSensors[SENSOR_INDEX_GYRO] = gyroHardware;
 315     sensorsSet(SENSOR_GYRO);
 316
 317     // Driver initialisation
 318     gyroDev[0].lpf = GYRO_LPF_256HZ;
 319     gyroDev[0].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME;
 320     gyroDev[0].sampleRateIntervalUs = TASK_GYRO_LOOPTIME;
 321     gyroDev[0].initFn(&gyroDev[0]);
 322
 323     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
 324     gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
 325
 326     gyroInitFilters();
 327
 328 #ifdef USE_DYNAMIC_FILTERS
 329     // Dynamic notch running at PID frequency
 330     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
 331
 332     secondaryDynamicGyroNotchFiltersInit(&secondaryDynamicGyroNotchState);
 333
 334     gyroDataAnalyseStateInit(
 335         &gyroAnalyseState,
 336         gyroConfig()->dynamicGyroNotchMinHz,
 337         getLooptime()
 338     );
 339 #endif
 340     return true;
 341 }
 342
 343 void gyroStartCalibration(void)
 344 {
 345     if (!gyro.initialized) {
 346         return;
 347     }
 348
 349 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
 350     if (!gyroConfig()->init_gyro_cal_enabled) {
 351         return;
 352     }
 353 #endif
 354
 355     zeroCalibrationStartV(&gyroCalibration[0], CALIBRATING_GYRO_TIME_MS, CALIBRATING_GYRO_MORON_THRESHOLD, false);
 356 }
 357
 358 bool gyroIsCalibrationComplete(void)
 359 {
 360     if (!gyro.initialized) {
 361         return true;
 362     }
 363
 364 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
 365     if (!gyroConfig()->init_gyro_cal_enabled) {
 366         return true;
 367     }
 368 #endif
 369
 370     return zeroCalibrationIsCompleteV(&gyroCalibration[0]) && zeroCalibrationIsSuccessfulV(&gyroCalibration[0]);
 371 }
 372
 373 STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *dev, zeroCalibrationVector_t *gyroCalibration)
 374 {
 375     fpVector3_t v;
 376
 377     // Consume gyro reading
 378     v.v[X] = dev->gyroADCRaw[X];
 379     v.v[Y] = dev->gyroADCRaw[Y];
 380     v.v[Z] = dev->gyroADCRaw[Z];
 381
 382     zeroCalibrationAddValueV(gyroCalibration, &v);
 383
 384     // Check if calibration is complete after this cycle
 385     if (zeroCalibrationIsCompleteV(gyroCalibration)) {
 386         zeroCalibrationGetZeroV(gyroCalibration, &v);
 387         dev->gyroZero[X] = v.v[X];
 388         dev->gyroZero[Y] = v.v[Y];
 389         dev->gyroZero[Z] = v.v[Z];
 390
 391 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
 392         setGyroCalibration(dev->gyroZero);
 393 #endif
 394
 395         LOG_DEBUG(GYRO, "Gyro calibration complete (%d, %d, %d)", (int16_t) dev->gyroZero[X], (int16_t) dev->gyroZero[Y], (int16_t) dev->gyroZero[Z]);
 396         schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
 397     } else {
 398         dev->gyroZero[X] = 0;
 399         dev->gyroZero[Y] = 0;
 400         dev->gyroZero[Z] = 0;
 401     }
 402 }
 403
 404 /*
 405  * Calculate rotation rate in rad/s in body frame
 406  */
 407 void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
 408 {
 409     for (int axis = 0; axis < 3; axis++) {
 410         measuredRotationRate->v[axis] = DEGREES_TO_RADIANS(gyro.gyroADCf[axis]);
 411     }
 412 }
 413
 414 static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroCalibrationVector_t * gyroCal, float * gyroADCf)
 415 {
 416
 417     // range: +/- 8192; +/- 2000 deg/sec
 418     if (gyroDev->readFn(gyroDev)) {
 419
 420 #ifndef USE_IMU_FAKE // fixes Test Unit compilation error
 421     if (!gyroConfig()->init_gyro_cal_enabled) {
 422         // marks that the gyro calibration has ended
 423         gyroCalibration[0].params.state = ZERO_CALIBRATION_DONE;
 424         // pass the calibration values
 425         gyroDev->gyroZero[X] = gyroConfig()->gyro_zero_cal[X];
 426         gyroDev->gyroZero[Y] = gyroConfig()->gyro_zero_cal[Y];
 427         gyroDev->gyroZero[Z] = gyroConfig()->gyro_zero_cal[Z];
 428     }
 429 #endif
 430
 431         if (zeroCalibrationIsCompleteV(gyroCal)) {
 432             float gyroADCtmp[XYZ_AXIS_COUNT];
 433
 434             //Apply zero calibration with CMSIS DSP
 435             arm_sub_f32(gyroDev->gyroADCRaw, gyroDev->gyroZero, gyroADCtmp, 3);
 436
 437             // Apply sensor alignment
 438             applySensorAlignment(gyroADCtmp, gyroADCtmp, gyroDev->gyroAlign);
 439             applyBoardAlignment(gyroADCtmp);
 440
 441             // Convert to deg/s and store in unified data
 442             arm_scale_f32(gyroADCtmp, gyroDev->scale, gyroADCf, 3);
 443
 444             return true;
 445         } else {
 446             performGyroCalibration(gyroDev, gyroCal);
 447
 448             // Reset gyro values to zero to prevent other code from using uncalibrated data
 449             gyroADCf[X] = 0.0f;
 450             gyroADCf[Y] = 0.0f;
 451             gyroADCf[Z] = 0.0f;
 452
 453             return false;
 454         }
 455     } else {
 456         // no gyro reading to process
 457         return false;
 458     }
 459 }
 460
 461 void FAST_CODE NOINLINE gyroFilter(void)
 462 {
 463     if (!gyro.initialized) {
 464         return;
 465     }
 466
 467     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 468         float gyroADCf = gyro.gyroADCf[axis];
 469
 470 #ifdef USE_RPM_FILTER
 471         gyroADCf = rpmFilterGyroApply(axis, gyroADCf);
 472 #endif
 473
 474         // LULU gyro filter
 475         DEBUG_SET(DEBUG_LULU, axis, gyroADCf); //Pre LULU debug
 476         float preLulu = gyroADCf;
 477         gyroADCf = gyroLuluApplyFn((filter_t *) &gyroLuluState[axis], gyroADCf);
 478         DEBUG_SET(DEBUG_LULU, axis + 3, gyroADCf); //Post LULU debug
 479
 480         if (axis == ROLL) {
 481             DEBUG_SET(DEBUG_LULU, 6, gyroADCf - preLulu); //LULU delta debug
 482         }
 483
 484         // Gyro Main LPF
 485         gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf);
 486
 487 #ifdef USE_ADAPTIVE_FILTER
 488         adaptiveFilterPush(axis, gyroADCf);
 489 #endif
 490
 491 #ifdef USE_DYNAMIC_FILTERS
 492         if (dynamicGyroNotchState.enabled) {
 493             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
 494             gyroADCf = dynamicGyroNotchFiltersApply(&dynamicGyroNotchState, axis, gyroADCf);
 495         }
 496
 497         /**
 498          * Secondary dynamic notch filter.
 499          * In some cases, noise amplitude is high enough not to be filtered by the primary filter.
 500          * This happens on the first frequency with the biggest aplitude
 501          */
 502         gyroADCf = secondaryDynamicGyroNotchFiltersApply(&secondaryDynamicGyroNotchState, axis, gyroADCf);
 503
 504 #endif
 505
 506 #ifdef USE_GYRO_KALMAN
 507         if (gyroConfig()->kalmanEnabled) {
 508             gyroADCf = gyroKalmanUpdate(axis, gyroADCf);
 509         }
 510 #endif
 511
 512         gyro.gyroADCf[axis] = gyroADCf;
 513     }
 514
 515 #ifdef USE_DYNAMIC_FILTERS
 516     if (dynamicGyroNotchState.enabled) {
 517         gyroDataAnalyse(&gyroAnalyseState);
 518
 519         if (gyroAnalyseState.filterUpdateExecute) {
 520             dynamicGyroNotchFiltersUpdate(
 521                 &dynamicGyroNotchState,
 522                 gyroAnalyseState.filterUpdateAxis,
 523                 gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
 524             );
 525
 526             secondaryDynamicGyroNotchFiltersUpdate(
 527                 &secondaryDynamicGyroNotchState,
 528                 gyroAnalyseState.filterUpdateAxis,
 529                 gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
 530             );
 531
 532         }
 533     }
 534 #endif
 535
 536 }
 537
 538 void FAST_CODE NOINLINE gyroUpdate(void)
 539 {
 540 #ifdef USE_SIMULATOR
 541     if (ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 542         //output: gyro.gyroADCf[axis]
 543         //unused: dev->gyroADCRaw[], dev->gyroZero[];
 544         return;
 545     }
 546 #endif
 547     if (!gyro.initialized) {
 548         return;
 549     }
 550
 551     if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) {
 552         return;
 553     }
 554
 555     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 556         // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s]
 557         float gyroADCf = gyro.gyroADCf[axis];
 558
 559         // Set raw gyro for blackbox purposes
 560         gyro.gyroRaw[axis] = gyroADCf;
 561
 562         /*
 563          * First gyro LPF is the only filter applied with the full gyro sampling speed
 564          */
 565         gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
 566
 567         gyro.gyroADCf[axis] = gyroADCf;
 568     }
 569 }
 570
 571 bool gyroReadTemperature(void)
 572 {
 573     if (!gyro.initialized) {
 574         return false;
 575     }
 576
 577     // Read gyro sensor temperature. temperatureFn returns temperature in [degC * 10]
 578     if (gyroDev[0].temperatureFn) {
 579         return gyroDev[0].temperatureFn(&gyroDev[0], &gyroTemperature[0]);
 580     }
 581
 582     return false;
 583 }
 584
 585 int16_t gyroGetTemperature(void)
 586 {
 587     if (!gyro.initialized) {
 588         return 0;
 589     }
 590
 591     return gyroTemperature[0];
 592 }
 593
 594 int16_t gyroRateDps(int axis)
 595 {
 596     if (!gyro.initialized) {
 597         return 0;
 598     }
 599
 600     return lrintf(gyro.gyroADCf[axis]);
 601 }
 602
 603 void gyroUpdateDynamicLpf(float cutoffFreq) {
 604     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 605         pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
 606     }
 607 }
 608
 609 float averageAbsGyroRates(void)
 610 {
 611     return (fabsf(gyro.gyroADCf[ROLL]) + fabsf(gyro.gyroADCf[PITCH]) + fabsf(gyro.gyroADCf[YAW])) / 3.0f;
 612 }