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/gyro_sync.h"
  37 #include "drivers/light_led.h"
  38 #include "drivers/time.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 static bool reverseMotors;
 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             blackboxFinish();
 182         }
 183 #endif
 184         BEEP_OFF;
 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         gyroStartCalibration();
 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             #ifdef USE_DSHOT
 210             if (!feature(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
 211                 reverseMotors = false;
 212                 for (unsigned index = 0; index < getMotorCount(); index++) {
 213                     pwmWriteDshotCommand(index, DSHOT_CMD_ROTATE_NORMAL);
 214                 }
 215             }
 216             if (!feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH)) {
 217                 reverseMotors = true;
 218                 for (unsigned index = 0; index < getMotorCount(); index++) {
 219                     pwmWriteDshotCommand(index, DSHOT_CMD_ROTATE_REVERSE);
 220                 }
 221             }
 222             #endif
 223             ENABLE_ARMING_FLAG(ARMED);
 224             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 225             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 226
 227             disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 228
 229             //beep to indicate arming
 230 #ifdef GPS
 231             if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
 232                 beeper(BEEPER_ARMING_GPS_FIX);
 233             else
 234                 beeper(BEEPER_ARMING);
 235 #else
 236             beeper(BEEPER_ARMING);
 237 #endif
 238
 239             return;
 240         }
 241     }
 242
 243     if (!ARMING_FLAG(ARMED)) {
 244         beeperConfirmationBeeps(1);
 245     }
 246 }
 247
 248 // Automatic ACC Offset Calibration
 249 bool AccInflightCalibrationArmed = false;
 250 bool AccInflightCalibrationMeasurementDone = false;
 251 bool AccInflightCalibrationSavetoEEProm = false;
 252 bool AccInflightCalibrationActive = false;
 253 uint16_t InflightcalibratingA = 0;
 254
 255 void handleInflightCalibrationStickPosition(void)
 256 {
 257     if (AccInflightCalibrationMeasurementDone) {
 258         // trigger saving into eeprom after landing
 259         AccInflightCalibrationMeasurementDone = false;
 260         AccInflightCalibrationSavetoEEProm = true;
 261     } else {
 262         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 263         if (AccInflightCalibrationArmed) {
 264             beeper(BEEPER_ACC_CALIBRATION);
 265         } else {
 266             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 267         }
 268     }
 269 }
 270
 271 static void updateInflightCalibrationState(void)
 272 {
 273     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
 274         InflightcalibratingA = 50;
 275         AccInflightCalibrationArmed = false;
 276     }
 277     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
 278         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 279             InflightcalibratingA = 50;
 280         AccInflightCalibrationActive = true;
 281     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 282         AccInflightCalibrationMeasurementDone = false;
 283         AccInflightCalibrationSavetoEEProm = true;
 284     }
 285 }
 286
 287 #if defined(GPS) || defined(MAG)
 288 void updateMagHold(void)
 289 {
 290     if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 291         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 292         if (dif <= -180)
 293             dif += 360;
 294         if (dif >= +180)
 295             dif -= 360;
 296         dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
 297         if (STATE(SMALL_ANGLE))
 298             rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30;    // 18 deg
 299     } else
 300         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 301 }
 302 #endif
 303
 304
 305 void processRx(timeUs_t currentTimeUs)
 306 {
 307     static bool armedBeeperOn = false;
 308     static bool airmodeIsActivated;
 309
 310     calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
 311
 312     // in 3D mode, we need to be able to disarm by switch at any time
 313     if (feature(FEATURE_3D)) {
 314         if (!IS_RC_MODE_ACTIVE(BOXARM))
 315             mwDisarm();
 316     }
 317
 318     updateRSSI(currentTimeUs);
 319
 320     if (feature(FEATURE_FAILSAFE)) {
 321
 322         if (currentTimeUs > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 323             failsafeStartMonitoring();
 324         }
 325
 326         failsafeUpdateState();
 327     }
 328
 329     const throttleStatus_e throttleStatus = calculateThrottleStatus();
 330
 331     if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
 332         if (rcCommand[THROTTLE] >= rxConfig()->airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
 333     } else {
 334         airmodeIsActivated = false;
 335     }
 336
 337     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 338      This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
 339     if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
 340         pidResetErrorGyroState();
 341         if (currentPidProfile->pidAtMinThrottle)
 342             pidStabilisationState(PID_STABILISATION_ON);
 343         else
 344             pidStabilisationState(PID_STABILISATION_OFF);
 345     } else {
 346         pidStabilisationState(PID_STABILISATION_ON);
 347     }
 348
 349     // When armed and motors aren't spinning, do beeps and then disarm
 350     // board after delay so users without buzzer won't lose fingers.
 351     // mixTable constrains motor commands, so checking  throttleStatus is enough
 352     if (ARMING_FLAG(ARMED)
 353         && feature(FEATURE_MOTOR_STOP)
 354         && !STATE(FIXED_WING)
 355         && !feature(FEATURE_3D)
 356         && !isAirmodeActive()
 357     ) {
 358         if (isUsingSticksForArming()) {
 359             if (throttleStatus == THROTTLE_LOW) {
 360                 if (armingConfig()->auto_disarm_delay != 0
 361                     && (int32_t)(disarmAt - millis()) < 0
 362                 ) {
 363                     // auto-disarm configured and delay is over
 364                     mwDisarm();
 365                     armedBeeperOn = false;
 366                 } else {
 367                     // still armed; do warning beeps while armed
 368                     beeper(BEEPER_ARMED);
 369                     armedBeeperOn = true;
 370                 }
 371             } else {
 372                 // throttle is not low
 373                 if (armingConfig()->auto_disarm_delay != 0) {
 374                     // extend disarm time
 375                     disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000;
 376                 }
 377
 378                 if (armedBeeperOn) {
 379                     beeperSilence();
 380                     armedBeeperOn = false;
 381                 }
 382             }
 383         } else {
 384             // arming is via AUX switch; beep while throttle low
 385             if (throttleStatus == THROTTLE_LOW) {
 386                 beeper(BEEPER_ARMED);
 387                 armedBeeperOn = true;
 388             } else if (armedBeeperOn) {
 389                 beeperSilence();
 390                 armedBeeperOn = false;
 391             }
 392         }
 393     }
 394
 395     processRcStickPositions(throttleStatus);
 396
 397     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 398         updateInflightCalibrationState();
 399     }
 400
 401     updateActivatedModes();
 402
 403     if (!cliMode) {
 404         updateAdjustmentStates();
 405         processRcAdjustments(currentControlRateProfile);
 406     }
 407
 408     bool canUseHorizonMode = true;
 409
 410     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
 411         // bumpless transfer to Level mode
 412         canUseHorizonMode = false;
 413
 414         if (!FLIGHT_MODE(ANGLE_MODE)) {
 415             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 416         }
 417     } else {
 418         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 419     }
 420
 421     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 422
 423         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 424
 425         if (!FLIGHT_MODE(HORIZON_MODE)) {
 426             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 427         }
 428     } else {
 429         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 430     }
 431
 432     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 433         LED1_ON;
 434     } else {
 435         LED1_OFF;
 436     }
 437
 438 #if defined(ACC) || defined(MAG)
 439     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 440 #if defined(GPS) || defined(MAG)
 441         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 442             if (!FLIGHT_MODE(MAG_MODE)) {
 443                 ENABLE_FLIGHT_MODE(MAG_MODE);
 444                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 445             }
 446         } else {
 447             DISABLE_FLIGHT_MODE(MAG_MODE);
 448         }
 449 #endif
 450         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 451             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 452                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 453             }
 454         } else {
 455             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 456         }
 457         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 458             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 459         }
 460     }
 461 #endif
 462
 463 #ifdef GPS
 464     if (sensors(SENSOR_GPS)) {
 465         updateGpsWaypointsAndMode();
 466     }
 467 #endif
 468
 469     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 470         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 471     } else {
 472         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 473     }
 474
 475     if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) {
 476         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 477     }
 478
 479 #ifdef TELEMETRY
 480     if (feature(FEATURE_TELEMETRY)) {
 481         if ((!telemetryConfig()->telemetry_switch && ARMING_FLAG(ARMED)) ||
 482                 (telemetryConfig()->telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 483
 484             releaseSharedTelemetryPorts();
 485         } else {
 486             // the telemetry state must be checked immediately so that shared serial ports are released.
 487             telemetryCheckState();
 488             mspSerialAllocatePorts();
 489         }
 490     }
 491 #endif
 492
 493 #ifdef VTX_CONTROL
 494     vtxUpdateActivatedChannel();
 495
 496     VTX_IF_READY {
 497         handleVTXControlButton();
 498     }
 499 #endif
 500 }
 501
 502 static void subTaskPidController(timeUs_t currentTimeUs)
 503 {
 504     uint32_t startTime = 0;
 505     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 506     // PID - note this is function pointer set by setPIDController()
 507     pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
 508     DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
 509 }
 510
 511 static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 512 {
 513     uint32_t startTime = 0;
 514     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 515
 516     // Read out gyro temperature if used for telemmetry
 517     if (feature(FEATURE_TELEMETRY)) {
 518         gyroReadTemperature();
 519     }
 520
 521 #ifdef MAG
 522     if (sensors(SENSOR_MAG)) {
 523         updateMagHold();
 524     }
 525 #endif
 526
 527 #if defined(BARO) || defined(SONAR)
 528     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
 529     updateRcCommands();
 530     if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 531         if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 532             applyAltHold();
 533         }
 534     }
 535 #endif
 536
 537     // If we're armed, at minimum throttle, and we do arming via the
 538     // sticks, do not process yaw input from the rx.  We do this so the
 539     // motors do not spin up while we are trying to arm or disarm.
 540     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 541     if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
 542 #ifndef USE_QUAD_MIXER_ONLY
 543 #ifdef USE_SERVOS
 544                 && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
 545 #endif
 546                 && mixerConfig()->mixerMode != MIXER_AIRPLANE
 547                 && mixerConfig()->mixerMode != MIXER_FLYING_WING
 548 #endif
 549     ) {
 550         resetYawAxis();
 551     }
 552
 553     if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 554         rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
 555     }
 556
 557     processRcCommand();
 558
 559 #ifdef GPS
 560     if (sensors(SENSOR_GPS)) {
 561         if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 562             updateGpsStateForHomeAndHoldMode();
 563         }
 564     }
 565 #endif
 566
 567 #ifdef USE_SDCARD
 568     afatfs_poll();
 569 #endif
 570
 571 #ifdef BLACKBOX
 572     if (!cliMode && blackboxConfig()->device) {
 573         blackboxUpdate(currentTimeUs);
 574     }
 575 #else
 576     UNUSED(currentTimeUs);
 577 #endif
 578
 579 #ifdef TRANSPONDER
 580     transponderUpdate(currentTimeUs);
 581 #endif
 582     DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
 583 }
 584
 585 static void subTaskMotorUpdate(void)
 586 {
 587     uint32_t startTime = 0;
 588     if (debugMode == DEBUG_CYCLETIME) {
 589         startTime = micros();
 590         static uint32_t previousMotorUpdateTime;
 591         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
 592         debug[2] = currentDeltaTime;
 593         debug[3] = currentDeltaTime - targetPidLooptime;
 594         previousMotorUpdateTime = startTime;
 595     } else if (debugMode == DEBUG_PIDLOOP) {
 596         startTime = micros();
 597     }
 598
 599     mixTable(currentPidProfile);
 600
 601 #ifdef USE_SERVOS
 602     // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos.
 603     if (isMixerUsingServos()) {
 604         writeServos();
 605     }
 606 #endif
 607
 608     if (motorControlEnable) {
 609         writeMotors();
 610     }
 611     DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
 612 }
 613
 614 uint8_t setPidUpdateCountDown(void)
 615 {
 616     if (gyroConfig()->gyro_soft_lpf_hz) {
 617         return pidConfig()->pid_process_denom - 1;
 618     } else {
 619         return 1;
 620     }
 621 }
 622
 623 // Function for loop trigger
 624 void taskMainPidLoop(timeUs_t currentTimeUs)
 625 {
 626     static bool runTaskMainSubprocesses;
 627     static uint8_t pidUpdateCountdown;
 628
 629 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC)
 630     if(lockMainPID() != 0) return;
 631 #endif
 632
 633     if (debugMode == DEBUG_CYCLETIME) {
 634         debug[0] = getTaskDeltaTime(TASK_SELF);
 635         debug[1] = averageSystemLoadPercent;
 636     }
 637
 638     if (runTaskMainSubprocesses) {
 639         subTaskMainSubprocesses(currentTimeUs);
 640         runTaskMainSubprocesses = false;
 641     }
 642
 643     // DEBUG_PIDLOOP, timings for:
 644     // 0 - gyroUpdate()
 645     // 1 - pidController()
 646     // 2 - subTaskMainSubprocesses()
 647     // 3 - subTaskMotorUpdate()
 648     uint32_t startTime = 0;
 649     if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
 650     gyroUpdate();
 651     DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - startTime);
 652
 653     if (pidUpdateCountdown) {
 654         pidUpdateCountdown--;
 655     } else {
 656         pidUpdateCountdown = setPidUpdateCountDown();
 657         subTaskPidController(currentTimeUs);
 658         subTaskMotorUpdate();
 659         runTaskMainSubprocesses = true;
 660     }
 661 }
 662
 663 bool isMotorsReversed()
 664 {
 665     return reverseMotors;
 666 }