src/main/fc/fc_core.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
  26 #include "platform.h"
  27
  28 #include "build/debug.h"
  29
  30 #include "blackbox/blackbox.h"
  31
  32 #include "common/axis.h"
  33 #include "common/filter.h"
  34 #include "common/maths.h"
  35 #include "common/utils.h"
  36
  37 #include "config/feature.h"
  38 #include "pg/pg.h"
  39 #include "pg/pg_ids.h"
  40 #include "pg/rx.h"
  41
  42 #include "drivers/light_led.h"
  43 #include "drivers/sound_beeper.h"
  44 #include "drivers/system.h"
  45 #include "drivers/time.h"
  46 #include "drivers/transponder_ir.h"
  47
  48 #include "sensors/acceleration.h"
  49 #include "sensors/barometer.h"
  50 #include "sensors/battery.h"
  51 #include "sensors/boardalignment.h"
  52 #include "sensors/gyro.h"
  53 #include "sensors/sensors.h"
  54
  55 #include "fc/config.h"
  56 #include "fc/controlrate_profile.h"
  57 #include "fc/fc_core.h"
  58 #include "fc/fc_dispatch.h"
  59 #include "fc/fc_rc.h"
  60 #include "fc/rc_adjustments.h"
  61 #include "fc/rc_controls.h"
  62 #include "fc/runtime_config.h"
  63
  64 #include "msp/msp_serial.h"
  65
  66 #include "interface/cli.h"
  67
  68 #include "io/beeper.h"
  69 #include "io/gps.h"
  70 #include "io/motors.h"
  71 #include "io/pidaudio.h"
  72 #include "io/servos.h"
  73 #include "io/serial.h"
  74 #include "io/statusindicator.h"
  75 #include "io/transponder_ir.h"
  76 #include "io/vtx_control.h"
  77 #include "io/vtx_rtc6705.h"
  78
  79 #include "rx/rx.h"
  80
  81 #include "scheduler/scheduler.h"
  82
  83 #include "telemetry/telemetry.h"
  84
  85 #include "flight/position.h"
  86 #include "flight/failsafe.h"
  87 #include "flight/imu.h"
  88 #include "flight/mixer.h"
  89 #include "flight/pid.h"
  90 #include "flight/servos.h"
  91 #include "flight/gps_rescue.h"
  92
  93
  94 // June 2013     V2.2-dev
  95
  96 enum {
  97     ALIGN_GYRO = 0,
  98     ALIGN_ACCEL = 1,
  99     ALIGN_MAG = 2
 100 };
 101
 102
 103 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
 104
 105 #ifdef USE_RUNAWAY_TAKEOFF
 106 #define RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD         600   // The pidSum threshold required to trigger - corresponds to a pidSum value of 60% (raw 600) in the blackbox viewer
 107 #define RUNAWAY_TAKEOFF_ACTIVATE_DELAY           75000 // (75ms) Time in microseconds where pidSum is above threshold to trigger
 108 #define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15    // 15% - minimum stick deflection during deactivation phase
 109 #define RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT  100   // 10.0% - pidSum limit during deactivation phase
 110 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_RP            15    // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props
 111 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW           50    // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props
 112 #define RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT    75    // High throttle limit to accelerate deactivation (halves the deactivation delay)
 113
 114 #define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE      0
 115 #define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY   1
 116 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY 2
 117 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME  3
 118
 119 #define DEBUG_RUNAWAY_TAKEOFF_TRUE  1
 120 #define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
 121 #endif
 122
 123 #define PARALYZE_PREVENT_MODE_CHANGES_DELAY_US   100000 // Delay after paralyze mode activates to let other mode changes propagate
 124
 125 #if defined(USE_GPS) || defined(USE_MAG)
 126 int16_t magHold;
 127 #endif
 128
 129 static bool flipOverAfterCrashMode = false;
 130
 131 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 132
 133 bool isRXDataNew;
 134 static int lastArmingDisabledReason = 0;
 135 static timeUs_t lastDisarmTimeUs;
 136 static bool tryingToArm;
 137
 138 #ifdef USE_RUNAWAY_TAKEOFF
 139 static timeUs_t runawayTakeoffDeactivateUs = 0;
 140 static timeUs_t runawayTakeoffAccumulatedUs = 0;
 141 static bool runawayTakeoffCheckDisabled = false;
 142 static timeUs_t runawayTakeoffTriggerUs = 0;
 143 static bool runawayTakeoffTemporarilyDisabled = false;
 144 #endif
 145
 146 static bool paralyzeModeAllowed = false;
 147
 148 void preventModeChangesDispatch(dispatchEntry_t *self) {
 149     UNUSED(self);
 150     preventModeChanges();
 151 }
 152
 153 static dispatchEntry_t preventModeChangesDispatchEntry = { .dispatch = preventModeChangesDispatch};
 154
 155 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 156
 157 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
 158     .throttle_correction_value = 0,      // could 10 with althold or 40 for fpv
 159     .throttle_correction_angle = 800     // could be 80.0 deg with atlhold or 45.0 for fpv
 160 );
 161
 162 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 163 {
 164     accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 165     accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 166
 167     saveConfigAndNotify();
 168 }
 169
 170 static bool isCalibrating(void)
 171 {
 172 #ifdef USE_BARO
 173     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 174         return true;
 175     }
 176 #endif
 177
 178     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 179
 180     return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 181 }
 182
 183 void resetArmingDisabled(void)
 184 {
 185     lastArmingDisabledReason = 0;
 186 }
 187
 188 void updateArmingStatus(void)
 189 {
 190     if (ARMING_FLAG(ARMED)) {
 191         LED0_ON;
 192     } else {
 193         // Check if the power on arming grace time has elapsed
 194         if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)) {
 195             // If so, unset the grace time arming disable flag
 196             unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 197         }
 198
 199         // Clear the crash flip active status
 200         flipOverAfterCrashMode = false;
 201
 202         // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
 203         if (!isUsingSticksForArming()) {
 204             static bool hadRx = false;
 205             const bool haveRx = rxIsReceivingSignal();
 206
 207             const bool justGotRxBack = !hadRx && haveRx;
 208
 209             if (justGotRxBack && IS_RC_MODE_ACTIVE(BOXARM)) {
 210                 // If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction
 211                 setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
 212             } else if (haveRx && !IS_RC_MODE_ACTIVE(BOXARM)) {
 213                 // If RX signal is OK and the arm switch is off, remove arming restriction
 214                 unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
 215             }
 216
 217             hadRx = haveRx;
 218         }
 219
 220         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 221             setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
 222         } else {
 223             unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
 224         }
 225
 226         if (calculateThrottleStatus() != THROTTLE_LOW) {
 227             setArmingDisabled(ARMING_DISABLED_THROTTLE);
 228         } else {
 229             unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
 230         }
 231
 232         if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
 233             setArmingDisabled(ARMING_DISABLED_ANGLE);
 234         } else {
 235             unsetArmingDisabled(ARMING_DISABLED_ANGLE);
 236         }
 237
 238         if (averageSystemLoadPercent > 100) {
 239             setArmingDisabled(ARMING_DISABLED_LOAD);
 240         } else {
 241             unsetArmingDisabled(ARMING_DISABLED_LOAD);
 242         }
 243
 244         if (isCalibrating()) {
 245             setArmingDisabled(ARMING_DISABLED_CALIBRATING);
 246         } else {
 247             unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
 248         }
 249
 250         if (isModeActivationConditionPresent(BOXPREARM)) {
 251             if (IS_RC_MODE_ACTIVE(BOXPREARM) && !ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
 252                 unsetArmingDisabled(ARMING_DISABLED_NOPREARM);
 253             } else {
 254                 setArmingDisabled(ARMING_DISABLED_NOPREARM);
 255             }
 256         }
 257
 258 #ifdef USE_GPS_RESCUE
 259         if (isModeActivationConditionPresent(BOXGPSRESCUE)) {
 260             if (!gpsRescueConfig()->minSats || STATE(GPS_FIX_HOME) || ARMING_FLAG(WAS_EVER_ARMED)) {
 261                 unsetArmingDisabled(ARMING_DISABLED_GPS);
 262             } else {
 263                 setArmingDisabled(ARMING_DISABLED_GPS);
 264             }
 265         }
 266 #endif
 267
 268         if (IS_RC_MODE_ACTIVE(BOXPARALYZE) && paralyzeModeAllowed) {
 269             setArmingDisabled(ARMING_DISABLED_PARALYZE);
 270             dispatchAdd(&preventModeChangesDispatchEntry, PARALYZE_PREVENT_MODE_CHANGES_DELAY_US);
 271         }
 272
 273         if (!isUsingSticksForArming()) {
 274           /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */
 275           bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm
 276                          && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING));
 277
 278           /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */
 279           bool ignoreThrottle = feature(FEATURE_3D)
 280                              && !IS_RC_MODE_ACTIVE(BOX3D)
 281                              && !flight3DConfig()->switched_mode3d
 282                              && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE));
 283
 284 #ifdef USE_RUNAWAY_TAKEOFF
 285            if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 286                unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 287            }
 288 #endif
 289
 290           // If arming is disabled and the ARM switch is on
 291           if (isArmingDisabled()
 292               && !ignoreGyro
 293               && !ignoreThrottle
 294               && IS_RC_MODE_ACTIVE(BOXARM)) {
 295               setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 296           } else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 297               unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 298           }
 299         }
 300
 301         if (isArmingDisabled()) {
 302             warningLedFlash();
 303         } else {
 304             warningLedDisable();
 305         }
 306
 307         warningLedUpdate();
 308     }
 309
 310     // Check if entering into paralyze mode can be allowed regardless of arming status
 311     if (!IS_RC_MODE_ACTIVE(BOXPARALYZE) && !paralyzeModeAllowed) {
 312         paralyzeModeAllowed = true;
 313     }
 314 }
 315
 316 void disarm(void)
 317 {
 318     if (ARMING_FLAG(ARMED)) {
 319         DISABLE_ARMING_FLAG(ARMED);
 320         lastDisarmTimeUs = micros();
 321
 322 #ifdef USE_BLACKBOX
 323         if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON
 324             blackboxFinish();
 325         }
 326 #endif
 327         BEEP_OFF;
 328 #ifdef USE_DSHOT
 329         if (isMotorProtocolDshot() && flipOverAfterCrashMode && !feature(FEATURE_3D)) {
 330             pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
 331         }
 332 #endif
 333         flipOverAfterCrashMode = false;
 334
 335         // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead
 336         if (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)) {
 337             beeper(BEEPER_DISARMING);      // emit disarm tone
 338         }
 339     }
 340 }
 341
 342 void tryArm(void)
 343 {
 344     if (armingConfig()->gyro_cal_on_first_arm) {
 345         gyroStartCalibration(true);
 346     }
 347
 348     updateArmingStatus();
 349
 350     if (!isArmingDisabled()) {
 351         if (ARMING_FLAG(ARMED)) {
 352             return;
 353         }
 354 #ifdef USE_DSHOT
 355         if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
 356             tryingToArm = true;
 357             return;
 358         }
 359         if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
 360             if (!IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
 361                 flipOverAfterCrashMode = false;
 362                 if (!feature(FEATURE_3D)) {
 363                     pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
 364                 }
 365             } else {
 366                 flipOverAfterCrashMode = true;
 367 #ifdef USE_RUNAWAY_TAKEOFF
 368                 runawayTakeoffCheckDisabled = false;
 369 #endif
 370                 if (!feature(FEATURE_3D)) {
 371                     pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
 372                 }
 373             }
 374         }
 375 #endif
 376
 377         ENABLE_ARMING_FLAG(ARMED);
 378         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 379
 380         resetTryingToArm();
 381
 382 #ifdef USE_ACRO_TRAINER
 383         pidAcroTrainerInit();
 384 #endif // USE_ACRO_TRAINER
 385
 386         if (isModeActivationConditionPresent(BOXPREARM)) {
 387             ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
 388         }
 389         imuQuaternionHeadfreeOffsetSet();
 390
 391         disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 392
 393         lastArmingDisabledReason = 0;
 394
 395         //beep to indicate arming
 396 #ifdef USE_GPS
 397         if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) {
 398             beeper(BEEPER_ARMING_GPS_FIX);
 399         } else {
 400             beeper(BEEPER_ARMING);
 401         }
 402 #else
 403         beeper(BEEPER_ARMING);
 404 #endif
 405
 406 #ifdef USE_RUNAWAY_TAKEOFF
 407         runawayTakeoffDeactivateUs = 0;
 408         runawayTakeoffAccumulatedUs = 0;
 409         runawayTakeoffTriggerUs = 0;
 410 #endif
 411     } else {
 412        resetTryingToArm();
 413         if (!isFirstArmingGyroCalibrationRunning()) {
 414             int armingDisabledReason = ffs(getArmingDisableFlags());
 415             if (lastArmingDisabledReason != armingDisabledReason) {
 416                 lastArmingDisabledReason = armingDisabledReason;
 417
 418                 beeperWarningBeeps(armingDisabledReason);
 419             }
 420         }
 421     }
 422 }
 423
 424 // Automatic ACC Offset Calibration
 425 bool AccInflightCalibrationArmed = false;
 426 bool AccInflightCalibrationMeasurementDone = false;
 427 bool AccInflightCalibrationSavetoEEProm = false;
 428 bool AccInflightCalibrationActive = false;
 429 uint16_t InflightcalibratingA = 0;
 430
 431 void handleInflightCalibrationStickPosition(void)
 432 {
 433     if (AccInflightCalibrationMeasurementDone) {
 434         // trigger saving into eeprom after landing
 435         AccInflightCalibrationMeasurementDone = false;
 436         AccInflightCalibrationSavetoEEProm = true;
 437     } else {
 438         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 439         if (AccInflightCalibrationArmed) {
 440             beeper(BEEPER_ACC_CALIBRATION);
 441         } else {
 442             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 443         }
 444     }
 445 }
 446
 447 static void updateInflightCalibrationState(void)
 448 {
 449     if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) {   // Copter is airborne and you are turning it off via boxarm : start measurement
 450         InflightcalibratingA = 50;
 451         AccInflightCalibrationArmed = false;
 452     }
 453     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
 454         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 455             InflightcalibratingA = 50;
 456         AccInflightCalibrationActive = true;
 457     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 458         AccInflightCalibrationMeasurementDone = false;
 459         AccInflightCalibrationSavetoEEProm = true;
 460     }
 461 }
 462
 463 #if defined(USE_GPS) || defined(USE_MAG)
 464 void updateMagHold(void)
 465 {
 466     if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 467         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 468         if (dif <= -180)
 469             dif += 360;
 470         if (dif >= +180)
 471             dif -= 360;
 472         dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 473         if (STATE(SMALL_ANGLE))
 474             rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30;    // 18 deg
 475     } else
 476         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 477 }
 478 #endif
 479
 480 #ifdef USE_VTX_CONTROL
 481 static bool canUpdateVTX(void)
 482 {
 483 #ifdef USE_VTX_RTC6705
 484     return vtxRTC6705CanUpdate();
 485 #endif
 486     return true;
 487 }
 488 #endif
 489
 490 #ifdef USE_RUNAWAY_TAKEOFF
 491 // determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
 492 bool areSticksActive(uint8_t stickPercentLimit)
 493 {
 494     for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) {
 495         const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
 496         uint8_t stickPercent = 0;
 497         if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) {
 498             stickPercent = 100;
 499         } else {
 500             if (rcData[axis] > (rxConfig()->midrc + deadband)) {
 501                 stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband);
 502             } else if (rcData[axis] < (rxConfig()->midrc - deadband)) {
 503                 stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN);
 504             }
 505         }
 506         if (stickPercent >= stickPercentLimit) {
 507             return true;
 508         }
 509     }
 510     return false;
 511 }
 512
 513
 514 // allow temporarily disabling runaway takeoff prevention if we are connected
 515 // to the configurator and the ARMING_DISABLED_MSP flag is cleared.
 516 void runawayTakeoffTemporaryDisable(uint8_t disableFlag)
 517 {
 518     runawayTakeoffTemporarilyDisabled = disableFlag;
 519 }
 520 #endif
 521
 522
 523 // calculate the throttle stick percent - integer math is good enough here.
 524 uint8_t calculateThrottlePercent(void)
 525 {
 526     uint8_t ret = 0;
 527     if (feature(FEATURE_3D)
 528         && !IS_RC_MODE_ACTIVE(BOX3D)
 529         && !flight3DConfig()->switched_mode3d) {
 530
 531         if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) {
 532             ret = 100;
 533         } else {
 534             if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
 535                 ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
 536             } else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
 537                 ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
 538             }
 539         }
 540     } else {
 541         ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
 542     }
 543     return ret;
 544 }
 545
 546 static bool airmodeIsActivated;
 547
 548 bool isAirmodeActivated()
 549 {
 550     return airmodeIsActivated;
 551 }
 552
 553
 554
 555 /*
 556  * processRx called from taskUpdateRxMain
 557  */
 558 bool processRx(timeUs_t currentTimeUs)
 559 {
 560     static bool armedBeeperOn = false;
 561     static bool sharedPortTelemetryEnabled = false;
 562
 563     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
 564         return false;
 565     }
 566
 567     // in 3D mode, we need to be able to disarm by switch at any time
 568     if (feature(FEATURE_3D)) {
 569         if (!IS_RC_MODE_ACTIVE(BOXARM))
 570             disarm();
 571     }
 572
 573     updateRSSI(currentTimeUs);
 574
 575     if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 576         failsafeStartMonitoring();
 577     }
 578     failsafeUpdateState();
 579
 580     const throttleStatus_e throttleStatus = calculateThrottleStatus();
 581     const uint8_t throttlePercent = calculateThrottlePercent();
 582
 583     if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
 584         if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
 585             airmodeIsActivated = true; // Prevent Iterm from being reset
 586         }
 587     } else {
 588         airmodeIsActivated = false;
 589     }
 590
 591     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 592      This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
 593     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
 594         pidResetITerm();
 595         if (currentPidProfile->pidAtMinThrottle)
 596             pidStabilisationState(PID_STABILISATION_ON);
 597         else
 598             pidStabilisationState(PID_STABILISATION_OFF);
 599     } else {
 600         pidStabilisationState(PID_STABILISATION_ON);
 601     }
 602
 603 #ifdef USE_RUNAWAY_TAKEOFF
 604     // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
 605     // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
 606     // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
 607     // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
 608     // prevention for the remainder of the battery.
 609
 610     if (ARMING_FLAG(ARMED)
 611         && pidConfig()->runaway_takeoff_prevention
 612         && !runawayTakeoffCheckDisabled
 613         && !flipOverAfterCrashMode
 614         && !runawayTakeoffTemporarilyDisabled
 615         && !STATE(FIXED_WING)) {
 616
 617         // Determine if we're in "flight"
 618         //   - motors running
 619         //   - throttle over runaway_takeoff_deactivate_throttle_percent
 620         //   - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
 621         //   - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
 622         bool inStableFlight = false;
 623         if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
 624             const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
 625             const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
 626             if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
 627                 && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
 628                 && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
 629                 && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
 630
 631                 inStableFlight = true;
 632                 if (runawayTakeoffDeactivateUs == 0) {
 633                     runawayTakeoffDeactivateUs = currentTimeUs;
 634                 }
 635             }
 636         }
 637
 638         // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
 639         if (inStableFlight) {
 640             if (runawayTakeoffDeactivateUs == 0) {
 641                 runawayTakeoffDeactivateUs = currentTimeUs;
 642             }
 643             uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay;
 644             // at high throttle levels reduce deactivation delay by 50%
 645             if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) {
 646                 deactivateDelay = deactivateDelay / 2;
 647             }
 648             if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) {
 649                 runawayTakeoffCheckDisabled = true;
 650             }
 651
 652         } else {
 653             if (runawayTakeoffDeactivateUs != 0) {
 654                 runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
 655             }
 656             runawayTakeoffDeactivateUs = 0;
 657         }
 658         if (runawayTakeoffDeactivateUs == 0) {
 659             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 660             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
 661         } else {
 662             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
 663             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
 664         }
 665     } else {
 666         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 667         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 668     }
 669 #endif
 670
 671     // When armed and motors aren't spinning, do beeps and then disarm
 672     // board after delay so users without buzzer won't lose fingers.
 673     // mixTable constrains motor commands, so checking  throttleStatus is enough
 674     if (ARMING_FLAG(ARMED)
 675         && feature(FEATURE_MOTOR_STOP)
 676         && !STATE(FIXED_WING)
 677         && !feature(FEATURE_3D)
 678         && !isAirmodeActive()
 679     ) {
 680         if (isUsingSticksForArming()) {
 681             if (throttleStatus == THROTTLE_LOW) {
 682                 if (armingConfig()->auto_disarm_delay != 0
 683                     && (int32_t)(disarmAt - millis()) < 0
 684                 ) {
 685                     // auto-disarm configured and delay is over
 686                     disarm();
 687                     armedBeeperOn = false;
 688                 } else {
 689                     // still armed; do warning beeps while armed
 690                     beeper(BEEPER_ARMED);
 691                     armedBeeperOn = true;
 692                 }
 693             } else {
 694                 // throttle is not low
 695                 if (armingConfig()->auto_disarm_delay != 0) {
 696                     // extend disarm time
 697                     disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
 698                 }
 699
 700                 if (armedBeeperOn) {
 701                     beeperSilence();
 702                     armedBeeperOn = false;
 703                 }
 704             }
 705         } else {
 706             // arming is via AUX switch; beep while throttle low
 707             if (throttleStatus == THROTTLE_LOW) {
 708                 beeper(BEEPER_ARMED);
 709                 armedBeeperOn = true;
 710             } else if (armedBeeperOn) {
 711                 beeperSilence();
 712                 armedBeeperOn = false;
 713             }
 714         }
 715     }
 716
 717     processRcStickPositions();
 718
 719     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 720         updateInflightCalibrationState();
 721     }
 722
 723     updateActivatedModes();
 724
 725 #ifdef USE_DSHOT
 726     /* Enable beep warning when the crash flip mode is active */
 727     if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
 728         beeper(BEEPER_CRASH_FLIP_MODE);
 729     }
 730 #endif
 731
 732     if (!cliMode) {
 733         updateAdjustmentStates();
 734         processRcAdjustments(currentControlRateProfile);
 735     }
 736
 737     bool canUseHorizonMode = true;
 738
 739     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) {
 740         // bumpless transfer to Level mode
 741         canUseHorizonMode = false;
 742
 743         if (!FLIGHT_MODE(ANGLE_MODE)) {
 744             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 745         }
 746     } else {
 747         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 748     }
 749
 750     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 751
 752         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 753
 754         if (!FLIGHT_MODE(HORIZON_MODE)) {
 755             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 756         }
 757     } else {
 758         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 759     }
 760
 761 #ifdef USE_GPS_RESCUE
 762     if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE)) {
 763         if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
 764             ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 765         }
 766     } else {
 767         DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 768     }
 769 #endif
 770
 771     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 772         LED1_ON;
 773         // increase frequency of attitude task to reduce drift when in angle or horizon mode
 774         rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
 775     } else {
 776         LED1_OFF;
 777         rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
 778     }
 779
 780     if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
 781         DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
 782     }
 783
 784 #if defined(USE_ACC) || defined(USE_MAG)
 785     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 786 #if defined(USE_GPS) || defined(USE_MAG)
 787         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 788             if (!FLIGHT_MODE(MAG_MODE)) {
 789                 ENABLE_FLIGHT_MODE(MAG_MODE);
 790                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 791             }
 792         } else {
 793             DISABLE_FLIGHT_MODE(MAG_MODE);
 794         }
 795 #endif
 796         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 797             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 798                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 799             }
 800         } else {
 801             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 802         }
 803         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 804             if (imuQuaternionHeadfreeOffsetSet()){
 805                beeper(BEEPER_RX_SET);
 806             }
 807         }
 808     }
 809 #endif
 810
 811     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 812         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 813     } else {
 814         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 815     }
 816
 817     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
 818         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 819     }
 820
 821 #ifdef USE_TELEMETRY
 822     if (feature(FEATURE_TELEMETRY)) {
 823         bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
 824         if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
 825             mspSerialReleaseSharedTelemetryPorts();
 826             telemetryCheckState();
 827
 828             sharedPortTelemetryEnabled = true;
 829         } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
 830             // the telemetry state must be checked immediately so that shared serial ports are released.
 831             telemetryCheckState();
 832             mspSerialAllocatePorts();
 833
 834             sharedPortTelemetryEnabled = false;
 835         }
 836     }
 837 #endif
 838
 839 #ifdef USE_VTX_CONTROL
 840     vtxUpdateActivatedChannel();
 841
 842     if (canUpdateVTX()) {
 843         handleVTXControlButton();
 844     }
 845 #endif
 846
 847 #ifdef USE_ACRO_TRAINER
 848     pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
 849 #endif // USE_ACRO_TRAINER
 850
 851     return true;
 852 }
 853
 854 static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
 855 {
 856     uint32_t startTime = 0;
 857     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 858     // PID - note this is function pointer set by setPIDController()
 859     pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
 860     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
 861
 862 #ifdef USE_RUNAWAY_TAKEOFF
 863     // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
 864     // and gyro rate for any axis is above the limit for at least the activate delay period.
 865     // If so, disarm for safety
 866     if (ARMING_FLAG(ARMED)
 867         && !STATE(FIXED_WING)
 868         && pidConfig()->runaway_takeoff_prevention
 869         && !runawayTakeoffCheckDisabled
 870         && !flipOverAfterCrashMode
 871         && !runawayTakeoffTemporarilyDisabled
 872         && (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
 873
 874         if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
 875             || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
 876             || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
 877             && ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
 878                 || (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
 879                 || (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
 880
 881             if (runawayTakeoffTriggerUs == 0) {
 882                 runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
 883             } else if (currentTimeUs > runawayTakeoffTriggerUs) {
 884                 setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 885                 disarm();
 886             }
 887         } else {
 888             runawayTakeoffTriggerUs = 0;
 889         }
 890         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
 891         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
 892     } else {
 893         runawayTakeoffTriggerUs = 0;
 894         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 895         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 896     }
 897 #endif
 898
 899
 900 #ifdef USE_PID_AUDIO
 901     if (isModeActivationConditionPresent(BOXPIDAUDIO)) {
 902         pidAudioUpdate();
 903     }
 904 #endif
 905 }
 906
 907 static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
 908 {
 909     uint32_t startTime = 0;
 910     if (debugMode == DEBUG_PIDLOOP) {
 911         startTime = micros();
 912     }
 913
 914 #ifdef USE_MAG
 915     if (sensors(SENSOR_MAG)) {
 916         updateMagHold();
 917     }
 918 #endif
 919
 920 #ifdef USE_BLACKBOX
 921     if (!cliMode && blackboxConfig()->device) {
 922         blackboxUpdate(currentTimeUs);
 923     }
 924 #else
 925     UNUSED(currentTimeUs);
 926 #endif
 927
 928     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 929 }
 930
 931 #ifdef USE_TELEMETRY
 932 void subTaskTelemetryPollSensors(timeUs_t currentTimeUs)
 933 {
 934     UNUSED(currentTimeUs);
 935
 936     // Read out gyro temperature if used for telemmetry
 937     gyroReadTemperature();
 938 }
 939 #endif
 940
 941 static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 942 {
 943     uint32_t startTime = 0;
 944     if (debugMode == DEBUG_CYCLETIME) {
 945         startTime = micros();
 946         static uint32_t previousMotorUpdateTime;
 947         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
 948         debug[2] = currentDeltaTime;
 949         debug[3] = currentDeltaTime - targetPidLooptime;
 950         previousMotorUpdateTime = startTime;
 951     } else if (debugMode == DEBUG_PIDLOOP) {
 952         startTime = micros();
 953     }
 954
 955     mixTable(currentTimeUs, currentPidProfile->vbatPidCompensation);
 956
 957 #ifdef USE_SERVOS
 958     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
 959     if (isMixerUsingServos()) {
 960         writeServos();
 961     }
 962 #endif
 963
 964     writeMotors();
 965
 966     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 967 }
 968
 969 static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
 970 {
 971     UNUSED(currentTimeUs);
 972
 973     // If we're armed, at minimum throttle, and we do arming via the
 974     // sticks, do not process yaw input from the rx.  We do this so the
 975     // motors do not spin up while we are trying to arm or disarm.
 976     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 977     if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
 978 #ifndef USE_QUAD_MIXER_ONLY
 979 #ifdef USE_SERVOS
 980                 && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
 981 #endif
 982                 && mixerConfig()->mixerMode != MIXER_AIRPLANE
 983                 && mixerConfig()->mixerMode != MIXER_FLYING_WING
 984 #endif
 985     ) {
 986         resetYawAxis();
 987     }
 988
 989     processRcCommand();
 990
 991 }
 992
 993 // Function for loop trigger
 994 FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
 995 {
 996     static uint32_t pidUpdateCounter = 0;
 997
 998 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
 999     if (lockMainPID() != 0) return;
1000 #endif
1001
1002     // DEBUG_PIDLOOP, timings for:
1003     // 0 - gyroUpdate()
1004     // 1 - subTaskPidController()
1005     // 2 - subTaskMotorUpdate()
1006     // 3 - subTaskPidSubprocesses()
1007     gyroUpdate(currentTimeUs);
1008     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
1009
1010     if (pidUpdateCounter++ % pidConfig()->pid_process_denom == 0) {
1011         subTaskRcCommand(currentTimeUs);
1012         subTaskPidController(currentTimeUs);
1013         subTaskMotorUpdate(currentTimeUs);
1014         subTaskPidSubprocesses(currentTimeUs);
1015     }
1016
1017     if (debugMode == DEBUG_CYCLETIME) {
1018         debug[0] = getTaskDeltaTime(TASK_SELF);
1019         debug[1] = averageSystemLoadPercent;
1020     }
1021 }
1022
1023
1024 bool isFlipOverAfterCrashMode(void)
1025 {
1026     return flipOverAfterCrashMode;
1027 }
1028
1029 timeUs_t getLastDisarmTimeUs(void)
1030 {
1031     return lastDisarmTimeUs;
1032 }
1033
1034 bool isTryingToArm()
1035 {
1036     return tryingToArm;
1037 }
1038
1039 void resetTryingToArm()
1040 {
1041     tryingToArm = false;
1042 }