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 <stdint.h>
  20
  21 #include "platform.h"
  22
  23 #include "build/debug.h"
  24
  25 #include "blackbox/blackbox.h"
  26
  27 #include "common/axis.h"
  28 #include "common/filter.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "config/feature.h"
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35
  36 #include "drivers/light_led.h"
  37 #include "drivers/system.h"
  38 #include "drivers/gyro_sync.h"
  39 #include "drivers/transponder_ir.h"
  40
  41 #include "sensors/acceleration.h"
  42 #include "sensors/barometer.h"
  43 #include "sensors/battery.h"
  44 #include "sensors/boardalignment.h"
  45 #include "sensors/gyro.h"
  46 #include "sensors/sensors.h"
  47
  48 #include "fc/cli.h"
  49 #include "fc/config.h"
  50 #include "fc/controlrate_profile.h"
  51 #include "fc/fc_core.h"
  52 #include "fc/fc_rc.h"
  53 #include "fc/rc_adjustments.h"
  54 #include "fc/rc_controls.h"
  55 #include "fc/runtime_config.h"
  56
  57 #include "msp/msp_serial.h"
  58
  59 #include "io/asyncfatfs/asyncfatfs.h"
  60 #include "io/beeper.h"
  61 #include "io/gps.h"
  62 #include "io/motors.h"
  63 #include "io/servos.h"
  64 #include "io/serial.h"
  65 #include "io/statusindicator.h"
  66 #include "io/transponder_ir.h"
  67 #include "io/vtx_control.h"
  68 #include "rx/rx.h"
  69
  70 #include "scheduler/scheduler.h"
  71
  72 #include "telemetry/telemetry.h"
  73
  74 #include "flight/altitude.h"
  75 #include "flight/failsafe.h"
  76 #include "flight/imu.h"
  77 #include "flight/mixer.h"
  78 #include "flight/navigation.h"
  79 #include "flight/pid.h"
  80 #include "flight/servos.h"
  81
  82
  83 // June 2013     V2.2-dev
  84
  85 #ifdef VTX_RTC6705
  86 bool canUpdateVTX(void);
  87 #define VTX_IF_READY if (canUpdateVTX())
  88 #else
  89 #define VTX_IF_READY
  90 #endif
  91
  92 enum {
  93     ALIGN_GYRO = 0,
  94     ALIGN_ACCEL = 1,
  95     ALIGN_MAG = 2
  96 };
  97
  98
  99 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
 100
 101 #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
 102
 103 #if defined(GPS) || defined(MAG)
 104 int16_t magHold;
 105 #endif
 106
 107 int16_t headFreeModeHold;
 108
 109 uint8_t motorControlEnable = false;
 110
 111 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 112
 113 bool isRXDataNew;
 114 static bool armingCalibrationWasInitialised;
 115
 116 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 117
 118 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
 119     .throttle_correction_value = 0,      // could 10 with althold or 40 for fpv
 120     .throttle_correction_angle = 800     // could be 80.0 deg with atlhold or 45.0 for fpv
 121 );
 122
 123 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 124 {
 125     accelerometerConfigMutable()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 126     accelerometerConfigMutable()->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 127
 128     saveConfigAndNotify();
 129 }
 130
 131 bool isCalibrating()
 132 {
 133 #ifdef BARO
 134     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 135         return true;
 136     }
 137 #endif
 138
 139     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 140
 141     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 142 }
 143
 144 void updateLEDs(void)
 145 {
 146     if (ARMING_FLAG(ARMED)) {
 147         LED0_ON;
 148     } else {
 149         if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
 150             ENABLE_ARMING_FLAG(OK_TO_ARM);
 151         }
 152
 153         if (!STATE(SMALL_ANGLE)) {
 154             DISABLE_ARMING_FLAG(OK_TO_ARM);
 155         }
 156
 157         if (isCalibrating() || (averageSystemLoadPercent > 100)) {
 158             warningLedFlash();
 159             DISABLE_ARMING_FLAG(OK_TO_ARM);
 160         } else {
 161             if (ARMING_FLAG(OK_TO_ARM)) {
 162                 warningLedDisable();
 163             } else {
 164                 warningLedFlash();
 165             }
 166         }
 167
 168         warningLedUpdate();
 169     }
 170 }
 171
 172 void mwDisarm(void)
 173 {
 174     armingCalibrationWasInitialised = false;
 175
 176     if (ARMING_FLAG(ARMED)) {
 177         DISABLE_ARMING_FLAG(ARMED);
 178
 179 #ifdef BLACKBOX
 180         if (blackboxConfig()->device) {
 181             finishBlackbox();
 182         }
 183 #endif
 184
 185         beeper(BEEPER_DISARMING);      // emit disarm tone
 186     }
 187 }
 188
 189 void mwArm(void)
 190 {
 191     static bool firstArmingCalibrationWasCompleted;
 192
 193     if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
 194         gyroSetCalibrationCycles();
 195         armingCalibrationWasInitialised = true;
 196         firstArmingCalibrationWasCompleted = true;
 197     }
 198
 199     if (!isGyroCalibrationComplete()) return;  // prevent arming before gyro is calibrated
 200
 201     if (ARMING_FLAG(OK_TO_ARM)) {
 202         if (ARMING_FLAG(ARMED)) {
 203             return;
 204         }
 205         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 206             return;
 207         }
 208         if (!ARMING_FLAG(PREVENT_ARMING)) {
 209             ENABLE_ARMING_FLAG(ARMED);
 210             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 211             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 212
 213             disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 214
 215             //beep to indicate arming
 216 #ifdef GPS
 217             if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
 218                 beeper(BEEPER_ARMING_GPS_FIX);
 219             else
 220                 beeper(BEEPER_ARMING);
 221 #else
 222             beeper(BEEPER_ARMING);
 223 #endif
 224
 225             return;
 226         }
 227     }
 228
 229     if (!ARMING_FLAG(ARMED)) {
 230         beeperConfirmationBeeps(1);
 231     }
 232 }
 233
 234 // Automatic ACC Offset Calibration
 235 bool AccInflightCalibrationArmed = false;
 236 bool AccInflightCalibrationMeasurementDone = false;
 237 bool AccInflightCalibrationSavetoEEProm = false;
 238 bool AccInflightCalibrationActive = false;
 239 uint16_t InflightcalibratingA = 0;
 240
 241 void handleInflightCalibrationStickPosition(void)
 242 {
 243     if (AccInflightCalibrationMeasurementDone) {
 244         // trigger saving into eeprom after landing
 245         AccInflightCalibrationMeasurementDone = false;
 246         AccInflightCalibrationSavetoEEProm = true;
 247     } else {
 248         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 249         if (AccInflightCalibrationArmed) {
 250             beeper(BEEPER_ACC_CALIBRATION);
 251         } else {
 252             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 253         }
 254     }
 255 }
 256
 257 static void updateInflightCalibrationState(void)
 258 {
 259     if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > rxConfig()->mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) {   // Copter is airborne and you are turning it off via boxarm : start measurement
 260         InflightcalibratingA = 50;
 261         AccInflightCalibrationArmed = false;
 262     }
 263     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
 264         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 265             InflightcalibratingA = 50;
 266         AccInflightCalibrationActive = true;
 267     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 268         AccInflightCalibrationMeasurementDone = false;
 269         AccInflightCalibrationSavetoEEProm = true;
 270     }
 271 }
 272
 273 #if defined(GPS) || defined(MAG)
 274 void updateMagHold(void)
 275 {
 276     if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 277         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 278         if (dif <= -180)
 279             dif += 360;
 280         if (dif >= +180)
 281             dif -= 360;
 282         dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 283         if (STATE(SMALL_ANGLE))
 284             rcCommand[YAW] -= dif * currentPidProfile->P8[PIDMAG] / 30;    // 18 deg
 285     } else
 286         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 287 }
 288 #endif
 289
 290
 291 void processRx(timeUs_t currentTimeUs)
 292 {
 293     static bool armedBeeperOn = false;
 294     static bool airmodeIsActivated;
 295
 296     calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 297
 298     // in 3D mode, we need to be able to disarm by switch at any time
 299     if (feature(FEATURE_3D)) {
 300         if (!IS_RC_MODE_ACTIVE(BOXARM))
 301             mwDisarm();
 302     }
 303
 304     updateRSSI(currentTimeUs);
 305
 306     if (feature(FEATURE_FAILSAFE)) {
 307
 308         if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 309             failsafeStartMonitoring();
 310         }
 311
 312         failsafeUpdateState();
 313     }
 314
 315     const throttleStatus_e throttleStatus = calculateThrottleStatus();
 316
 317     if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
 318         if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
 319     } else {
 320         airmodeIsActivated = false;
 321     }
 322
 323     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 324      This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
 325     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
 326         pidResetErrorGyroState();
 327         if (currentPidProfile->pidAtMinThrottle)
 328             pidStabilisationState(PID_STABILISATION_ON);
 329         else
 330             pidStabilisationState(PID_STABILISATION_OFF);
 331     } else {
 332         pidStabilisationState(PID_STABILISATION_ON);
 333     }
 334
 335     // When armed and motors aren't spinning, do beeps and then disarm
 336     // board after delay so users without buzzer won't lose fingers.
 337     // mixTable constrains motor commands, so checking  throttleStatus is enough
 338     if (ARMING_FLAG(ARMED)
 339         && feature(FEATURE_MOTOR_STOP)
 340         && !STATE(FIXED_WING)
 341         && !feature(FEATURE_3D)
 342         && !isAirmodeActive()
 343     ) {
 344         if (isUsingSticksForArming()) {
 345             if (throttleStatus == THROTTLE_LOW) {
 346                 if (armingConfig()->auto_disarm_delay != 0
 347                     && (int32_t)(disarmAt - millis()) < 0
 348                 ) {
 349                     // auto-disarm configured and delay is over
 350                     mwDisarm();
 351                     armedBeeperOn = false;
 352                 } else {
 353                     // still armed; do warning beeps while armed
 354                     beeper(BEEPER_ARMED);
 355                     armedBeeperOn = true;
 356                 }
 357             } else {
 358                 // throttle is not low
 359                 if (armingConfig()->auto_disarm_delay != 0) {
 360                     // extend disarm time
 361                     disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
 362                 }
 363
 364                 if (armedBeeperOn) {
 365                     beeperSilence();
 366                     armedBeeperOn = false;
 367                 }
 368             }
 369         } else {
 370             // arming is via AUX switch; beep while throttle low
 371             if (throttleStatus == THROTTLE_LOW) {
 372                 beeper(BEEPER_ARMED);
 373                 armedBeeperOn = true;
 374             } else if (armedBeeperOn) {
 375                 beeperSilence();
 376                 armedBeeperOn = false;
 377             }
 378         }
 379     }
 380
 381     processRcStickPositions(throttleStatus);
 382
 383     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 384         updateInflightCalibrationState();
 385     }
 386
 387     updateActivatedModes();
 388
 389     if (!cliMode) {
 390         updateAdjustmentStates();
 391         processRcAdjustments(currentControlRateProfile);
 392     }
 393
 394     bool canUseHorizonMode = true;
 395
 396     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
 397         // bumpless transfer to Level mode
 398         canUseHorizonMode = false;
 399
 400         if (!FLIGHT_MODE(ANGLE_MODE)) {
 401             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 402         }
 403     } else {
 404         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 405     }
 406
 407     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 408
 409         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 410
 411         if (!FLIGHT_MODE(HORIZON_MODE)) {
 412             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 413         }
 414     } else {
 415         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 416     }
 417
 418     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 419         LED1_ON;
 420     } else {
 421         LED1_OFF;
 422     }
 423
 424 #if defined(ACC) || defined(MAG)
 425     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 426 #if defined(GPS) || defined(MAG)
 427         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 428             if (!FLIGHT_MODE(MAG_MODE)) {
 429                 ENABLE_FLIGHT_MODE(MAG_MODE);
 430                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 431             }
 432         } else {
 433             DISABLE_FLIGHT_MODE(MAG_MODE);
 434         }
 435 #endif
 436         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 437             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 438                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 439             }
 440         } else {
 441             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 442         }
 443         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 444             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 445         }
 446     }
 447 #endif
 448
 449 #ifdef GPS
 450     if (sensors(SENSOR_GPS)) {
 451         updateGpsWaypointsAndMode();
 452     }
 453 #endif
 454
 455     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 456         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 457     } else {
 458         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 459     }
 460
 461     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
 462         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 463     }
 464
 465 #ifdef TELEMETRY
 466     if (feature(FEATURE_TELEMETRY)) {
 467         if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
 468                 (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 469
 470             releaseSharedTelemetryPorts();
 471         } else {
 472             // the telemetry state must be checked immediately so that shared serial ports are released.
 473             telemetryCheckState();
 474             mspSerialAllocatePorts();
 475         }
 476     }
 477 #endif
 478
 479 #ifdef VTX_CONTROL
 480     vtxUpdateActivatedChannel();
 481
 482     VTX_IF_READY {
 483         handleVTXControlButton();
 484     }
 485 #endif
 486 }
 487
 488 static void subTaskPidController(void)
 489 {
 490     uint32_t startTime;
 491     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 492     // PID - note this is function pointer set by setPIDController()
 493     pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims);
 494     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
 495 }
 496
 497 static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 498 {
 499     uint32_t startTime;
 500     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 501
 502     // Read out gyro temperature if used for telemmetry
 503     if (feature(FEATURE_TELEMETRY)) {
 504         gyroReadTemperature();
 505     }
 506
 507 #ifdef MAG
 508     if (sensors(SENSOR_MAG)) {
 509         updateMagHold();
 510     }
 511 #endif
 512
 513 #if defined(BARO) || defined(SONAR)
 514     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
 515     updateRcCommands();
 516     if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 517         if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 518             applyAltHold();
 519         }
 520     }
 521 #endif
 522
 523     // If we're armed, at minimum throttle, and we do arming via the
 524     // sticks, do not process yaw input from the rx.  We do this so the
 525     // motors do not spin up while we are trying to arm or disarm.
 526     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 527     if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
 528 #ifndef USE_QUAD_MIXER_ONLY
 529 #ifdef USE_SERVOS
 530                 && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
 531 #endif
 532                 && mixerConfig()->mixerMode != MIXER_AIRPLANE
 533                 && mixerConfig()->mixerMode != MIXER_FLYING_WING
 534 #endif
 535     ) {
 536         resetYawAxis();
 537     }
 538
 539     if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 540         rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
 541     }
 542
 543     processRcCommand();
 544
 545 #ifdef GPS
 546     if (sensors(SENSOR_GPS)) {
 547         if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 548             updateGpsStateForHomeAndHoldMode();
 549         }
 550     }
 551 #endif
 552
 553 #ifdef USE_SDCARD
 554     afatfs_poll();
 555 #endif
 556
 557 #ifdef BLACKBOX
 558     if (!cliMode && blackboxConfig()->device) {
 559         handleBlackbox(currentTimeUs);
 560     }
 561 #else
 562     UNUSED(currentTimeUs);
 563 #endif
 564
 565 #ifdef TRANSPONDER
 566     transponderUpdate(currentTimeUs);
 567 #endif
 568     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 569 }
 570
 571 static void subTaskMotorUpdate(void)
 572 {
 573     uint32_t startTime;
 574     if (debugMode == DEBUG_CYCLETIME) {
 575         startTime = micros();
 576         static uint32_t previousMotorUpdateTime;
 577         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
 578         debug[2] = currentDeltaTime;
 579         debug[3] = currentDeltaTime - targetPidLooptime;
 580         previousMotorUpdateTime = startTime;
 581     } else if (debugMode == DEBUG_PIDLOOP) {
 582         startTime = micros();
 583     }
 584
 585     mixTable(currentPidProfile);
 586
 587 #ifdef USE_SERVOS
 588     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
 589     if (isMixerUsingServos()) {
 590         writeServos();
 591     }
 592 #endif
 593
 594     if (motorControlEnable) {
 595         writeMotors();
 596     }
 597     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 598 }
 599
 600 uint8_t setPidUpdateCountDown(void)
 601 {
 602     if (gyroConfig()->gyro_soft_lpf_hz) {
 603         return pidConfig()->pid_process_denom - 1;
 604     } else {
 605         return 1;
 606     }
 607 }
 608
 609 // Function for loop trigger
 610 void taskMainPidLoop(timeUs_t currentTimeUs)
 611 {
 612     static bool runTaskMainSubprocesses;
 613     static uint8_t pidUpdateCountdown;
 614
 615 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
 616         if(lockMainPID() != 0) return;
 617 #endif
 618
 619     if (debugMode == DEBUG_CYCLETIME) {
 620         debug[0] = getTaskDeltaTime(TASK_SELF);
 621         debug[1] = averageSystemLoadPercent;
 622     }
 623
 624     if (runTaskMainSubprocesses) {
 625         subTaskMainSubprocesses(currentTimeUs);
 626         runTaskMainSubprocesses = false;
 627     }
 628
 629     // DEBUG_PIDLOOP, timings for:
 630     // 0 - gyroUpdate()
 631     // 1 - pidController()
 632     // 2 - subTaskMainSubprocesses()
 633     // 3 - subTaskMotorUpdate()
 634     uint32_t startTime;
 635     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 636     gyroUpdate();
 637     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
 638
 639     if (pidUpdateCountdown) {
 640         pidUpdateCountdown--;
 641     } else {
 642         pidUpdateCountdown = setPidUpdateCountDown();
 643         subTaskPidController();
 644         subTaskMotorUpdate();
 645         runTaskMainSubprocesses = true;
 646     }
 647 }