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