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