src/main/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 #include "blackbox/blackbox_fielddefs.h"
  32
  33 #include "cli/cli.h"
  34
  35 #include "cms/cms.h"
  36
  37 #include "common/axis.h"
  38 #include "common/filter.h"
  39 #include "common/maths.h"
  40 #include "common/utils.h"
  41
  42 #include "config/feature.h"
  43
  44 #include "drivers/dshot.h"
  45 #include "drivers/dshot_command.h"
  46 #include "drivers/light_led.h"
  47 #include "drivers/motor.h"
  48 #include "drivers/sound_beeper.h"
  49 #include "drivers/system.h"
  50 #include "drivers/time.h"
  51 #include "drivers/transponder_ir.h"
  52
  53 #include "config/config.h"
  54 #include "fc/controlrate_profile.h"
  55 #include "fc/core.h"
  56 #include "fc/rc.h"
  57 #include "fc/rc_adjustments.h"
  58 #include "fc/rc_controls.h"
  59 #include "fc/runtime_config.h"
  60 #include "fc/stats.h"
  61 #include "fc/tasks.h"
  62
  63 #include "flight/failsafe.h"
  64 #include "flight/gps_rescue.h"
  65 #if defined(USE_GYRO_DATA_ANALYSE)
  66 #include "flight/gyroanalyse.h"
  67 #endif
  68 #include "flight/imu.h"
  69 #include "flight/mixer.h"
  70 #include "flight/pid.h"
  71 #include "flight/position.h"
  72 #include "flight/rpm_filter.h"
  73 #include "flight/servos.h"
  74
  75 #include "io/beeper.h"
  76 #include "io/gps.h"
  77 #include "io/motors.h"
  78 #include "io/pidaudio.h"
  79 #include "io/serial.h"
  80 #include "io/servos.h"
  81 #include "io/statusindicator.h"
  82 #include "io/transponder_ir.h"
  83 #include "io/vtx_control.h"
  84 #include "io/vtx_rtc6705.h"
  85
  86 #include "msp/msp_serial.h"
  87
  88 #include "osd/osd.h"
  89
  90 #include "pg/motor.h"
  91 #include "pg/pg.h"
  92 #include "pg/pg_ids.h"
  93 #include "pg/rx.h"
  94
  95 #include "rx/rx.h"
  96
  97 #include "scheduler/scheduler.h"
  98
  99 #include "sensors/acceleration.h"
 100 #include "sensors/barometer.h"
 101 #include "sensors/battery.h"
 102 #include "sensors/boardalignment.h"
 103 #include "sensors/compass.h"
 104 #include "sensors/gyro.h"
 105
 106 #include "telemetry/telemetry.h"
 107
 108
 109 enum {
 110     ALIGN_GYRO = 0,
 111     ALIGN_ACCEL = 1,
 112     ALIGN_MAG = 2
 113 };
 114
 115 enum {
 116     ARMING_DELAYED_DISARMED = 0,
 117     ARMING_DELAYED_NORMAL = 1,
 118     ARMING_DELAYED_CRASHFLIP = 2,
 119     ARMING_DELAYED_LAUNCH_CONTROL = 3,
 120 };
 121
 122 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
 123
 124 #ifdef USE_RUNAWAY_TAKEOFF
 125 #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
 126 #define RUNAWAY_TAKEOFF_ACTIVATE_DELAY           75000 // (75ms) Time in microseconds where pidSum is above threshold to trigger
 127 #define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15    // 15% - minimum stick deflection during deactivation phase
 128 #define RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT  100   // 10.0% - pidSum limit during deactivation phase
 129 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_RP            15    // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props
 130 #define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW           50    // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props
 131 #define RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT    75    // High throttle limit to accelerate deactivation (halves the deactivation delay)
 132
 133 #define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE      0
 134 #define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY   1
 135 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY 2
 136 #define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME  3
 137
 138 #define DEBUG_RUNAWAY_TAKEOFF_TRUE  1
 139 #define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
 140 #endif
 141
 142 #if defined(USE_GPS) || defined(USE_MAG)
 143 int16_t magHold;
 144 #endif
 145
 146 static FAST_RAM_ZERO_INIT uint8_t pidUpdateCounter;
 147
 148 static bool flipOverAfterCrashActive = false;
 149
 150 static timeUs_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 151
 152 bool isRXDataNew;
 153 static int lastArmingDisabledReason = 0;
 154 static timeUs_t lastDisarmTimeUs;
 155 static int tryingToArm = ARMING_DELAYED_DISARMED;
 156
 157 #ifdef USE_RUNAWAY_TAKEOFF
 158 static timeUs_t runawayTakeoffDeactivateUs = 0;
 159 static timeUs_t runawayTakeoffAccumulatedUs = 0;
 160 static bool runawayTakeoffCheckDisabled = false;
 161 static timeUs_t runawayTakeoffTriggerUs = 0;
 162 static bool runawayTakeoffTemporarilyDisabled = false;
 163 #endif
 164
 165 #ifdef USE_LAUNCH_CONTROL
 166 static launchControlState_e launchControlState = LAUNCH_CONTROL_DISABLED;
 167
 168 const char * const osdLaunchControlModeNames[] = {
 169     "NORMAL",
 170     "PITCHONLY",
 171     "FULL"
 172 };
 173 #endif
 174
 175 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 176
 177 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
 178     .throttle_correction_value = 0,      // could 10 with althold or 40 for fpv
 179     .throttle_correction_angle = 800     // could be 80.0 deg with atlhold or 45.0 for fpv
 180 );
 181
 182 static bool isCalibrating(void)
 183 {
 184     return (sensors(SENSOR_GYRO) && !gyroIsCalibrationComplete())
 185 #ifdef USE_ACC
 186         || (sensors(SENSOR_ACC) && !accIsCalibrationComplete())
 187 #endif
 188 #ifdef USE_BARO
 189         || (sensors(SENSOR_BARO) && !baroIsCalibrationComplete())
 190 #endif
 191 #ifdef USE_MAG
 192         || (sensors(SENSOR_MAG) && !compassIsCalibrationComplete())
 193 #endif
 194         ;
 195 }
 196
 197 #ifdef USE_LAUNCH_CONTROL
 198 bool canUseLaunchControl(void)
 199 {
 200     if (!isFixedWing()
 201         && !isUsingSticksForArming()     // require switch arming for safety
 202         && IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)
 203         && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled())  // can't use when motors are stopped
 204         && !featureIsEnabled(FEATURE_3D) // pitch control is not 3D aware
 205         && (flightModeFlags == 0)) {     // don't want to use unless in acro mode
 206         return true;
 207     }
 208     return false;
 209 }
 210 #endif
 211
 212 void resetArmingDisabled(void)
 213 {
 214     lastArmingDisabledReason = 0;
 215 }
 216
 217 #ifdef USE_ACC
 218 static bool accNeedsCalibration(void)
 219 {
 220     if (sensors(SENSOR_ACC)) {
 221
 222         // Check to see if the ACC has already been calibrated
 223         if (accHasBeenCalibrated()) {
 224             return false;
 225         }
 226
 227         // We've determined that there's a detected ACC that has not
 228         // yet been calibrated. Check to see if anything is using the
 229         // ACC that would be affected by the lack of calibration.
 230
 231         // Check for any configured modes that use the ACC
 232         if (isModeActivationConditionPresent(BOXANGLE) ||
 233             isModeActivationConditionPresent(BOXHORIZON) ||
 234             isModeActivationConditionPresent(BOXGPSRESCUE) ||
 235             isModeActivationConditionPresent(BOXCAMSTAB) ||
 236             isModeActivationConditionPresent(BOXCALIB) ||
 237             isModeActivationConditionPresent(BOXACROTRAINER)) {
 238
 239             return true;
 240         }
 241
 242         // Launch Control only requires the ACC if a angle limit is set
 243         if (isModeActivationConditionPresent(BOXLAUNCHCONTROL) && currentPidProfile->launchControlAngleLimit) {
 244             return true;
 245         }
 246
 247 #ifdef USE_OSD
 248         // Check for any enabled OSD elements that need the ACC
 249         if (featureIsEnabled(FEATURE_OSD)) {
 250             if (osdNeedsAccelerometer()) {
 251                 return true;
 252             }
 253         }
 254 #endif
 255
 256 #ifdef USE_GPS_RESCUE
 257         // Check if failsafe will use GPS Rescue
 258         if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
 259             return true;
 260         }
 261 #endif
 262     }
 263
 264     return false;
 265 }
 266 #endif
 267
 268 void updateArmingStatus(void)
 269 {
 270     if (ARMING_FLAG(ARMED)) {
 271         LED0_ON;
 272     } else {
 273         // Check if the power on arming grace time has elapsed
 274         if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)
 275 #ifdef USE_DSHOT
 276             // We also need to prevent arming until it's possible to send DSHOT commands.
 277             // Otherwise if the initial arming is in crash-flip the motor direction commands
 278             // might not be sent.
 279             && dshotCommandsAreEnabled()
 280 #endif
 281         ) {
 282             // If so, unset the grace time arming disable flag
 283             unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
 284         }
 285
 286         // Clear the crash flip active status
 287         flipOverAfterCrashActive = false;
 288
 289         // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
 290         if (!isUsingSticksForArming()) {
 291             static bool hadRx = false;
 292             const bool haveRx = rxIsReceivingSignal();
 293
 294             const bool justGotRxBack = !hadRx && haveRx;
 295
 296             if (justGotRxBack && IS_RC_MODE_ACTIVE(BOXARM)) {
 297                 // If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction
 298                 setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
 299             } else if (haveRx && !IS_RC_MODE_ACTIVE(BOXARM)) {
 300                 // If RX signal is OK and the arm switch is off, remove arming restriction
 301                 unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
 302             }
 303
 304             hadRx = haveRx;
 305         }
 306
 307         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 308             setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
 309         } else {
 310             unsetArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
 311         }
 312
 313         if (calculateThrottleStatus() != THROTTLE_LOW) {
 314             setArmingDisabled(ARMING_DISABLED_THROTTLE);
 315         } else {
 316             unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
 317         }
 318
 319         if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
 320             setArmingDisabled(ARMING_DISABLED_ANGLE);
 321         } else {
 322             unsetArmingDisabled(ARMING_DISABLED_ANGLE);
 323         }
 324
 325         if (averageSystemLoadPercent > 100) {
 326             setArmingDisabled(ARMING_DISABLED_LOAD);
 327         } else {
 328             unsetArmingDisabled(ARMING_DISABLED_LOAD);
 329         }
 330
 331         if (isCalibrating()) {
 332             setArmingDisabled(ARMING_DISABLED_CALIBRATING);
 333         } else {
 334             unsetArmingDisabled(ARMING_DISABLED_CALIBRATING);
 335         }
 336
 337         if (isModeActivationConditionPresent(BOXPREARM)) {
 338             if (IS_RC_MODE_ACTIVE(BOXPREARM) && !ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
 339                 unsetArmingDisabled(ARMING_DISABLED_NOPREARM);
 340             } else {
 341                 setArmingDisabled(ARMING_DISABLED_NOPREARM);
 342             }
 343         }
 344
 345 #ifdef USE_GPS_RESCUE
 346         if (gpsRescueIsConfigured()) {
 347             if (gpsRescueConfig()->allowArmingWithoutFix || STATE(GPS_FIX) || ARMING_FLAG(WAS_EVER_ARMED)) {
 348                 unsetArmingDisabled(ARMING_DISABLED_GPS);
 349             } else {
 350                 setArmingDisabled(ARMING_DISABLED_GPS);
 351             }
 352             if (IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) {
 353                 setArmingDisabled(ARMING_DISABLED_RESC);
 354             } else {
 355                 unsetArmingDisabled(ARMING_DISABLED_RESC);
 356             }
 357         }
 358 #endif
 359
 360 #ifdef USE_RPM_FILTER
 361         // USE_RPM_FILTER will only be defined if USE_DSHOT and USE_DSHOT_TELEMETRY are defined
 362         // If the RPM filter is anabled and any motor isn't providing telemetry, then disable arming
 363         if (isRpmFilterEnabled() && !isDshotTelemetryActive()) {
 364             setArmingDisabled(ARMING_DISABLED_RPMFILTER);
 365         } else {
 366             unsetArmingDisabled(ARMING_DISABLED_RPMFILTER);
 367         }
 368 #endif
 369
 370 #ifdef USE_DSHOT_BITBANG
 371         if (isDshotBitbangActive(&motorConfig()->dev) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK) {
 372             setArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);
 373         } else {
 374             unsetArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);
 375         }
 376 #endif
 377
 378         if (IS_RC_MODE_ACTIVE(BOXPARALYZE)) {
 379             setArmingDisabled(ARMING_DISABLED_PARALYZE);
 380         }
 381
 382 #ifdef USE_ACC
 383         if (accNeedsCalibration()) {
 384             setArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION);
 385         } else {
 386             unsetArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION);
 387         }
 388 #endif
 389
 390         if (!isUsingSticksForArming()) {
 391           /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */
 392           bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm
 393                          && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_CALIBRATING));
 394
 395           /* Ignore ARMING_DISABLED_THROTTLE (once arm switch is on) if we are in 3D mode */
 396           bool ignoreThrottle = featureIsEnabled(FEATURE_3D)
 397                              && !IS_RC_MODE_ACTIVE(BOX3D)
 398                              && !flight3DConfig()->switched_mode3d
 399                              && !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE));
 400
 401            if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 402 #ifdef USE_RUNAWAY_TAKEOFF
 403                unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
 404 #endif
 405                unsetArmingDisabled(ARMING_DISABLED_CRASH_DETECTED);
 406            }
 407
 408           // If arming is disabled and the ARM switch is on
 409           if (isArmingDisabled()
 410               && !ignoreGyro
 411               && !ignoreThrottle
 412               && IS_RC_MODE_ACTIVE(BOXARM)) {
 413               setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 414           } else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 415               unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
 416           }
 417         }
 418
 419         if (isArmingDisabled()) {
 420             warningLedFlash();
 421         } else {
 422             warningLedDisable();
 423         }
 424
 425         warningLedUpdate();
 426     }
 427 }
 428
 429 void disarm(flightLogDisarmReason_e reason)
 430 {
 431     if (ARMING_FLAG(ARMED)) {
 432         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 433         DISABLE_ARMING_FLAG(ARMED);
 434         lastDisarmTimeUs = micros();
 435
 436 #ifdef USE_OSD
 437         if (flipOverAfterCrashActive || isLaunchControlActive()) {
 438             osdSuppressStats(true);
 439         }
 440 #endif
 441
 442 #ifdef USE_BLACKBOX
 443         flightLogEvent_disarm_t eventData;
 444         eventData.reason = reason;
 445         blackboxLogEvent(FLIGHT_LOG_EVENT_DISARM, (flightLogEventData_t*)&eventData);
 446
 447         if (blackboxConfig()->device && blackboxConfig()->mode != BLACKBOX_MODE_ALWAYS_ON) { // Close the log upon disarm except when logging mode is ALWAYS ON
 448             blackboxFinish();
 449         }
 450 #else
 451         UNUSED(reason);
 452 #endif
 453         BEEP_OFF;
 454 #ifdef USE_DSHOT
 455         if (isMotorProtocolDshot() && flipOverAfterCrashActive && !featureIsEnabled(FEATURE_3D)) {
 456             dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
 457         }
 458 #endif
 459         flipOverAfterCrashActive = false;
 460
 461 #ifdef USE_PERSISTENT_STATS
 462         statsOnDisarm();
 463 #endif
 464
 465         // if ARMING_DISABLED_RUNAWAY_TAKEOFF is set then we want to play it's beep pattern instead
 466         if (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))) {
 467             beeper(BEEPER_DISARMING);      // emit disarm tone
 468         }
 469     }
 470 }
 471
 472 void tryArm(void)
 473 {
 474     if (armingConfig()->gyro_cal_on_first_arm) {
 475         gyroStartCalibration(true);
 476     }
 477
 478     updateArmingStatus();
 479
 480     if (!isArmingDisabled()) {
 481         if (ARMING_FLAG(ARMED)) {
 482             return;
 483         }
 484
 485         const timeUs_t currentTimeUs = micros();
 486
 487 #ifdef USE_DSHOT
 488         if (currentTimeUs - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) {
 489             if (tryingToArm == ARMING_DELAYED_DISARMED) {
 490                 if (IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
 491                     tryingToArm = ARMING_DELAYED_CRASHFLIP;
 492 #ifdef USE_LAUNCH_CONTROL
 493                 } else if (canUseLaunchControl()) {
 494                     tryingToArm = ARMING_DELAYED_LAUNCH_CONTROL;
 495 #endif
 496                 } else {
 497                     tryingToArm = ARMING_DELAYED_NORMAL;
 498                 }
 499             }
 500             return;
 501         }
 502
 503         if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) {
 504             if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) {
 505                 flipOverAfterCrashActive = false;
 506                 if (!featureIsEnabled(FEATURE_3D)) {
 507                     dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false);
 508                 }
 509             } else {
 510                 flipOverAfterCrashActive = true;
 511 #ifdef USE_RUNAWAY_TAKEOFF
 512                 runawayTakeoffCheckDisabled = false;
 513 #endif
 514                 if (!featureIsEnabled(FEATURE_3D)) {
 515                     dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false);
 516                 }
 517             }
 518         }
 519 #endif
 520
 521 #ifdef USE_LAUNCH_CONTROL
 522         if (!flipOverAfterCrashActive && (canUseLaunchControl() || (tryingToArm == ARMING_DELAYED_LAUNCH_CONTROL))) {
 523             if (launchControlState == LAUNCH_CONTROL_DISABLED) {  // only activate if it hasn't already been triggered
 524                 launchControlState = LAUNCH_CONTROL_ACTIVE;
 525             }
 526         }
 527 #endif
 528
 529 #ifdef USE_OSD
 530         osdSuppressStats(false);
 531 #endif
 532         ENABLE_ARMING_FLAG(ARMED);
 533
 534         resetTryingToArm();
 535
 536 #ifdef USE_ACRO_TRAINER
 537         pidAcroTrainerInit();
 538 #endif // USE_ACRO_TRAINER
 539
 540         if (isModeActivationConditionPresent(BOXPREARM)) {
 541             ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
 542         }
 543         imuQuaternionHeadfreeOffsetSet();
 544
 545 #if defined(USE_GYRO_DATA_ANALYSE)
 546         resetMaxFFT();
 547 #endif
 548
 549         disarmAt = currentTimeUs + armingConfig()->auto_disarm_delay * 1e6;   // start disarm timeout, will be extended when throttle is nonzero
 550
 551         lastArmingDisabledReason = 0;
 552
 553 #ifdef USE_GPS
 554         GPS_reset_home_position();
 555
 556         //beep to indicate arming
 557         if (featureIsEnabled(FEATURE_GPS)) {
 558             if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
 559                 beeper(BEEPER_ARMING_GPS_FIX);
 560             } else {
 561                 beeper(BEEPER_ARMING_GPS_NO_FIX);
 562             }
 563         } else {
 564             beeper(BEEPER_ARMING);
 565         }
 566 #else
 567         beeper(BEEPER_ARMING);
 568 #endif
 569
 570 #ifdef USE_PERSISTENT_STATS
 571         statsOnArm();
 572 #endif
 573
 574 #ifdef USE_RUNAWAY_TAKEOFF
 575         runawayTakeoffDeactivateUs = 0;
 576         runawayTakeoffAccumulatedUs = 0;
 577         runawayTakeoffTriggerUs = 0;
 578 #endif
 579     } else {
 580        resetTryingToArm();
 581         if (!isFirstArmingGyroCalibrationRunning()) {
 582             int armingDisabledReason = ffs(getArmingDisableFlags());
 583             if (lastArmingDisabledReason != armingDisabledReason) {
 584                 lastArmingDisabledReason = armingDisabledReason;
 585
 586                 beeperWarningBeeps(armingDisabledReason);
 587             }
 588         }
 589     }
 590 }
 591
 592 // Automatic ACC Offset Calibration
 593 bool AccInflightCalibrationArmed = false;
 594 bool AccInflightCalibrationMeasurementDone = false;
 595 bool AccInflightCalibrationSavetoEEProm = false;
 596 bool AccInflightCalibrationActive = false;
 597 uint16_t InflightcalibratingA = 0;
 598
 599 void handleInflightCalibrationStickPosition(void)
 600 {
 601     if (AccInflightCalibrationMeasurementDone) {
 602         // trigger saving into eeprom after landing
 603         AccInflightCalibrationMeasurementDone = false;
 604         AccInflightCalibrationSavetoEEProm = true;
 605     } else {
 606         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 607         if (AccInflightCalibrationArmed) {
 608             beeper(BEEPER_ACC_CALIBRATION);
 609         } else {
 610             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 611         }
 612     }
 613 }
 614
 615 static void updateInflightCalibrationState(void)
 616 {
 617     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
 618         InflightcalibratingA = 50;
 619         AccInflightCalibrationArmed = false;
 620     }
 621     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
 622         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 623             InflightcalibratingA = 50;
 624         AccInflightCalibrationActive = true;
 625     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 626         AccInflightCalibrationMeasurementDone = false;
 627         AccInflightCalibrationSavetoEEProm = true;
 628     }
 629 }
 630
 631 #if defined(USE_GPS) || defined(USE_MAG)
 632 static void updateMagHold(void)
 633 {
 634     if (fabsf(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 635         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 636         if (dif <= -180)
 637             dif += 360;
 638         if (dif >= +180)
 639             dif -= 360;
 640         dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 641         if (isUpright()) {
 642             rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30;    // 18 deg
 643         }
 644     } else
 645         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 646 }
 647 #endif
 648
 649 #ifdef USE_VTX_CONTROL
 650 static bool canUpdateVTX(void)
 651 {
 652 #ifdef USE_VTX_RTC6705
 653     return vtxRTC6705CanUpdate();
 654 #endif
 655     return true;
 656 }
 657 #endif
 658
 659 #if defined(USE_RUNAWAY_TAKEOFF) || defined(USE_GPS_RESCUE)
 660 // determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
 661 bool areSticksActive(uint8_t stickPercentLimit)
 662 {
 663     for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) {
 664         const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
 665         uint8_t stickPercent = 0;
 666         if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) {
 667             stickPercent = 100;
 668         } else {
 669             if (rcData[axis] > (rxConfig()->midrc + deadband)) {
 670                 stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband);
 671             } else if (rcData[axis] < (rxConfig()->midrc - deadband)) {
 672                 stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN);
 673             }
 674         }
 675         if (stickPercent >= stickPercentLimit) {
 676             return true;
 677         }
 678     }
 679     return false;
 680 }
 681 #endif
 682
 683 #ifdef USE_RUNAWAY_TAKEOFF
 684 // allow temporarily disabling runaway takeoff prevention if we are connected
 685 // to the configurator and the ARMING_DISABLED_MSP flag is cleared.
 686 void runawayTakeoffTemporaryDisable(uint8_t disableFlag)
 687 {
 688     runawayTakeoffTemporarilyDisabled = disableFlag;
 689 }
 690 #endif
 691
 692
 693 // calculate the throttle stick percent - integer math is good enough here.
 694 // returns negative values for reversed thrust in 3D mode
 695 int8_t calculateThrottlePercent(void)
 696 {
 697     uint8_t ret = 0;
 698     int channelData = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 699
 700     if (featureIsEnabled(FEATURE_3D)
 701         && !IS_RC_MODE_ACTIVE(BOX3D)
 702         && !flight3DConfig()->switched_mode3d) {
 703
 704         if (channelData > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
 705             ret = ((channelData - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
 706         } else if (channelData < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
 707             ret = -((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - channelData) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
 708         }
 709     } else {
 710         ret = constrain(((channelData - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
 711         if (featureIsEnabled(FEATURE_3D)
 712             && IS_RC_MODE_ACTIVE(BOX3D)
 713             && flight3DConfig()->switched_mode3d) {
 714
 715             ret = -ret;  // 3D on a switch is active
 716         }
 717     }
 718     return ret;
 719 }
 720
 721 uint8_t calculateThrottlePercentAbs(void)
 722 {
 723     return ABS(calculateThrottlePercent());
 724 }
 725
 726 static bool airmodeIsActivated;
 727
 728 bool isAirmodeActivated()
 729 {
 730     return airmodeIsActivated;
 731 }
 732
 733
 734
 735 /*
 736  * processRx called from taskUpdateRxMain
 737  */
 738 bool processRx(timeUs_t currentTimeUs)
 739 {
 740     static bool armedBeeperOn = false;
 741 #ifdef USE_TELEMETRY
 742     static bool sharedPortTelemetryEnabled = false;
 743 #endif
 744
 745     if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
 746         return false;
 747     }
 748
 749     // in 3D mode, we need to be able to disarm by switch at any time
 750     if (featureIsEnabled(FEATURE_3D)) {
 751         if (!IS_RC_MODE_ACTIVE(BOXARM))
 752             disarm(DISARM_REASON_SWITCH);
 753     }
 754
 755     updateRSSI(currentTimeUs);
 756
 757     if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 758         failsafeStartMonitoring();
 759     }
 760     failsafeUpdateState();
 761
 762     const throttleStatus_e throttleStatus = calculateThrottleStatus();
 763     const uint8_t throttlePercent = calculateThrottlePercentAbs();
 764
 765     const bool launchControlActive = isLaunchControlActive();
 766
 767     if (airmodeIsEnabled() && ARMING_FLAG(ARMED) && !launchControlActive) {
 768         if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
 769             airmodeIsActivated = true; // Prevent iterm from being reset
 770         }
 771     } else {
 772         airmodeIsActivated = false;
 773     }
 774
 775     /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 776      This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
 777     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) {
 778         pidSetItermReset(true);
 779         if (currentPidProfile->pidAtMinThrottle)
 780             pidStabilisationState(PID_STABILISATION_ON);
 781         else
 782             pidStabilisationState(PID_STABILISATION_OFF);
 783     } else {
 784         pidSetItermReset(false);
 785         pidStabilisationState(PID_STABILISATION_ON);
 786     }
 787
 788 #ifdef USE_RUNAWAY_TAKEOFF
 789     // If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
 790     // is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
 791     // to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
 792     // Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
 793     // prevention for the remainder of the battery.
 794
 795     if (ARMING_FLAG(ARMED)
 796         && pidConfig()->runaway_takeoff_prevention
 797         && !runawayTakeoffCheckDisabled
 798         && !flipOverAfterCrashActive
 799         && !runawayTakeoffTemporarilyDisabled
 800         && !isFixedWing()) {
 801
 802         // Determine if we're in "flight"
 803         //   - motors running
 804         //   - throttle over runaway_takeoff_deactivate_throttle_percent
 805         //   - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
 806         //   - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
 807         bool inStableFlight = false;
 808         if (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
 809             const uint8_t lowThrottleLimit = pidConfig()->runaway_takeoff_deactivate_throttle;
 810             const uint8_t midThrottleLimit = constrain(lowThrottleLimit * 2, lowThrottleLimit * 2, RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT);
 811             if ((((throttlePercent >= lowThrottleLimit) && areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)) || (throttlePercent >= midThrottleLimit))
 812                 && (fabsf(pidData[FD_PITCH].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
 813                 && (fabsf(pidData[FD_ROLL].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
 814                 && (fabsf(pidData[FD_YAW].Sum) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
 815
 816                 inStableFlight = true;
 817                 if (runawayTakeoffDeactivateUs == 0) {
 818                     runawayTakeoffDeactivateUs = currentTimeUs;
 819                 }
 820             }
 821         }
 822
 823         // If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
 824         if (inStableFlight) {
 825             if (runawayTakeoffDeactivateUs == 0) {
 826                 runawayTakeoffDeactivateUs = currentTimeUs;
 827             }
 828             uint16_t deactivateDelay = pidConfig()->runaway_takeoff_deactivate_delay;
 829             // at high throttle levels reduce deactivation delay by 50%
 830             if (throttlePercent >= RUNAWAY_TAKEOFF_HIGH_THROTTLE_PERCENT) {
 831                 deactivateDelay = deactivateDelay / 2;
 832             }
 833             if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > deactivateDelay * 1000) {
 834                 runawayTakeoffCheckDisabled = true;
 835             }
 836
 837         } else {
 838             if (runawayTakeoffDeactivateUs != 0) {
 839                 runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
 840             }
 841             runawayTakeoffDeactivateUs = 0;
 842         }
 843         if (runawayTakeoffDeactivateUs == 0) {
 844             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 845             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
 846         } else {
 847             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
 848             DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
 849         }
 850     } else {
 851         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 852         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
 853     }
 854 #endif
 855
 856 #ifdef USE_LAUNCH_CONTROL
 857     if (ARMING_FLAG(ARMED)) {
 858         if (launchControlActive && (throttlePercent > currentPidProfile->launchControlThrottlePercent)) {
 859             // throttle limit trigger reached, launch triggered
 860             // reset the iterms as they may be at high values from holding the launch position
 861             launchControlState = LAUNCH_CONTROL_TRIGGERED;
 862             pidResetIterm();
 863         }
 864     } else {
 865         if (launchControlState == LAUNCH_CONTROL_TRIGGERED) {
 866             // If trigger mode is MULTIPLE then reset the state when disarmed
 867             // and the mode switch is turned off.
 868             // For trigger mode SINGLE we never reset the state and only a single
 869             // launch is allowed until a reboot.
 870             if (currentPidProfile->launchControlAllowTriggerReset && !IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)) {
 871                 launchControlState = LAUNCH_CONTROL_DISABLED;
 872             }
 873         } else {
 874             launchControlState = LAUNCH_CONTROL_DISABLED;
 875         }
 876     }
 877 #endif
 878
 879     // When armed and motors aren't spinning, do beeps and then disarm
 880     // board after delay so users without buzzer won't lose fingers.
 881     // mixTable constrains motor commands, so checking  throttleStatus is enough
 882     const timeUs_t autoDisarmDelayUs = armingConfig()->auto_disarm_delay * 1e6;
 883     if (ARMING_FLAG(ARMED)
 884         && featureIsEnabled(FEATURE_MOTOR_STOP)
 885         && !isFixedWing()
 886         && !featureIsEnabled(FEATURE_3D)
 887         && !airmodeIsEnabled()
 888         && !FLIGHT_MODE(GPS_RESCUE_MODE)  // disable auto-disarm when GPS Rescue is active
 889     ) {
 890         if (isUsingSticksForArming()) {
 891             if (throttleStatus == THROTTLE_LOW) {
 892                 if ((autoDisarmDelayUs > 0) && (currentTimeUs > disarmAt)) {
 893                     // auto-disarm configured and delay is over
 894                     disarm(DISARM_REASON_THROTTLE_TIMEOUT);
 895                     armedBeeperOn = false;
 896                 } else {
 897                     // still armed; do warning beeps while armed
 898                     beeper(BEEPER_ARMED);
 899                     armedBeeperOn = true;
 900                 }
 901             } else {
 902                 // throttle is not low - extend disarm time
 903                 disarmAt = currentTimeUs + autoDisarmDelayUs;
 904
 905                 if (armedBeeperOn) {
 906                     beeperSilence();
 907                     armedBeeperOn = false;
 908                 }
 909             }
 910         } else {
 911             // arming is via AUX switch; beep while throttle low
 912             if (throttleStatus == THROTTLE_LOW) {
 913                 beeper(BEEPER_ARMED);
 914                 armedBeeperOn = true;
 915             } else if (armedBeeperOn) {
 916                 beeperSilence();
 917                 armedBeeperOn = false;
 918             }
 919         }
 920     } else {
 921         disarmAt = currentTimeUs + autoDisarmDelayUs;  // extend auto-disarm timer
 922     }
 923
 924     if (!IS_RC_MODE_ACTIVE(BOXPARALYZE)
 925 #ifdef USE_CMS
 926         && !cmsInMenu
 927 #endif
 928         ) {
 929         processRcStickPositions();
 930     }
 931
 932     if (featureIsEnabled(FEATURE_INFLIGHT_ACC_CAL)) {
 933         updateInflightCalibrationState();
 934     }
 935
 936     updateActivatedModes();
 937
 938 #ifdef USE_DSHOT
 939     /* Enable beep warning when the crash flip mode is active */
 940     if (flipOverAfterCrashActive) {
 941         beeper(BEEPER_CRASH_FLIP_MODE);
 942     }
 943 #endif
 944
 945     if (!cliMode && !IS_RC_MODE_ACTIVE(BOXPARALYZE)) {
 946         processRcAdjustments(currentControlRateProfile);
 947     }
 948
 949     bool canUseHorizonMode = true;
 950
 951     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeIsActive()) && (sensors(SENSOR_ACC))) {
 952         // bumpless transfer to Level mode
 953         canUseHorizonMode = false;
 954
 955         if (!FLIGHT_MODE(ANGLE_MODE)) {
 956             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 957         }
 958     } else {
 959         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 960     }
 961
 962     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 963
 964         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 965
 966         if (!FLIGHT_MODE(HORIZON_MODE)) {
 967             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 968         }
 969     } else {
 970         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 971     }
 972
 973 #ifdef USE_GPS_RESCUE
 974     if (ARMING_FLAG(ARMED) && (IS_RC_MODE_ACTIVE(BOXGPSRESCUE) || (failsafeIsActive() && failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE))) {
 975         if (!FLIGHT_MODE(GPS_RESCUE_MODE)) {
 976             ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 977         }
 978     } else {
 979         DISABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
 980     }
 981 #endif
 982
 983     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 984         LED1_ON;
 985         // increase frequency of attitude task to reduce drift when in angle or horizon mode
 986         rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(500));
 987     } else {
 988         LED1_OFF;
 989         rescheduleTask(TASK_ATTITUDE, TASK_PERIOD_HZ(100));
 990     }
 991
 992     if (!IS_RC_MODE_ACTIVE(BOXPREARM) && ARMING_FLAG(WAS_ARMED_WITH_PREARM)) {
 993         DISABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM);
 994     }
 995
 996 #if defined(USE_ACC) || defined(USE_MAG)
 997     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 998 #if defined(USE_GPS) || defined(USE_MAG)
 999         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
1000             if (!FLIGHT_MODE(MAG_MODE)) {
1001                 ENABLE_FLIGHT_MODE(MAG_MODE);
1002                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
1003             }
1004         } else {
1005             DISABLE_FLIGHT_MODE(MAG_MODE);
1006         }
1007 #endif
1008         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
1009             if (!FLIGHT_MODE(HEADFREE_MODE)) {
1010                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
1011             }
1012         } else {
1013             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
1014         }
1015         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
1016             if (imuQuaternionHeadfreeOffsetSet()) {
1017                beeper(BEEPER_RX_SET);
1018             }
1019         }
1020     }
1021 #endif
1022
1023     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
1024         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
1025     } else {
1026         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
1027     }
1028
1029     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
1030         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
1031     }
1032
1033 #ifdef USE_TELEMETRY
1034     if (featureIsEnabled(FEATURE_TELEMETRY)) {
1035         bool enableSharedPortTelemetry = (!isModeActivationConditionPresent(BOXTELEMETRY) && ARMING_FLAG(ARMED)) || (isModeActivationConditionPresent(BOXTELEMETRY) && IS_RC_MODE_ACTIVE(BOXTELEMETRY));
1036         if (enableSharedPortTelemetry && !sharedPortTelemetryEnabled) {
1037             mspSerialReleaseSharedTelemetryPorts();
1038             telemetryCheckState();
1039
1040             sharedPortTelemetryEnabled = true;
1041         } else if (!enableSharedPortTelemetry && sharedPortTelemetryEnabled) {
1042             // the telemetry state must be checked immediately so that shared serial ports are released.
1043             telemetryCheckState();
1044             mspSerialAllocatePorts();
1045
1046             sharedPortTelemetryEnabled = false;
1047         }
1048     }
1049 #endif
1050
1051 #ifdef USE_VTX_CONTROL
1052     vtxUpdateActivatedChannel();
1053
1054     if (canUpdateVTX()) {
1055         handleVTXControlButton();
1056     }
1057 #endif
1058
1059 #ifdef USE_ACRO_TRAINER
1060     pidSetAcroTrainerState(IS_RC_MODE_ACTIVE(BOXACROTRAINER) && sensors(SENSOR_ACC));
1061 #endif // USE_ACRO_TRAINER
1062
1063 #ifdef USE_RC_SMOOTHING_FILTER
1064     if (ARMING_FLAG(ARMED) && !rcSmoothingInitializationComplete()) {
1065         beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
1066     }
1067 #endif
1068
1069     pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || featureIsEnabled(FEATURE_ANTI_GRAVITY));
1070
1071     return true;
1072 }
1073
1074 static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
1075 {
1076     uint32_t startTime = 0;
1077     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
1078     // PID - note this is function pointer set by setPIDController()
1079     pidController(currentPidProfile, currentTimeUs);
1080     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
1081
1082 #ifdef USE_RUNAWAY_TAKEOFF
1083     // Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
1084     // and gyro rate for any axis is above the limit for at least the activate delay period.
1085     // If so, disarm for safety
1086     if (ARMING_FLAG(ARMED)
1087         && !isFixedWing()
1088         && pidConfig()->runaway_takeoff_prevention
1089         && !runawayTakeoffCheckDisabled
1090         && !flipOverAfterCrashActive
1091         && !runawayTakeoffTemporarilyDisabled
1092         && !FLIGHT_MODE(GPS_RESCUE_MODE)   // disable Runaway Takeoff triggering if GPS Rescue is active
1093         && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled() || (calculateThrottleStatus() != THROTTLE_LOW))) {
1094
1095         if (((fabsf(pidData[FD_PITCH].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
1096             || (fabsf(pidData[FD_ROLL].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD)
1097             || (fabsf(pidData[FD_YAW].Sum) >= RUNAWAY_TAKEOFF_PIDSUM_THRESHOLD))
1098             && ((gyroAbsRateDps(FD_PITCH) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
1099                 || (gyroAbsRateDps(FD_ROLL) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
1100                 || (gyroAbsRateDps(FD_YAW) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
1101
1102             if (runawayTakeoffTriggerUs == 0) {
1103                 runawayTakeoffTriggerUs = currentTimeUs + RUNAWAY_TAKEOFF_ACTIVATE_DELAY;
1104             } else if (currentTimeUs > runawayTakeoffTriggerUs) {
1105                 setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
1106                 disarm(DISARM_REASON_RUNAWAY_TAKEOFF);
1107             }
1108         } else {
1109             runawayTakeoffTriggerUs = 0;
1110         }
1111         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
1112         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
1113     } else {
1114         runawayTakeoffTriggerUs = 0;
1115         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
1116         DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
1117     }
1118 #endif
1119
1120
1121 #ifdef USE_PID_AUDIO
1122     if (isModeActivationConditionPresent(BOXPIDAUDIO)) {
1123         pidAudioUpdate();
1124     }
1125 #endif
1126 }
1127
1128 static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
1129 {
1130     uint32_t startTime = 0;
1131     if (debugMode == DEBUG_PIDLOOP) {
1132         startTime = micros();
1133     }
1134
1135 #if defined(USE_GPS) || defined(USE_MAG)
1136     if (sensors(SENSOR_GPS) || sensors(SENSOR_MAG)) {
1137         updateMagHold();
1138     }
1139 #endif
1140
1141 #ifdef USE_BLACKBOX
1142     if (!cliMode && blackboxConfig()->device) {
1143         blackboxUpdate(currentTimeUs);
1144     }
1145 #else
1146     UNUSED(currentTimeUs);
1147 #endif
1148
1149     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
1150 }
1151
1152 #ifdef USE_TELEMETRY
1153 void subTaskTelemetryPollSensors(timeUs_t currentTimeUs)
1154 {
1155     UNUSED(currentTimeUs);
1156
1157     // Read out gyro temperature if used for telemmetry
1158     gyroReadTemperature();
1159 }
1160 #endif
1161
1162 static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
1163 {
1164     uint32_t startTime = 0;
1165     if (debugMode == DEBUG_CYCLETIME) {
1166         startTime = micros();
1167         static uint32_t previousMotorUpdateTime;
1168         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
1169         debug[2] = currentDeltaTime;
1170         debug[3] = currentDeltaTime - targetPidLooptime;
1171         previousMotorUpdateTime = startTime;
1172     } else if (debugMode == DEBUG_PIDLOOP) {
1173         startTime = micros();
1174     }
1175
1176     mixTable(currentTimeUs, currentPidProfile->vbatPidCompensation);
1177
1178 #ifdef USE_SERVOS
1179     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
1180     if (isMixerUsingServos()) {
1181         writeServos();
1182     }
1183 #endif
1184
1185     writeMotors();
1186
1187 #ifdef USE_DSHOT_TELEMETRY_STATS
1188     if (debugMode == DEBUG_DSHOT_RPM_ERRORS && useDshotTelemetry) {
1189         const uint8_t motorCount = MIN(getMotorCount(), 4);
1190         for (uint8_t i = 0; i < motorCount; i++) {
1191             debug[i] = getDshotTelemetryMotorInvalidPercent(i);
1192         }
1193     }
1194 #endif
1195
1196     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
1197 }
1198
1199 static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs)
1200 {
1201     UNUSED(currentTimeUs);
1202
1203     // If we're armed, at minimum throttle, and we do arming via the
1204     // sticks, do not process yaw input from the rx.  We do this so the
1205     // motors do not spin up while we are trying to arm or disarm.
1206     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
1207     if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
1208 #ifndef USE_QUAD_MIXER_ONLY
1209 #ifdef USE_SERVOS
1210                 && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
1211 #endif
1212                 && mixerConfig()->mixerMode != MIXER_AIRPLANE
1213                 && mixerConfig()->mixerMode != MIXER_FLYING_WING
1214 #endif
1215     ) {
1216         resetYawAxis();
1217     }
1218
1219     processRcCommand();
1220 }
1221
1222 FAST_CODE void taskGyroSample(timeUs_t currentTimeUs)
1223 {
1224     UNUSED(currentTimeUs);
1225     gyroUpdate();
1226     if (pidUpdateCounter % activePidLoopDenom == 0) {
1227         pidUpdateCounter = 0;
1228     }
1229     pidUpdateCounter++;
1230 }
1231
1232 FAST_CODE bool gyroFilterReady(void)
1233 {
1234     if (pidUpdateCounter % activePidLoopDenom == 0) {
1235         return true;
1236     } else {
1237         return false;
1238     }
1239 }
1240
1241 FAST_CODE bool pidLoopReady(void)
1242 {
1243     if ((pidUpdateCounter % activePidLoopDenom) == (activePidLoopDenom / 2)) {
1244         return true;
1245     }
1246     return false;
1247 }
1248
1249 FAST_CODE void taskFiltering(timeUs_t currentTimeUs)
1250 {
1251     gyroFiltering(currentTimeUs);
1252
1253 }
1254
1255 // Function for loop trigger
1256 FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
1257 {
1258
1259 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
1260     if (lockMainPID() != 0) return;
1261 #endif
1262
1263     // DEBUG_PIDLOOP, timings for:
1264     // 0 - gyroUpdate()
1265     // 1 - subTaskPidController()
1266     // 2 - subTaskMotorUpdate()
1267     // 3 - subTaskPidSubprocesses()
1268     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs);
1269
1270     subTaskRcCommand(currentTimeUs);
1271     subTaskPidController(currentTimeUs);
1272     subTaskMotorUpdate(currentTimeUs);
1273     subTaskPidSubprocesses(currentTimeUs);
1274
1275     if (debugMode == DEBUG_CYCLETIME) {
1276         debug[0] = getTaskDeltaTime(TASK_SELF);
1277         debug[1] = averageSystemLoadPercent;
1278     }
1279 }
1280
1281 bool isFlipOverAfterCrashActive(void)
1282 {
1283     return flipOverAfterCrashActive;
1284 }
1285
1286 timeUs_t getLastDisarmTimeUs(void)
1287 {
1288     return lastDisarmTimeUs;
1289 }
1290
1291 bool isTryingToArm()
1292 {
1293     return (tryingToArm != ARMING_DELAYED_DISARMED);
1294 }
1295
1296 void resetTryingToArm()
1297 {
1298     tryingToArm = ARMING_DELAYED_DISARMED;
1299 }
1300
1301 bool isLaunchControlActive(void)
1302 {
1303 #ifdef USE_LAUNCH_CONTROL
1304     return launchControlState == LAUNCH_CONTROL_ACTIVE;
1305 #else
1306     return false;
1307 #endif
1308 }