src/main/fc/fc_core.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdlib.h>
  20 #include <stdint.h>
  21
  22 #include "platform.h"
  23
  24 #include "blackbox/blackbox.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30 #include "common/color.h"
  31 #include "common/utils.h"
  32 #include "common/filter.h"
  33
  34 #include "drivers/light_led.h"
  35 #include "drivers/serial.h"
  36 #include "drivers/time.h"
  37 #include "drivers/system.h"
  38 #include "drivers/pwm_output.h"
  39
  40 #include "sensors/sensors.h"
  41 #include "sensors/diagnostics.h"
  42 #include "sensors/boardalignment.h"
  43 #include "sensors/acceleration.h"
  44 #include "sensors/barometer.h"
  45 #include "sensors/compass.h"
  46 #include "sensors/pitotmeter.h"
  47 #include "sensors/gyro.h"
  48 #include "sensors/battery.h"
  49 #include "sensors/rangefinder.h"
  50 #include "sensors/opflow.h"
  51 #include "sensors/esc_sensor.h"
  52
  53 #include "fc/fc_core.h"
  54 #include "fc/cli.h"
  55 #include "fc/config.h"
  56 #include "fc/controlrate_profile.h"
  57 #include "fc/multifunction.h"
  58 #include "fc/rc_adjustments.h"
  59 #include "fc/rc_smoothing.h"
  60 #include "fc/rc_controls.h"
  61 #include "fc/rc_curves.h"
  62 #include "fc/rc_modes.h"
  63 #include "fc/runtime_config.h"
  64
  65 #include "io/beeper.h"
  66 #include "io/dashboard.h"
  67 #include "io/gps.h"
  68 #include "io/serial.h"
  69 #include "io/statusindicator.h"
  70 #include "io/asyncfatfs/asyncfatfs.h"
  71 #include "io/piniobox.h"
  72
  73 #include "msp/msp_serial.h"
  74
  75 #include "navigation/navigation.h"
  76
  77 #include "rx/rx.h"
  78 #include "rx/msp.h"
  79
  80 #include "scheduler/scheduler.h"
  81
  82 #include "telemetry/telemetry.h"
  83
  84 #include "flight/mixer_profile.h"
  85 #include "flight/mixer.h"
  86 #include "flight/servos.h"
  87 #include "flight/pid.h"
  88 #include "flight/imu.h"
  89 #include "flight/rate_dynamics.h"
  90
  91 #include "flight/failsafe.h"
  92 #include "flight/power_limits.h"
  93
  94 #include "config/feature.h"
  95 #include "common/vector.h"
  96 #include "programming/pid.h"
  97
  98 // June 2013     V2.2-dev
  99
 100 enum {
 101     ALIGN_GYRO = 0,
 102     ALIGN_ACCEL = 1,
 103     ALIGN_MAG = 2
 104 };
 105
 106 #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000
 107 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000
 108 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10
 109 #define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000
 110
 111 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 112 static timeUs_t flightTime = 0;
 113 static timeUs_t armTime = 0;
 114
 115 EXTENDED_FASTRAM float dT;
 116
 117 int16_t headFreeModeHold;
 118
 119 uint8_t motorControlEnable = false;
 120
 121 static bool isRXDataNew;
 122 static disarmReason_t lastDisarmReason = DISARM_NONE;
 123 timeUs_t lastDisarmTimeUs = 0;
 124 timeMs_t emergRearmStabiliseTimeout = 0;
 125
 126 static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
 127 static timeMs_t prearmActivationTime = 0;
 128
 129 static bool isAccRequired(void) {
 130     return isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
 131         isModeActivationConditionPresent(BOXNAVRTH) ||
 132         isModeActivationConditionPresent(BOXNAVWP) ||
 133         isModeActivationConditionPresent(BOXANGLE) ||
 134         isModeActivationConditionPresent(BOXHORIZON) ||
 135         isModeActivationConditionPresent(BOXNAVALTHOLD) ||
 136         isModeActivationConditionPresent(BOXHEADINGHOLD) ||
 137         isModeActivationConditionPresent(BOXNAVLAUNCH) ||
 138         isModeActivationConditionPresent(BOXTURNASSIST) ||
 139         isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
 140         isModeActivationConditionPresent(BOXSOARING) ||
 141         failsafeConfig()->failsafe_procedure != FAILSAFE_PROCEDURE_DROP_IT;
 142 }
 143
 144 bool areSensorsCalibrating(void)
 145 {
 146 #ifdef USE_BARO
 147     if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) {
 148         return true;
 149     }
 150 #endif
 151
 152 #ifdef USE_MAG
 153     if (sensors(SENSOR_MAG) && !compassIsCalibrationComplete()) {
 154         return true;
 155     }
 156 #endif
 157
 158 #ifdef USE_PITOT
 159     if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) {
 160         return true;
 161     }
 162 #endif
 163
 164     if (!navIsCalibrationComplete() && isAccRequired()) {
 165         return true;
 166     }
 167
 168     if (!accIsCalibrationComplete() && sensors(SENSOR_ACC) && isAccRequired()) {
 169         return true;
 170     }
 171
 172     if (!gyroIsCalibrationComplete()) {
 173         return true;
 174     }
 175
 176     return false;
 177 }
 178
 179 int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
 180 {
 181     int16_t stickDeflection = 0;
 182
 183 #if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914
 184     const int16_t value = rawData - PWM_RANGE_MIDDLE;
 185     if (value < -500) {
 186         stickDeflection = -500;
 187     } else if (value > 500) {
 188         stickDeflection = 500;
 189     } else {
 190         stickDeflection = value;
 191     }
 192 #else
 193     stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500);
 194 #endif
 195
 196     stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500);
 197     return rcLookup(stickDeflection, rate);
 198 }
 199
 200 static void updateArmingStatus(void)
 201 {
 202     if (ARMING_FLAG(ARMED)) {
 203         LED0_ON;
 204     } else {
 205         /* CHECK: Run-time calibration */
 206         static bool calibratingFinishedBeep = false;
 207         if (areSensorsCalibrating()) {
 208             ENABLE_ARMING_FLAG(ARMING_DISABLED_SENSORS_CALIBRATING);
 209             calibratingFinishedBeep = false;
 210         }
 211         else {
 212             DISABLE_ARMING_FLAG(ARMING_DISABLED_SENSORS_CALIBRATING);
 213
 214             if (!calibratingFinishedBeep) {
 215                 calibratingFinishedBeep = true;
 216                 beeper(BEEPER_RUNTIME_CALIBRATION_DONE);
 217             }
 218         }
 219
 220         /* CHECK: RX signal */
 221         if (!failsafeIsReceivingRxData()) {
 222             ENABLE_ARMING_FLAG(ARMING_DISABLED_RC_LINK);
 223         }
 224         else {
 225             DISABLE_ARMING_FLAG(ARMING_DISABLED_RC_LINK);
 226         }
 227
 228         /* CHECK: Throttle */
 229         if (!armingConfig()->fixed_wing_auto_arm) {
 230             // Don't want this check if fixed_wing_auto_arm is in use - machine arms on throttle > LOW
 231             if (throttleStickIsLow()) {
 232                 DISABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
 233             } else {
 234                 ENABLE_ARMING_FLAG(ARMING_DISABLED_THROTTLE);
 235             }
 236         }
 237
 238         /* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
 239         if (isNavLaunchEnabled()) {
 240             if (isRollPitchStickDeflected(CONTROL_DEADBAND)) {
 241                 ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
 242             } else {
 243                 DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
 244             }
 245         }
 246
 247         /* CHECK: Angle */
 248         if (!STATE(SMALL_ANGLE)) {
 249             ENABLE_ARMING_FLAG(ARMING_DISABLED_NOT_LEVEL);
 250         }
 251         else {
 252             DISABLE_ARMING_FLAG(ARMING_DISABLED_NOT_LEVEL);
 253         }
 254
 255         /* CHECK: CPU load */
 256         if (isSystemOverloaded()) {
 257             ENABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED);
 258         }
 259         else {
 260             DISABLE_ARMING_FLAG(ARMING_DISABLED_SYSTEM_OVERLOADED);
 261         }
 262
 263         /* CHECK: Navigation safety */
 264         if (navigationIsBlockingArming(NULL) != NAV_ARMING_BLOCKER_NONE) {
 265             ENABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE);
 266         }
 267         else {
 268             DISABLE_ARMING_FLAG(ARMING_DISABLED_NAVIGATION_UNSAFE);
 269         }
 270
 271 #if defined(USE_MAG)
 272         /* CHECK: */
 273         if (sensors(SENSOR_MAG) && !STATE(COMPASS_CALIBRATED)) {
 274             ENABLE_ARMING_FLAG(ARMING_DISABLED_COMPASS_NOT_CALIBRATED);
 275         }
 276         else {
 277             DISABLE_ARMING_FLAG(ARMING_DISABLED_COMPASS_NOT_CALIBRATED);
 278         }
 279 #endif
 280
 281         /* CHECK: */
 282         if (
 283             sensors(SENSOR_ACC) &&
 284             !STATE(ACCELEROMETER_CALIBRATED) &&
 285             // Require ACC calibration only if any of the setting might require it
 286             isAccRequired()
 287         ) {
 288             ENABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED);
 289         }
 290         else {
 291             DISABLE_ARMING_FLAG(ARMING_DISABLED_ACCELEROMETER_NOT_CALIBRATED);
 292         }
 293
 294         /* CHECK: */
 295         if (!isHardwareHealthy()) {
 296             ENABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
 297         }
 298         else {
 299             DISABLE_ARMING_FLAG(ARMING_DISABLED_HARDWARE_FAILURE);
 300         }
 301
 302         /* CHECK: BOXFAILSAFE */
 303         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 304             ENABLE_ARMING_FLAG(ARMING_DISABLED_BOXFAILSAFE);
 305         }
 306         else {
 307             DISABLE_ARMING_FLAG(ARMING_DISABLED_BOXFAILSAFE);
 308         }
 309
 310         /* CHECK: Do not allow arming if Servo AutoTrim is enabled */
 311         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
 312            ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 313         }
 314         else {
 315            DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 316         }
 317
 318 #ifdef USE_DSHOT
 319         /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */
 320         if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) {
 321             ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 322         } else {
 323             DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 324         }
 325 #else
 326         DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 327 #endif
 328
 329         if (isModeActivationConditionPresent(BOXPREARM)) {
 330             if (IS_RC_MODE_ACTIVE(BOXPREARM)) {
 331                 if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) {
 332                     DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 333                 } else {
 334                     ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 335                 }
 336             } else {
 337                 prearmWasReset = true;
 338                 prearmActivationTime = millis();
 339                 ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 340             }
 341         } else {
 342             DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 343         }
 344
 345         if (ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED) && !IS_RC_MODE_ACTIVE(BOXARM)) {
 346             DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
 347         }
 348
 349         /* CHECK: Arming switch */
 350         // If arming is disabled and the ARM switch is on
 351         // Note that this should be last check so all other blockers could be cleared correctly
 352         // if blocking modes are linked to the same RC channel
 353         if (isArmingDisabled() && IS_RC_MODE_ACTIVE(BOXARM)) {
 354             ENABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH);
 355         } else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 356             DISABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH);
 357         }
 358
 359         if (isArmingDisabled()) {
 360             warningLedFlash();
 361         } else {
 362             warningLedDisable();
 363         }
 364
 365         warningLedUpdate();
 366     }
 367 }
 368
 369 static bool emergencyArmingCanOverrideArmingDisabled(void)
 370 {
 371     uint32_t armingPrevention = armingFlags & ARMING_DISABLED_ALL_FLAGS;
 372     armingPrevention &= ~ARMING_DISABLED_EMERGENCY_OVERRIDE;
 373     return armingPrevention == 0;
 374 }
 375
 376 static bool emergencyArmingIsEnabled(void)
 377 {
 378     return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled();
 379 }
 380
 381 static void processPilotAndFailSafeActions(float dT)
 382 {
 383     if (failsafeShouldApplyControlInput()) {
 384         // Failsafe will apply rcCommand for us
 385         failsafeApplyControlInput();
 386     }
 387     else {
 388         // Compute ROLL PITCH and YAW command
 389         rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
 390         rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
 391         rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
 392
 393         // Apply manual control rates
 394         if (FLIGHT_MODE(MANUAL_MODE)) {
 395             rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L;
 396             rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L;
 397             rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L;
 398         } else {
 399             DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]);
 400             rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT);
 401             DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]);
 402
 403             DEBUG_SET(DEBUG_RATE_DYNAMICS, 2, rcCommand[PITCH]);
 404             rcCommand[PITCH] = applyRateDynamics(rcCommand[PITCH], PITCH, dT);
 405             DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]);
 406
 407             DEBUG_SET(DEBUG_RATE_DYNAMICS, 4, rcCommand[YAW]);
 408             rcCommand[YAW] = applyRateDynamics(rcCommand[YAW], YAW, dT);
 409             DEBUG_SET(DEBUG_RATE_DYNAMICS, 5, rcCommand[YAW]);
 410
 411         }
 412
 413         //Compute THROTTLE command
 414         rcCommand[THROTTLE] = throttleStickMixedValue();
 415
 416         // Signal updated rcCommand values to Failsafe system
 417         failsafeUpdateRcCommandValues();
 418
 419         if (FLIGHT_MODE(HEADFREE_MODE)) {
 420             const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
 421             const float cosDiff = cos_approx(radDiff);
 422             const float sinDiff = sin_approx(radDiff);
 423             const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 424             rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 425             rcCommand[PITCH] = rcCommand_PITCH;
 426         }
 427     }
 428 }
 429
 430 void disarm(disarmReason_t disarmReason)
 431 {
 432     if (ARMING_FLAG(ARMED)) {
 433         lastDisarmReason = disarmReason;
 434         lastDisarmTimeUs = micros();
 435         DISABLE_ARMING_FLAG(ARMED);
 436         DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 437
 438 #ifdef USE_BLACKBOX
 439         if (feature(FEATURE_BLACKBOX)) {
 440             blackboxFinish();
 441         }
 442 #endif
 443 #ifdef USE_DSHOT
 444         if (FLIGHT_MODE(TURTLE_MODE)) {
 445             sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
 446             DISABLE_FLIGHT_MODE(TURTLE_MODE);
 447         }
 448 #endif
 449         statsOnDisarm();
 450         logicConditionReset();
 451
 452 #ifdef USE_PROGRAMMING_FRAMEWORK
 453         programmingPidReset();
 454 #endif
 455         beeper(BEEPER_DISARMING);      // emit disarm tone
 456
 457         prearmWasReset = false;
 458     }
 459 }
 460
 461 timeUs_t getLastDisarmTimeUs(void) {
 462     return lastDisarmTimeUs;
 463 }
 464
 465 disarmReason_t getDisarmReason(void)
 466 {
 467     return lastDisarmReason;
 468 }
 469
 470 bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
 471 {
 472     if (ARMING_FLAG(ARMED)) {
 473         return false;
 474     }
 475
 476     static timeMs_t timeout = 0;
 477     static int8_t counter = 0;
 478     static bool toggle;
 479     timeMs_t currentTimeMs = millis();
 480
 481     if (timeout && currentTimeMs > timeout) {
 482         timeout += EMERGENCY_ARMING_COUNTER_STEP_MS;
 483         counter -= counter ? 1 : 0;
 484         if (!counter) {
 485             timeout = 0;
 486         }
 487     }
 488
 489     if (armingSwitchIsOn) {
 490         if (!timeout && toggle) {
 491             timeout = currentTimeMs + EMERGENCY_ARMING_TIME_WINDOW_MS;
 492         }
 493         counter += toggle;
 494         toggle = false;
 495     } else {
 496         toggle = true;
 497     }
 498
 499     if (forceArm) {
 500         counter = EMERGENCY_ARMING_MIN_ARM_COUNT;
 501     }
 502
 503     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 504 }
 505
 506 bool emergInflightRearmEnabled(void)
 507 {
 508     /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
 509     timeMs_t currentTimeMs = millis();
 510     emergRearmStabiliseTimeout = 0;
 511
 512     if ((lastDisarmReason != DISARM_SWITCH) ||
 513         (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
 514         return false;
 515     }
 516
 517     // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying
 518     bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f;
 519
 520     if (isProbablyStillFlying() || mcDisarmVertVelCheck) {
 521         emergRearmStabiliseTimeout = currentTimeMs + 5000;  // activate Angle mode for 5s after rearm to help stabilise craft
 522         ENABLE_STATE(IN_FLIGHT_EMERG_REARM);
 523         return true;
 524     }
 525
 526     return false;   // craft doesn't appear to be flying, don't allow emergency rearm
 527 }
 528
 529 void tryArm(void)
 530 {
 531     updateArmingStatus();
 532
 533     if (ARMING_FLAG(ARMED)) {
 534         return;
 535     }
 536
 537 #ifdef USE_DSHOT
 538 #ifdef USE_MULTI_FUNCTIONS
 539     const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE);
 540 #else
 541     const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
 542 #endif
 543     if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
 544         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
 545         ENABLE_ARMING_FLAG(ARMED);
 546         ENABLE_FLIGHT_MODE(TURTLE_MODE);
 547         return;
 548     }
 549 #endif
 550
 551 #ifdef USE_PROGRAMMING_FRAMEWORK
 552     if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() ||
 553         LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
 554 #else
 555     if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) {
 556 #endif
 557         // If nav_extra_arming_safety was bypassed we always
 558         // allow bypassing it even without the sticks set
 559         // in the correct position to allow re-arming quickly
 560         // in case of a mid-air accidental disarm.
 561         bool usedBypass = false;
 562         navigationIsBlockingArming(&usedBypass);
 563         if (usedBypass) {
 564             ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED);
 565         }
 566
 567         lastDisarmReason = DISARM_NONE;
 568
 569         ENABLE_ARMING_FLAG(ARMED);
 570         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 571         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
 572         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
 573
 574         if (!STATE(IN_FLIGHT_EMERG_REARM)) {
 575             resetLandingDetectorActiveState();  // reset landing detector after arming to avoid false detection before flight
 576             logicConditionReset();
 577 #ifdef USE_PROGRAMMING_FRAMEWORK
 578             programmingPidReset();
 579 #endif
 580         }
 581
 582         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 583
 584         resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 585
 586 #ifdef USE_BLACKBOX
 587         if (feature(FEATURE_BLACKBOX)) {
 588             serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 589             if (sharedBlackboxAndMspPort) {
 590                 mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
 591             }
 592             blackboxStart();
 593         }
 594 #endif
 595
 596         //beep to indicate arming
 597         if (navigationPositionEstimateIsHealthy()) {
 598             beeper(BEEPER_ARMING_GPS_FIX);
 599         } else {
 600             beeper(BEEPER_ARMING);
 601         }
 602
 603         statsOnArm();
 604
 605         return;
 606     }
 607
 608     if (!ARMING_FLAG(ARMED)) {
 609         beeperConfirmationBeeps(1);
 610     }
 611 }
 612
 613 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
 614
 615 void releaseSharedTelemetryPorts(void) {
 616     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 617     while (sharedPort) {
 618         mspSerialReleasePortIfAllocated(sharedPort);
 619         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 620     }
 621 }
 622
 623 void processRx(timeUs_t currentTimeUs)
 624 {
 625     // Calculate RPY channel data
 626     calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 627
 628     // in 3D mode, we need to be able to disarm by switch at any time
 629     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 630         if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 631             disarm(DISARM_SWITCH_3D);
 632         }
 633     }
 634
 635     updateRSSI(currentTimeUs);
 636
 637     // Update failsafe monitoring system
 638     if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 639         failsafeStartMonitoring();
 640     }
 641
 642     failsafeUpdateState();
 643
 644     const bool throttleIsLow = throttleStickIsLow();
 645
 646     // When armed and motors aren't spinning, do beeps periodically
 647     if (ARMING_FLAG(ARMED) && ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY)) {
 648         static bool armedBeeperOn = false;
 649
 650         if (throttleIsLow) {
 651             beeper(BEEPER_ARMED);
 652             armedBeeperOn = true;
 653         } else if (armedBeeperOn) {
 654             beeperSilence();
 655             armedBeeperOn = false;
 656         }
 657     }
 658
 659     processRcStickPositions(throttleIsLow);
 660     processAirmode();
 661     updateActivatedModes();
 662
 663 #ifdef USE_PINIOBOX
 664     pinioBoxUpdate();
 665 #endif
 666
 667     if (!cliMode) {
 668         bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE);
 669         updateAdjustmentStates(canUseRxData);
 670         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
 671     }
 672
 673     // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
 674     bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
 675     bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
 676
 677     /* Disable stabilised modes initially, will be enabled as required with priority ANGLE > HORIZON > ANGLEHOLD
 678      * MANUAL mode has priority over these modes except when ANGLE auto enabled */
 679     DISABLE_FLIGHT_MODE(ANGLE_MODE);
 680     DISABLE_FLIGHT_MODE(HORIZON_MODE);
 681     DISABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
 682
 683     if (sensors(SENSOR_ACC) && (!FLIGHT_MODE(MANUAL_MODE) || autoEnableAngle)) {
 684         if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
 685             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 686         } else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
 687             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 688         } else if (STATE(AIRPLANE) && IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
 689             ENABLE_FLIGHT_MODE(ANGLEHOLD_MODE);
 690         }
 691     }
 692
 693     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 694         LED1_ON;
 695     } else {
 696         LED1_OFF;
 697     }
 698
 699     /* Flaperon mode */
 700     if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
 701         ENABLE_FLIGHT_MODE(FLAPERON);
 702     } else {
 703         DISABLE_FLIGHT_MODE(FLAPERON);
 704     }
 705
 706     /* Turn assistant mode */
 707     if (IS_RC_MODE_ACTIVE(BOXTURNASSIST) || navigationRequiresTurnAssistance()) {
 708          ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
 709     } else {
 710         DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
 711     }
 712
 713     if (sensors(SENSOR_ACC)) {
 714         if (IS_RC_MODE_ACTIVE(BOXHEADINGHOLD)) {
 715             if (!FLIGHT_MODE(HEADING_MODE)) {
 716                 resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 717                 ENABLE_FLIGHT_MODE(HEADING_MODE);
 718             }
 719         } else {
 720             DISABLE_FLIGHT_MODE(HEADING_MODE);
 721         }
 722     }
 723
 724 #if defined(USE_MAG)
 725     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 726         if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) {
 727             ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 728         } else {
 729             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 730         }
 731         if (IS_RC_MODE_ACTIVE(BOXHEADADJ) && STATE(MULTIROTOR)) {
 732             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 733         }
 734     }
 735 #endif
 736
 737     // Handle passthrough mode
 738     if (STATE(FIXED_WING_LEGACY)) {
 739         if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
 740             (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
 741             ENABLE_FLIGHT_MODE(MANUAL_MODE);
 742         } else {
 743             DISABLE_FLIGHT_MODE(MANUAL_MODE);
 744         }
 745     } else {
 746         DISABLE_FLIGHT_MODE(MANUAL_MODE);
 747     }
 748
 749     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 750        This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
 751        Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
 752
 753     if (!ARMING_FLAG(ARMED)) {
 754         DISABLE_STATE(ANTI_WINDUP_DEACTIVATED);
 755     }
 756
 757     const rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
 758
 759     // In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL)
 760     if (FLIGHT_MODE(MANUAL_MODE) || !ARMING_FLAG(ARMED)) {
 761         DISABLE_STATE(ANTI_WINDUP);
 762         pidResetErrorAccumulators();
 763     }
 764     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
 765         if (throttleIsLow) {
 766             if (STATE(AIRMODE_ACTIVE)) {
 767                 if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
 768                     ENABLE_STATE(ANTI_WINDUP);
 769                 }
 770                 else {
 771                     DISABLE_STATE(ANTI_WINDUP);
 772                 }
 773             }
 774             else {
 775                 DISABLE_STATE(ANTI_WINDUP);
 776                 pidResetErrorAccumulators();
 777             }
 778         }
 779         else {
 780             DISABLE_STATE(ANTI_WINDUP);
 781         }
 782     }
 783     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
 784         if (throttleIsLow) {
 785             if (STATE(AIRMODE_ACTIVE)) {
 786                 if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
 787                     ENABLE_STATE(ANTI_WINDUP);
 788                 }
 789                 else {
 790                     DISABLE_STATE(ANTI_WINDUP);
 791                 }
 792             }
 793             else {
 794                 DISABLE_STATE(ANTI_WINDUP);
 795                 pidResetErrorAccumulators();
 796             }
 797         }
 798         else {
 799             DISABLE_STATE(ANTI_WINDUP);
 800             if (rollPitchStatus != CENTERED) {
 801                 ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
 802             }
 803         }
 804     }
 805     else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
 806         DISABLE_STATE(ANTI_WINDUP);
 807         //This case applies only to MR when Airmode management is throttle threshold activated
 808         if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
 809             pidResetErrorAccumulators();
 810         }
 811     }
 812 //---------------------------------------------------------
 813     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
 814         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 815     }
 816
 817 #if defined(USE_AUTOTUNE_FIXED_WING) || defined(USE_AUTOTUNE_MULTIROTOR)
 818     autotuneUpdateState();
 819 #endif
 820
 821 #ifdef USE_TELEMETRY
 822     if (feature(FEATURE_TELEMETRY)) {
 823         if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
 824                 (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 825
 826             releaseSharedTelemetryPorts();
 827         } else {
 828             // the telemetry state must be checked immediately so that shared serial ports are released.
 829             telemetryCheckState();
 830             mspSerialAllocatePorts();
 831         }
 832     }
 833 #endif
 834     // Sound a beeper if the flight mode state has changed
 835     updateFlightModeChangeBeeper();
 836 }
 837
 838 // Function for loop trigger
 839 void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
 840     UNUSED(currentTimeUs);
 841     // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
 842     // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
 843     const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 844
 845     /* Update actual hardware readings */
 846     gyroUpdate();
 847
 848 #ifdef USE_OPFLOW
 849     if (sensors(SENSOR_OPFLOW)) {
 850         opflowGyroUpdateCallback(currentDeltaTime);
 851     }
 852 #endif
 853 }
 854
 855 static void applyThrottleTiltCompensation(void)
 856 {
 857     if (STATE(MULTIROTOR)) {
 858         int16_t thrTiltCompStrength = 0;
 859
 860         if (navigationRequiresThrottleTiltCompensation()) {
 861             thrTiltCompStrength = 100;
 862         }
 863         else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 864             thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
 865         }
 866
 867         if (thrTiltCompStrength) {
 868             const int throttleIdleValue = getThrottleIdleValue();
 869             float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
 870             tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f);
 871
 872             rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false);
 873         }
 874     }
 875 }
 876
 877 void taskMainPidLoop(timeUs_t currentTimeUs)
 878 {
 879
 880     cycleTime = getTaskDeltaTime(TASK_SELF);
 881     dT = (float)cycleTime * 0.000001f;
 882
 883     bool fwLaunchIsActive = STATE(AIRPLANE) && isNavLaunchEnabled() && armTime == 0;
 884
 885     if (ARMING_FLAG(ARMED) && (!STATE(AIRPLANE) || !fwLaunchIsActive || fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED)) {
 886         flightTime += cycleTime;
 887         armTime += cycleTime;
 888         updateAccExtremes();
 889     }
 890
 891     if (!ARMING_FLAG(ARMED)) {
 892         armTime = 0;
 893
 894         // Delay saving for 0.5s to allow other functions to process save actions on disarm
 895         if (currentTimeUs - lastDisarmTimeUs > USECS_PER_SEC / 2) {
 896             processDelayedSave();
 897         }
 898     }
 899
 900     if (armTime > 1 * USECS_PER_SEC) {     // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
 901         DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 902     }
 903
 904 #if defined(SITL_BUILD)
 905     if (ARMING_FLAG(SIMULATOR_MODE_HITL) || lockMainPID()) {
 906 #endif
 907
 908     gyroFilter();
 909
 910     imuUpdateAccelerometer();
 911     imuUpdateAttitude(currentTimeUs);
 912
 913 #if defined(SITL_BUILD)
 914     }
 915 #endif
 916
 917     processPilotAndFailSafeActions(dT);
 918
 919     updateArmingStatus();
 920
 921     if (rxConfig()->rcFilterFrequency) {
 922         rcInterpolationApply(isRXDataNew, currentTimeUs);
 923     }
 924
 925     if (isRXDataNew) {
 926         updateWaypointsAndNavigationMode();
 927     }
 928     isRXDataNew = false;
 929
 930     updatePositionEstimator();
 931     applyWaypointNavigationAndAltitudeHold();
 932
 933     // Apply throttle tilt compensation
 934     applyThrottleTiltCompensation();
 935
 936 #ifdef USE_POWER_LIMITS
 937     powerLimiterApply(&rcCommand[THROTTLE]);
 938 #endif
 939
 940     // Calculate stabilisation
 941     pidController(dT);
 942
 943     mixTable();
 944
 945     if (isMixerUsingServos()) {
 946         servoMixer(dT);
 947         processServoAutotrim(dT);
 948     }
 949
 950     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
 951
 952 #ifdef USE_SIMULATOR
 953     if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 954         if (isServoOutputEnabled()) {
 955             writeServos();
 956         }
 957
 958         if (motorControlEnable) {
 959             writeMotors();
 960         }
 961     }
 962 #else
 963     if (isServoOutputEnabled()) {
 964         writeServos();
 965     }
 966
 967     if (motorControlEnable) {
 968         writeMotors();
 969     }
 970 #endif
 971     // Check if landed, FW and MR
 972     if (STATE(ALTITUDE_CONTROL)) {
 973         updateLandingStatus(US2MS(currentTimeUs));
 974     }
 975
 976 #ifdef USE_BLACKBOX
 977     if (!cliMode && feature(FEATURE_BLACKBOX)) {
 978         blackboxUpdate(micros());
 979     }
 980 #endif
 981 }
 982
 983 // This function is called in a busy-loop, everything called from here should do it's own
 984 // scheduling and avoid doing heavy calculations
 985 void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 986 {
 987     UNUSED(currentTimeUs);
 988
 989 #ifdef USE_SDCARD
 990     afatfs_poll();
 991 #endif
 992
 993 #ifdef USE_DSHOT
 994     pwmCompleteMotorUpdate();
 995 #endif
 996
 997 #ifdef USE_ESC_SENSOR
 998     escSensorUpdate(currentTimeUs);
 999 #endif
1000 }
1001
1002 bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
1003 {
1004     UNUSED(currentDeltaTime);
1005
1006     return rxUpdateCheck(currentTimeUs, currentDeltaTime);
1007 }
1008
1009 void taskUpdateRxMain(timeUs_t currentTimeUs)
1010 {
1011     processRx(currentTimeUs);
1012     isRXDataNew = true;
1013 }
1014
1015 // returns seconds
1016 float getFlightTime(void)
1017 {
1018     return US2S(flightTime);
1019 }
1020
1021 void resetFlightTime(void) {
1022     flightTime = 0;
1023 }
1024
1025 float getArmTime(void)
1026 {
1027     return US2S(armTime);
1028 }
1029
1030 void fcReboot(bool bootLoader)
1031 {
1032     // stop motor/servo outputs
1033     stopMotors();
1034     stopPwmAllMotors();
1035
1036     // extra delay before reboot to give ESCs chance to reset
1037     delay(1000);
1038
1039     if (bootLoader) {
1040         systemResetToBootloader();
1041     }
1042     else {
1043         systemReset();
1044     }
1045
1046     while (true);
1047 }