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