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(rcControlsConfig()->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: BOXKILLSWITCH */
 311         if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
 312             ENABLE_ARMING_FLAG(ARMING_DISABLED_BOXKILLSWITCH);
 313         }
 314         else {
 315             DISABLE_ARMING_FLAG(ARMING_DISABLED_BOXKILLSWITCH);
 316         }
 317
 318         /* CHECK: Do not allow arming if Servo AutoTrim is enabled */
 319         if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
 320            ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 321         }
 322         else {
 323            DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
 324         }
 325
 326 #ifdef USE_DSHOT
 327         /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */
 328         if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) {
 329             ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 330         } else {
 331             DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 332         }
 333 #else
 334         DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER);
 335 #endif
 336
 337         if (isModeActivationConditionPresent(BOXPREARM)) {
 338             if (IS_RC_MODE_ACTIVE(BOXPREARM)) {
 339                 if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) {
 340                     DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 341                 } else {
 342                     ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 343                 }
 344             } else {
 345                 prearmWasReset = true;
 346                 prearmActivationTime = millis();
 347                 ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 348             }
 349         } else {
 350             DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
 351         }
 352
 353         /* CHECK: Arming switch */
 354         // If arming is disabled and the ARM switch is on
 355         // Note that this should be last check so all other blockers could be cleared correctly
 356         // if blocking modes are linked to the same RC channel
 357         if (isArmingDisabled() && IS_RC_MODE_ACTIVE(BOXARM)) {
 358             ENABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH);
 359         } else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 360             DISABLE_ARMING_FLAG(ARMING_DISABLED_ARM_SWITCH);
 361         }
 362
 363         if (isArmingDisabled()) {
 364             warningLedFlash();
 365         } else {
 366             warningLedDisable();
 367         }
 368
 369         warningLedUpdate();
 370     }
 371 }
 372
 373 static bool emergencyArmingCanOverrideArmingDisabled(void)
 374 {
 375     uint32_t armingPrevention = armingFlags & ARMING_DISABLED_ALL_FLAGS;
 376     armingPrevention &= ~ARMING_DISABLED_EMERGENCY_OVERRIDE;
 377     return armingPrevention == 0;
 378 }
 379
 380 static bool emergencyArmingIsEnabled(void)
 381 {
 382     return emergencyArmingUpdate(IS_RC_MODE_ACTIVE(BOXARM), false) && emergencyArmingCanOverrideArmingDisabled();
 383 }
 384
 385 static void processPilotAndFailSafeActions(float dT)
 386 {
 387     if (failsafeShouldApplyControlInput()) {
 388         // Failsafe will apply rcCommand for us
 389         failsafeApplyControlInput();
 390     }
 391     else {
 392         // Compute ROLL PITCH and YAW command
 393         rcCommand[ROLL] = getAxisRcCommand(rxGetChannelValue(ROLL), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
 394         rcCommand[PITCH] = getAxisRcCommand(rxGetChannelValue(PITCH), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcExpo8 : currentControlRateProfile->stabilized.rcExpo8, rcControlsConfig()->deadband);
 395         rcCommand[YAW] = -getAxisRcCommand(rxGetChannelValue(YAW), FLIGHT_MODE(MANUAL_MODE) ? currentControlRateProfile->manual.rcYawExpo8 : currentControlRateProfile->stabilized.rcYawExpo8, rcControlsConfig()->yaw_deadband);
 396
 397         // Apply manual control rates
 398         if (FLIGHT_MODE(MANUAL_MODE)) {
 399             rcCommand[ROLL] = rcCommand[ROLL] * currentControlRateProfile->manual.rates[FD_ROLL] / 100L;
 400             rcCommand[PITCH] = rcCommand[PITCH] * currentControlRateProfile->manual.rates[FD_PITCH] / 100L;
 401             rcCommand[YAW] = rcCommand[YAW] * currentControlRateProfile->manual.rates[FD_YAW] / 100L;
 402         } else {
 403             DEBUG_SET(DEBUG_RATE_DYNAMICS, 0, rcCommand[ROLL]);
 404             rcCommand[ROLL] = applyRateDynamics(rcCommand[ROLL], ROLL, dT);
 405             DEBUG_SET(DEBUG_RATE_DYNAMICS, 1, rcCommand[ROLL]);
 406
 407             DEBUG_SET(DEBUG_RATE_DYNAMICS, 2, rcCommand[PITCH]);
 408             rcCommand[PITCH] = applyRateDynamics(rcCommand[PITCH], PITCH, dT);
 409             DEBUG_SET(DEBUG_RATE_DYNAMICS, 3, rcCommand[PITCH]);
 410
 411             DEBUG_SET(DEBUG_RATE_DYNAMICS, 4, rcCommand[YAW]);
 412             rcCommand[YAW] = applyRateDynamics(rcCommand[YAW], YAW, dT);
 413             DEBUG_SET(DEBUG_RATE_DYNAMICS, 5, rcCommand[YAW]);
 414
 415         }
 416
 417         //Compute THROTTLE command
 418         rcCommand[THROTTLE] = throttleStickMixedValue();
 419
 420         // Signal updated rcCommand values to Failsafe system
 421         failsafeUpdateRcCommandValues();
 422
 423         if (FLIGHT_MODE(HEADFREE_MODE)) {
 424             const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
 425             const float cosDiff = cos_approx(radDiff);
 426             const float sinDiff = sin_approx(radDiff);
 427             const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 428             rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 429             rcCommand[PITCH] = rcCommand_PITCH;
 430         }
 431     }
 432 }
 433
 434 void disarm(disarmReason_t disarmReason)
 435 {
 436     if (ARMING_FLAG(ARMED)) {
 437         lastDisarmReason = disarmReason;
 438         lastDisarmTimeUs = micros();
 439         DISABLE_ARMING_FLAG(ARMED);
 440         DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 441
 442 #ifdef USE_BLACKBOX
 443         if (feature(FEATURE_BLACKBOX)) {
 444             blackboxFinish();
 445         }
 446 #endif
 447 #ifdef USE_DSHOT
 448         if (FLIGHT_MODE(TURTLE_MODE)) {
 449             sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
 450             DISABLE_FLIGHT_MODE(TURTLE_MODE);
 451         }
 452 #endif
 453         statsOnDisarm();
 454         logicConditionReset();
 455
 456 #ifdef USE_PROGRAMMING_FRAMEWORK
 457         programmingPidReset();
 458 #endif
 459
 460         beeper(BEEPER_DISARMING);      // emit disarm tone
 461
 462         prearmWasReset = false;
 463     }
 464 }
 465
 466 timeUs_t getLastDisarmTimeUs(void) {
 467     return lastDisarmTimeUs;
 468 }
 469
 470 disarmReason_t getDisarmReason(void)
 471 {
 472     return lastDisarmReason;
 473 }
 474
 475 bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
 476 {
 477     if (ARMING_FLAG(ARMED)) {
 478         return false;
 479     }
 480
 481     static timeMs_t timeout = 0;
 482     static int8_t counter = 0;
 483     static bool toggle;
 484     timeMs_t currentTimeMs = millis();
 485
 486     if (timeout && currentTimeMs > timeout) {
 487         timeout += EMERGENCY_ARMING_COUNTER_STEP_MS;
 488         counter -= counter ? 1 : 0;
 489         if (!counter) {
 490             timeout = 0;
 491         }
 492     }
 493
 494     if (armingSwitchIsOn) {
 495         if (!timeout && toggle) {
 496             timeout = currentTimeMs + EMERGENCY_ARMING_TIME_WINDOW_MS;
 497         }
 498         counter += toggle;
 499         toggle = false;
 500     } else {
 501         toggle = true;
 502     }
 503
 504     if (forceArm) {
 505         counter = EMERGENCY_ARMING_MIN_ARM_COUNT;
 506     }
 507
 508     return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT;
 509 }
 510
 511 bool emergInflightRearmEnabled(void)
 512 {
 513     /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
 514     timeMs_t currentTimeMs = millis();
 515     emergRearmStabiliseTimeout = 0;
 516
 517     if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) ||
 518         (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
 519         return false;
 520     }
 521
 522     // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying
 523     bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f;
 524
 525     if (isProbablyStillFlying() || mcDisarmVertVelCheck) {
 526         emergRearmStabiliseTimeout = currentTimeMs + 5000;  // activate Angle mode for 5s after rearm to help stabilise craft
 527         ENABLE_STATE(IN_FLIGHT_EMERG_REARM);
 528         return true;
 529     }
 530
 531     return false;   // craft doesn't appear to be flying, don't allow emergency rearm
 532 }
 533
 534 void tryArm(void)
 535 {
 536     updateArmingStatus();
 537
 538     if (ARMING_FLAG(ARMED)) {
 539         return;
 540     }
 541
 542 #ifdef USE_DSHOT
 543 #ifdef USE_MULTI_FUNCTIONS
 544     const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE) || MULTI_FUNC_FLAG(MF_TURTLE_MODE);
 545 #else
 546     const bool turtleIsActive = IS_RC_MODE_ACTIVE(BOXTURTLE);
 547 #endif
 548     if (STATE(MULTIROTOR) && turtleIsActive && !FLIGHT_MODE(TURTLE_MODE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot()) {
 549         sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
 550         ENABLE_ARMING_FLAG(ARMED);
 551         enableFlightMode(TURTLE_MODE);
 552         return;
 553     }
 554 #endif
 555
 556 #ifdef USE_PROGRAMMING_FRAMEWORK
 557     if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() ||
 558         LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
 559 #else
 560     if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) {
 561 #endif
 562         // If nav_extra_arming_safety was bypassed we always
 563         // allow bypassing it even without the sticks set
 564         // in the correct position to allow re-arming quickly
 565         // in case of a mid-air accidental disarm.
 566         bool usedBypass = false;
 567         navigationIsBlockingArming(&usedBypass);
 568         if (usedBypass) {
 569             ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED);
 570         }
 571
 572         lastDisarmReason = DISARM_NONE;
 573
 574         ENABLE_ARMING_FLAG(ARMED);
 575         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 576         //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction
 577         ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
 578
 579         if (!STATE(IN_FLIGHT_EMERG_REARM)) {
 580             resetLandingDetectorActiveState();  // reset landing detector after arming to avoid false detection before flight
 581             logicConditionReset();
 582 #ifdef USE_PROGRAMMING_FRAMEWORK
 583             programmingPidReset();
 584 #endif
 585         }
 586
 587         headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 588
 589         resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 590
 591 #ifdef USE_BLACKBOX
 592         if (feature(FEATURE_BLACKBOX)) {
 593             serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 594             if (sharedBlackboxAndMspPort) {
 595                 mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort);
 596             }
 597             blackboxStart();
 598         }
 599 #endif
 600
 601         //beep to indicate arming
 602         if (navigationPositionEstimateIsHealthy()) {
 603             beeper(BEEPER_ARMING_GPS_FIX);
 604         } else {
 605             beeper(BEEPER_ARMING);
 606         }
 607
 608         statsOnArm();
 609
 610         return;
 611     }
 612
 613     if (!ARMING_FLAG(ARMED)) {
 614         beeperConfirmationBeeps(1);
 615     }
 616 }
 617
 618 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS)
 619
 620 void releaseSharedTelemetryPorts(void) {
 621     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 622     while (sharedPort) {
 623         mspSerialReleasePortIfAllocated(sharedPort);
 624         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 625     }
 626 }
 627
 628 void processRx(timeUs_t currentTimeUs)
 629 {
 630     // Calculate RPY channel data
 631     calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 632
 633     // in 3D mode, we need to be able to disarm by switch at any time
 634     if (feature(FEATURE_REVERSIBLE_MOTORS)) {
 635         if (!IS_RC_MODE_ACTIVE(BOXARM)) {
 636             disarm(DISARM_SWITCH_3D);
 637         }
 638     }
 639
 640     updateRSSI(currentTimeUs);
 641
 642     // Update failsafe monitoring system
 643     if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 644         failsafeStartMonitoring();
 645     }
 646
 647     failsafeUpdateState();
 648
 649     const bool throttleIsLow = throttleStickIsLow();
 650
 651     // When armed and motors aren't spinning, do beeps periodically
 652     if (ARMING_FLAG(ARMED) && ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY)) {
 653         static bool armedBeeperOn = false;
 654
 655         if (throttleIsLow) {
 656             beeper(BEEPER_ARMED);
 657             armedBeeperOn = true;
 658         } else if (armedBeeperOn) {
 659             beeperSilence();
 660             armedBeeperOn = false;
 661         }
 662     }
 663
 664     processRcStickPositions(throttleIsLow);
 665     processAirmode();
 666     updateActivatedModes();
 667
 668 #ifdef USE_PINIOBOX
 669     pinioBoxUpdate();
 670 #endif
 671
 672     if (!cliMode) {
 673         bool canUseRxData = rxIsReceivingSignal() && !FLIGHT_MODE(FAILSAFE_MODE);
 674         updateAdjustmentStates(canUseRxData);
 675         processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
 676     }
 677
 678     // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR)
 679     bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs);
 680     bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce;
 681     bool canUseHorizonMode = true;
 682
 683     if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) {
 684         // bumpless transfer to Level mode
 685         canUseHorizonMode = false;
 686
 687         if (!FLIGHT_MODE(ANGLE_MODE)) {
 688             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 689         }
 690     } else {
 691         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 692     }
 693
 694     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 695
 696         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 697
 698         if (!FLIGHT_MODE(HORIZON_MODE)) {
 699             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 700         }
 701     } else {
 702         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 703     }
 704
 705     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 706         LED1_ON;
 707     } else {
 708         LED1_OFF;
 709     }
 710
 711     /* Flaperon mode */
 712     if (IS_RC_MODE_ACTIVE(BOXFLAPERON) && STATE(FLAPERON_AVAILABLE)) {
 713         if (!FLIGHT_MODE(FLAPERON)) {
 714             ENABLE_FLIGHT_MODE(FLAPERON);
 715         }
 716     } else {
 717         DISABLE_FLIGHT_MODE(FLAPERON);
 718     }
 719
 720     /* Turn assistant mode */
 721     if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) {
 722         if (!FLIGHT_MODE(TURN_ASSISTANT)) {
 723             ENABLE_FLIGHT_MODE(TURN_ASSISTANT);
 724         }
 725     } else {
 726         DISABLE_FLIGHT_MODE(TURN_ASSISTANT);
 727     }
 728
 729     if (sensors(SENSOR_ACC)) {
 730         if (IS_RC_MODE_ACTIVE(BOXHEADINGHOLD)) {
 731             if (!FLIGHT_MODE(HEADING_MODE)) {
 732                 resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 733                 ENABLE_FLIGHT_MODE(HEADING_MODE);
 734             }
 735         } else {
 736             DISABLE_FLIGHT_MODE(HEADING_MODE);
 737         }
 738     }
 739
 740 #if defined(USE_MAG)
 741     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 742         if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) {
 743             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 744                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 745             }
 746         } else {
 747             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 748         }
 749         if (IS_RC_MODE_ACTIVE(BOXHEADADJ) && STATE(MULTIROTOR)) {
 750             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 751         }
 752     }
 753 #endif
 754
 755     // Handle passthrough mode
 756     if (STATE(FIXED_WING_LEGACY)) {
 757         if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) ||    // Normal activation of passthrough
 758             (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){                                                              // Backup - if we are not armed - enforce passthrough while calibrating
 759             ENABLE_FLIGHT_MODE(MANUAL_MODE);
 760         } else {
 761             DISABLE_FLIGHT_MODE(MANUAL_MODE);
 762         }
 763     }else{
 764         DISABLE_FLIGHT_MODE(MANUAL_MODE);
 765     }
 766
 767     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 768        This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
 769        Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
 770
 771     if (!ARMING_FLAG(ARMED)) {
 772         DISABLE_STATE(ANTI_WINDUP_DEACTIVATED);
 773     }
 774
 775     const rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
 776
 777     // In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL)
 778     if (FLIGHT_MODE(MANUAL_MODE) || !ARMING_FLAG(ARMED)) {
 779         DISABLE_STATE(ANTI_WINDUP);
 780         pidResetErrorAccumulators();
 781     }
 782     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
 783         if (throttleIsLow) {
 784              if (STATE(AIRMODE_ACTIVE)) {
 785                  if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) {
 786                      ENABLE_STATE(ANTI_WINDUP);
 787                  }
 788                  else {
 789                      DISABLE_STATE(ANTI_WINDUP);
 790                  }
 791              }
 792              else {
 793                  DISABLE_STATE(ANTI_WINDUP);
 794                  pidResetErrorAccumulators();
 795              }
 796          }
 797          else {
 798              DISABLE_STATE(ANTI_WINDUP);
 799          }
 800     }
 801     else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) {
 802         if (throttleIsLow) {
 803              if (STATE(AIRMODE_ACTIVE)) {
 804                  if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) {
 805                      ENABLE_STATE(ANTI_WINDUP);
 806                  }
 807                  else {
 808                      DISABLE_STATE(ANTI_WINDUP);
 809                  }
 810              }
 811              else {
 812                  DISABLE_STATE(ANTI_WINDUP);
 813                  pidResetErrorAccumulators();
 814              }
 815          }
 816          else {
 817              DISABLE_STATE(ANTI_WINDUP);
 818              if (rollPitchStatus != CENTERED) {
 819                  ENABLE_STATE(ANTI_WINDUP_DEACTIVATED);
 820              }
 821          }
 822     }
 823     else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {
 824          DISABLE_STATE(ANTI_WINDUP);
 825          //This case applies only to MR when Airmode management is throttle threshold activated
 826          if (throttleIsLow && !STATE(AIRMODE_ACTIVE)) {
 827              pidResetErrorAccumulators();
 828          }
 829      }
 830 //---------------------------------------------------------
 831     if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
 832         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 833     }
 834
 835 #if defined(USE_AUTOTUNE_FIXED_WING) || defined(USE_AUTOTUNE_MULTIROTOR)
 836     autotuneUpdateState();
 837 #endif
 838
 839 #ifdef USE_TELEMETRY
 840     if (feature(FEATURE_TELEMETRY)) {
 841         if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
 842                 (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 843
 844             releaseSharedTelemetryPorts();
 845         } else {
 846             // the telemetry state must be checked immediately so that shared serial ports are released.
 847             telemetryCheckState();
 848             mspSerialAllocatePorts();
 849         }
 850     }
 851 #endif
 852 }
 853
 854 // Function for loop trigger
 855 void FAST_CODE taskGyro(timeUs_t currentTimeUs) {
 856     UNUSED(currentTimeUs);
 857     // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point.
 858     // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
 859     const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 860
 861     /* Update actual hardware readings */
 862     gyroUpdate();
 863
 864 #ifdef USE_OPFLOW
 865     if (sensors(SENSOR_OPFLOW)) {
 866         opflowGyroUpdateCallback(currentDeltaTime);
 867     }
 868 #endif
 869 }
 870
 871 static void applyThrottleTiltCompensation(void)
 872 {
 873     if (STATE(MULTIROTOR)) {
 874         int16_t thrTiltCompStrength = 0;
 875
 876         if (navigationRequiresThrottleTiltCompensation()) {
 877             thrTiltCompStrength = 100;
 878         }
 879         else if (systemConfig()->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 880             thrTiltCompStrength = systemConfig()->throttle_tilt_compensation_strength;
 881         }
 882
 883         if (thrTiltCompStrength) {
 884             const int throttleIdleValue = getThrottleIdleValue();
 885             float tiltCompFactor = 1.0f / constrainf(calculateCosTiltAngle(), 0.6f, 1.0f);  // max tilt about 50 deg
 886             tiltCompFactor = 1.0f + (tiltCompFactor - 1.0f) * (thrTiltCompStrength / 100.f);
 887
 888             rcCommand[THROTTLE] = setDesiredThrottle(throttleIdleValue + (rcCommand[THROTTLE] - throttleIdleValue) * tiltCompFactor, false);
 889         }
 890     }
 891 }
 892
 893 void taskMainPidLoop(timeUs_t currentTimeUs)
 894 {
 895
 896     cycleTime = getTaskDeltaTime(TASK_SELF);
 897     dT = (float)cycleTime * 0.000001f;
 898
 899     if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && fixedWingLaunchStatus() >= FW_LAUNCH_DETECTED))) {
 900         flightTime += cycleTime;
 901         armTime += cycleTime;
 902         updateAccExtremes();
 903     }
 904
 905     if (!ARMING_FLAG(ARMED)) {
 906         armTime = 0;
 907
 908         processDelayedSave();
 909     }
 910
 911     if (armTime > 1 * USECS_PER_SEC) {     // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose
 912         DISABLE_STATE(IN_FLIGHT_EMERG_REARM);
 913     }
 914
 915 #if defined(SITL_BUILD)
 916     if (lockMainPID()) {
 917 #endif
 918
 919     gyroFilter();
 920
 921     imuUpdateAccelerometer();
 922     imuUpdateAttitude(currentTimeUs);
 923
 924 #if defined(SITL_BUILD)
 925     }
 926 #endif
 927
 928     processPilotAndFailSafeActions(dT);
 929
 930     updateArmingStatus();
 931
 932     if (rxConfig()->rcFilterFrequency) {
 933         rcInterpolationApply(isRXDataNew, currentTimeUs);
 934     }
 935
 936     if (isRXDataNew) {
 937         updateWaypointsAndNavigationMode();
 938     }
 939     isRXDataNew = false;
 940
 941     updatePositionEstimator();
 942     applyWaypointNavigationAndAltitudeHold();
 943
 944     // Apply throttle tilt compensation
 945     applyThrottleTiltCompensation();
 946
 947 #ifdef USE_POWER_LIMITS
 948     powerLimiterApply(&rcCommand[THROTTLE]);
 949 #endif
 950
 951     // Calculate stabilisation
 952     pidController(dT);
 953
 954     mixTable();
 955
 956     if (isMixerUsingServos()) {
 957         servoMixer(dT);
 958         processServoAutotrim(dT);
 959     }
 960
 961     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
 962
 963 #ifdef USE_SIMULATOR
 964     if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
 965         if (isServoOutputEnabled()) {
 966             writeServos();
 967         }
 968
 969         if (motorControlEnable) {
 970             writeMotors();
 971         }
 972     }
 973 #else
 974     if (isServoOutputEnabled()) {
 975         writeServos();
 976     }
 977
 978     if (motorControlEnable) {
 979         writeMotors();
 980     }
 981 #endif
 982     // Check if landed, FW and MR
 983     if (STATE(ALTITUDE_CONTROL)) {
 984         updateLandingStatus(US2MS(currentTimeUs));
 985     }
 986
 987 #ifdef USE_BLACKBOX
 988     if (!cliMode && feature(FEATURE_BLACKBOX)) {
 989         blackboxUpdate(micros());
 990     }
 991 #endif
 992 }
 993
 994 // This function is called in a busy-loop, everything called from here should do it's own
 995 // scheduling and avoid doing heavy calculations
 996 void taskRunRealtimeCallbacks(timeUs_t currentTimeUs)
 997 {
 998     UNUSED(currentTimeUs);
 999
1000 #ifdef USE_SDCARD
1001     afatfs_poll();
1002 #endif
1003
1004 #ifdef USE_DSHOT
1005     pwmCompleteMotorUpdate();
1006 #endif
1007
1008 #ifdef USE_ESC_SENSOR
1009     escSensorUpdate(currentTimeUs);
1010 #endif
1011 }
1012
1013 bool taskUpdateRxCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
1014 {
1015     UNUSED(currentDeltaTime);
1016
1017     return rxUpdateCheck(currentTimeUs, currentDeltaTime);
1018 }
1019
1020 void taskUpdateRxMain(timeUs_t currentTimeUs)
1021 {
1022     processRx(currentTimeUs);
1023     isRXDataNew = true;
1024 }
1025
1026 // returns seconds
1027 float getFlightTime(void)
1028 {
1029     return US2S(flightTime);
1030 }
1031
1032 float getArmTime(void)
1033 {
1034     return US2S(armTime);
1035 }
1036
1037 void fcReboot(bool bootLoader)
1038 {
1039     // stop motor/servo outputs
1040     stopMotors();
1041     stopPwmAllMotors();
1042
1043     // extra delay before reboot to give ESCs chance to reset
1044     delay(1000);
1045
1046     if (bootLoader) {
1047         systemResetToBootloader();
1048     }
1049     else {
1050         systemReset();
1051     }
1052
1053     while (true);
1054 }