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