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