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