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