src/main/mw.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 #include <math.h>
  22
  23 #include "platform.h"
  24 #include "debug.h"
  25
  26 #include "scheduler/scheduler.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30 #include "common/color.h"
  31 #include "common/utils.h"
  32 #include "common/filter.h"
  33
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro.h"
  36 #include "drivers/compass.h"
  37 #include "drivers/light_led.h"
  38
  39 #include "drivers/gpio.h"
  40 #include "drivers/system.h"
  41 #include "drivers/serial.h"
  42 #include "drivers/timer.h"
  43 #include "drivers/pwm_rx.h"
  44 #include "drivers/gyro_sync.h"
  45
  46 #include "sensors/sensors.h"
  47 #include "sensors/boardalignment.h"
  48 #include "sensors/sonar.h"
  49 #include "sensors/compass.h"
  50 #include "sensors/acceleration.h"
  51 #include "sensors/barometer.h"
  52 #include "sensors/gyro.h"
  53 #include "sensors/battery.h"
  54
  55 #include "io/beeper.h"
  56 #include "io/display.h"
  57 #include "io/escservo.h"
  58 #include "io/rc_controls.h"
  59 #include "io/rc_curves.h"
  60 #include "io/gimbal.h"
  61 #include "io/gps.h"
  62 #include "io/ledstrip.h"
  63 #include "io/serial.h"
  64 #include "io/serial_cli.h"
  65 #include "io/serial_msp.h"
  66 #include "io/statusindicator.h"
  67
  68 #include "rx/rx.h"
  69 #include "rx/msp.h"
  70
  71 #include "telemetry/telemetry.h"
  72 #include "blackbox/blackbox.h"
  73
  74 #include "flight/mixer.h"
  75 #include "flight/pid.h"
  76 #include "flight/imu.h"
  77 #include "flight/hil.h"
  78 #include "flight/failsafe.h"
  79 #include "flight/navigation_rewrite.h"
  80
  81 #include "config/runtime_config.h"
  82 #include "config/config.h"
  83 #include "config/config_profile.h"
  84 #include "config/config_master.h"
  85
  86 // June 2013     V2.2-dev
  87
  88 enum {
  89     ALIGN_GYRO = 0,
  90     ALIGN_ACCEL = 1,
  91     ALIGN_MAG = 2
  92 };
  93
  94 /* VBAT monitoring interval (in microseconds) - 1s*/
  95 #define VBATINTERVAL (6 * 3500)
  96 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
  97 #define IBATINTERVAL (6 * 3500)
  98 #define GYRO_WATCHDOG_DELAY 100  // Watchdog for boards without interrupt for gyro
  99
 100 uint16_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
 101
 102 float dT;
 103
 104 int16_t magHold;
 105 int16_t headFreeModeHold;
 106
 107 uint8_t motorControlEnable = false;
 108
 109 int16_t telemTemperature1;      // gyro sensor temperature
 110 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 111
 112 extern uint32_t currentTime;
 113
 114 static bool isRXDataNew;
 115
 116 bool isCalibrating()
 117 {
 118 #ifdef BARO
 119     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 120         return true;
 121     }
 122 #endif
 123
 124     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 125
 126     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 127 }
 128
 129 void annexCode(void)
 130 {
 131     int32_t tmp, tmp2, axis;
 132
 133     for (axis = 0; axis < 3; axis++) {
 134         tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 135         if (axis == ROLL || axis == PITCH) {
 136             if (currentProfile->rcControlsConfig.deadband) {
 137                 if (tmp > currentProfile->rcControlsConfig.deadband) {
 138                     tmp -= currentProfile->rcControlsConfig.deadband;
 139                 } else {
 140                     tmp = 0;
 141                 }
 142             }
 143
 144             tmp2 = tmp / 100;
 145             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 146         } else if (axis == YAW) {
 147             if (currentProfile->rcControlsConfig.yaw_deadband) {
 148                 if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
 149                     tmp -= currentProfile->rcControlsConfig.yaw_deadband;
 150                 } else {
 151                     tmp = 0;
 152                 }
 153             }
 154             tmp2 = tmp / 100;
 155             rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
 156         }
 157
 158         if (rcData[axis] < masterConfig.rxConfig.midrc)
 159             rcCommand[axis] = -rcCommand[axis];
 160     }
 161
 162     tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 163     tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
 164     tmp2 = tmp / 100;
 165     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 166
 167     if (FLIGHT_MODE(HEADFREE_MODE)) {
 168         float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
 169         float cosDiff = cos_approx(radDiff);
 170         float sinDiff = sin_approx(radDiff);
 171         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 172         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 173         rcCommand[PITCH] = rcCommand_PITCH;
 174     }
 175
 176     if (ARMING_FLAG(ARMED)) {
 177         LED0_ON;
 178     } else {
 179         if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
 180             ENABLE_ARMING_FLAG(OK_TO_ARM);
 181         }
 182
 183         if (!STATE(SMALL_ANGLE)) {
 184             DISABLE_ARMING_FLAG(OK_TO_ARM);
 185         }
 186
 187         if (isCalibrating() || isSystemOverloaded()) {
 188             warningLedFlash();
 189             DISABLE_ARMING_FLAG(OK_TO_ARM);
 190         }
 191
 192 #if defined(NAV)
 193         if (naivationBlockArming()) {
 194             DISABLE_ARMING_FLAG(OK_TO_ARM);
 195         }
 196 #endif
 197
 198         if (ARMING_FLAG(OK_TO_ARM)) {
 199             warningLedDisable();
 200         } else {
 201             warningLedFlash();
 202         }
 203
 204         warningLedUpdate();
 205     }
 206
 207     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 208     if (gyro.temperature)
 209         gyro.temperature(&telemTemperature1);
 210 }
 211
 212 void mwDisarm(void)
 213 {
 214     if (ARMING_FLAG(ARMED)) {
 215         DISABLE_ARMING_FLAG(ARMED);
 216
 217 #ifdef BLACKBOX
 218         if (feature(FEATURE_BLACKBOX)) {
 219             finishBlackbox();
 220         }
 221 #endif
 222
 223         beeper(BEEPER_DISARMING);      // emit disarm tone
 224     }
 225 }
 226
 227 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM)
 228
 229 void releaseSharedTelemetryPorts(void) {
 230     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 231     while (sharedPort) {
 232         mspReleasePortIfAllocated(sharedPort);
 233         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 234     }
 235 }
 236
 237 void mwArm(void)
 238 {
 239     if (ARMING_FLAG(OK_TO_ARM)) {
 240         if (ARMING_FLAG(ARMED)) {
 241             return;
 242         }
 243         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 244             return;
 245         }
 246         if (!ARMING_FLAG(PREVENT_ARMING)) {
 247             ENABLE_ARMING_FLAG(ARMED);
 248             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 249             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 250
 251 #ifdef BLACKBOX
 252             if (feature(FEATURE_BLACKBOX)) {
 253                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 254                 if (sharedBlackboxAndMspPort) {
 255                     mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
 256                 }
 257                 startBlackbox();
 258             }
 259 #endif
 260             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 261
 262             //beep to indicate arming
 263 #ifdef NAV
 264             if (navigationPositionEstimateIsHealthy())
 265                 beeper(BEEPER_ARMING_GPS_FIX);
 266             else
 267                 beeper(BEEPER_ARMING);
 268 #else
 269             beeper(BEEPER_ARMING);
 270 #endif
 271
 272             return;
 273         }
 274     }
 275
 276     if (!ARMING_FLAG(ARMED)) {
 277         beeperConfirmationBeeps(1);
 278     }
 279 }
 280
 281 void applyMagHold(void)
 282 {
 283     int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 284
 285     if (dif <= -180) {
 286         dif += 360;
 287     }
 288
 289     if (dif >= +180) {
 290         dif -= 360;
 291     }
 292
 293     dif *= masterConfig.yaw_control_direction;
 294
 295     if (STATE(SMALL_ANGLE)) {
 296         rcCommand[YAW] = dif * currentProfile->pidProfile.P8[PIDMAG] / 30;
 297     }
 298 }
 299
 300 void updateMagHold(void)
 301 {
 302 #if defined(NAV)
 303     int navHeadingState = naivationGetHeadingControlState();
 304     // NAV will prevent MAG_MODE from activating, but require heading control
 305     if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
 306         // Apply maghold only if heading control is in auto mode
 307         if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
 308             applyMagHold();
 309         }
 310     }
 311     else
 312 #endif
 313     if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 314         applyMagHold();
 315     }
 316     else {
 317         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 318     }
 319 }
 320
 321 void processRx(void)
 322 {
 323     static bool armedBeeperOn = false;
 324
 325     calculateRxChannelsAndUpdateFailsafe(currentTime);
 326
 327     // in 3D mode, we need to be able to disarm by switch at any time
 328     if (feature(FEATURE_3D)) {
 329         if (!IS_RC_MODE_ACTIVE(BOXARM))
 330             mwDisarm();
 331     }
 332
 333     updateRSSI(currentTime);
 334
 335     if (feature(FEATURE_FAILSAFE)) {
 336
 337         if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 338             failsafeStartMonitoring();
 339         }
 340
 341         failsafeUpdateState();
 342     }
 343
 344     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 345
 346     // When armed and motors aren't spinning, do beeps and then disarm
 347     // board after delay so users without buzzer won't lose fingers.
 348     // mixTable constrains motor commands, so checking  throttleStatus is enough
 349     if (ARMING_FLAG(ARMED)
 350         && feature(FEATURE_MOTOR_STOP)
 351         && !STATE(FIXED_WING)
 352     ) {
 353         if (isUsingSticksForArming()) {
 354             if (throttleStatus == THROTTLE_LOW) {
 355                 if (masterConfig.auto_disarm_delay != 0
 356                     && (int32_t)(disarmAt - millis()) < 0
 357                 ) {
 358                     // auto-disarm configured and delay is over
 359                     mwDisarm();
 360                     armedBeeperOn = false;
 361                 } else {
 362                     // still armed; do warning beeps while armed
 363                     beeper(BEEPER_ARMED);
 364                     armedBeeperOn = true;
 365                 }
 366             } else {
 367                 // throttle is not low
 368                 if (masterConfig.auto_disarm_delay != 0) {
 369                     // extend disarm time
 370                     disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
 371                 }
 372
 373                 if (armedBeeperOn) {
 374                     beeperSilence();
 375                     armedBeeperOn = false;
 376                 }
 377             }
 378         } else {
 379             // arming is via AUX switch; beep while throttle low
 380             if (throttleStatus == THROTTLE_LOW) {
 381                 beeper(BEEPER_ARMED);
 382                 armedBeeperOn = true;
 383             } else if (armedBeeperOn) {
 384                 beeperSilence();
 385                 armedBeeperOn = false;
 386             }
 387         }
 388     }
 389
 390     processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch);
 391
 392     updateActivatedModes(currentProfile->modeActivationConditions);
 393
 394     if (!cliMode) {
 395         updateAdjustmentStates(currentProfile->adjustmentRanges);
 396         processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
 397     }
 398
 399     bool canUseHorizonMode = true;
 400
 401     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive()) || naivationRequiresAngleMode()) && sensors(SENSOR_ACC)) {
 402         // bumpless transfer to Level mode
 403         canUseHorizonMode = false;
 404
 405         if (!FLIGHT_MODE(ANGLE_MODE)) {
 406             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 407         }
 408     } else {
 409         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 410     }
 411
 412     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 413
 414         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 415
 416         if (!FLIGHT_MODE(HORIZON_MODE)) {
 417             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 418         }
 419     } else {
 420         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 421     }
 422
 423     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 424         LED1_ON;
 425     } else {
 426         LED1_OFF;
 427     }
 428
 429     /* Heading lock mode */
 430     if (IS_RC_MODE_ACTIVE(BOXHEADINGLOCK)) {
 431         if (!FLIGHT_MODE(HEADING_LOCK)) {
 432             ENABLE_FLIGHT_MODE(HEADING_LOCK);
 433         }
 434     } else {
 435         DISABLE_FLIGHT_MODE(HEADING_LOCK);
 436     }
 437
 438 #if defined(MAG)
 439     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 440         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 441             if (!FLIGHT_MODE(MAG_MODE)) {
 442                 ENABLE_FLIGHT_MODE(MAG_MODE);
 443                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 444             }
 445         } else {
 446             DISABLE_FLIGHT_MODE(MAG_MODE);
 447         }
 448         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 449             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 450                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 451             }
 452         } else {
 453             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 454         }
 455         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 456             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 457         }
 458     }
 459 #endif
 460
 461     // Navigation may override PASSTHRU_MODE
 462     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU) && !naivationRequiresAngleMode()) {
 463         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 464     } else {
 465         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 466     }
 467
 468     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 469        This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air
 470        Low Throttle + roll and Pitch centered is assuming the copter is on the ground. Done to prevent complex air/ground detections */
 471     if (FLIGHT_MODE(PASSTHRU_MODE) || !ARMING_FLAG(ARMED)) {
 472         /* In PASSTHRU mode anti-windup must be explicitly enabled to prevent I-term wind-up (PID output is not used in PASSTHRU) */
 473         //ENABLE_STATE(ANTI_WINDUP);
 474         pidResetErrorAccumulators();
 475     }
 476     else {
 477         if (throttleStatus == THROTTLE_LOW) {
 478             if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
 479                 rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
 480
 481                 // ANTI_WINDUP at centred stick with MOTOR_STOP is needed on MRs and not needed on FWs
 482                 if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING))) {
 483                     ENABLE_STATE(ANTI_WINDUP);
 484                 }
 485                 else {
 486                     DISABLE_STATE(ANTI_WINDUP);
 487                 }
 488             }
 489             else {
 490                 pidResetErrorAccumulators();
 491             }
 492
 493             ENABLE_STATE(PID_ATTENUATE);
 494         }
 495         else {
 496             DISABLE_STATE(PID_ATTENUATE);
 497             DISABLE_STATE(ANTI_WINDUP);
 498         }
 499     }
 500
 501     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) {
 502         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 503     }
 504
 505 #ifdef TELEMETRY
 506     if (feature(FEATURE_TELEMETRY)) {
 507         if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
 508                 (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 509
 510             releaseSharedTelemetryPorts();
 511         } else {
 512             // the telemetry state must be checked immediately so that shared serial ports are released.
 513             telemetryCheckState();
 514             mspAllocateSerialPorts();
 515         }
 516     }
 517 #endif
 518
 519 }
 520
 521 void filterRc(bool isRXDataNew)
 522 {
 523     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
 524     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
 525     static int16_t factor, rcInterpolationFactor;
 526     static biquad_t filteredCycleTimeState;
 527     static bool filterInitialised;
 528     uint16_t filteredCycleTime;
 529     uint16_t rxRefreshRate;
 530
 531     // Set RC refresh rate for sampling and channels to filter
 532     initRxRefreshRate(&rxRefreshRate);
 533
 534     // Calculate average cycle time (1Hz LPF on cycle time)
 535     if (!filterInitialised) {
 536         filterInitBiQuad(1, &filteredCycleTimeState, 0);
 537         filterInitialised = true;
 538     }
 539
 540     filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
 541
 542     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 543
 544     if (isRXDataNew) {
 545         for (int channel=0; channel < 4; channel++) {
 546             deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
 547             lastCommand[channel] = rcCommand[channel];
 548         }
 549
 550         factor = rcInterpolationFactor - 1;
 551     } else {
 552         factor--;
 553     }
 554
 555     // Interpolate steps of rcCommand
 556     if (factor > 0) {
 557         for (int channel=0; channel < 4; channel++) {
 558             rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
 559          }
 560     } else {
 561         factor = 0;
 562     }
 563 }
 564
 565 void taskMainPidLoop(void)
 566 {
 567     cycleTime = getTaskDeltaTime(TASK_SELF);
 568     dT = (float)cycleTime * 0.000001f;
 569
 570     imuUpdateAccelerometer();
 571     imuUpdateGyroAndAttitude();
 572
 573     annexCode();
 574
 575     if (masterConfig.rxConfig.rcSmoothing) {
 576         filterRc(isRXDataNew);
 577     }
 578
 579 #if defined(NAV)
 580     if (isRXDataNew) {
 581         updateWaypointsAndNavigationMode();
 582     }
 583 #endif
 584
 585     isRXDataNew = false;
 586
 587 #if defined(NAV)
 588     updatePositionEstimator();
 589     applyWaypointNavigationAndAltitudeHold();
 590 #endif
 591
 592 #ifdef MAG
 593     if (sensors(SENSOR_MAG)) {
 594         updateMagHold();
 595     }
 596 #endif
 597
 598     // If we're armed, at minimum throttle, and we do arming via the
 599     // sticks, do not process yaw input from the rx.  We do this so the
 600     // motors do not spin up while we are trying to arm or disarm.
 601     // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 602     if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 603 #ifndef USE_QUAD_MIXER_ONLY
 604 #ifdef USE_SERVOS
 605             && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
 606 #endif
 607             && masterConfig.mixerMode != MIXER_AIRPLANE
 608             && masterConfig.mixerMode != MIXER_FLYING_WING
 609             && masterConfig.mixerMode != MIXER_CUSTOM_AIRPLANE
 610 #endif
 611     ) {
 612         rcCommand[YAW] = 0;
 613     }
 614
 615     // Apply throttle tilt compensation
 616     if (!STATE(FIXED_WING)) {
 617         int16_t thrTiltCompStrength = 0;
 618
 619         if (navigationRequiresThrottleTiltCompensation()) {
 620             thrTiltCompStrength = 100;
 621         }
 622         else if (currentProfile->throttle_tilt_compensation_strength && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 623             thrTiltCompStrength = currentProfile->throttle_tilt_compensation_strength;
 624         }
 625
 626         if (thrTiltCompStrength) {
 627             rcCommand[THROTTLE] = constrain(masterConfig.escAndServoConfig.minthrottle
 628                                             + (rcCommand[THROTTLE] - masterConfig.escAndServoConfig.minthrottle) * calculateThrottleTiltCompensationFactor(thrTiltCompStrength),
 629                                             masterConfig.escAndServoConfig.minthrottle,
 630                                             masterConfig.escAndServoConfig.maxthrottle);
 631         }
 632     }
 633
 634     pidController(&currentProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
 635
 636 #ifdef HIL
 637     if (hilActive) {
 638         hilUpdateControlState();
 639         motorControlEnable = false;
 640     }
 641 #endif
 642
 643     mixTable();
 644
 645 #ifdef USE_SERVOS
 646     filterServos();
 647     writeServos();
 648 #endif
 649
 650     if (motorControlEnable) {
 651         writeMotors();
 652     }
 653
 654 #ifdef BLACKBOX
 655     if (!cliMode && feature(FEATURE_BLACKBOX)) {
 656         handleBlackbox();
 657     }
 658 #endif
 659 }
 660
 661 // Function for loop trigger
 662 void taskMainPidLoopChecker(void) {
 663     // getTaskDeltaTime() returns delta time freezed at the moment of entering the scheduler. currentTime is freezed at the very same point.
 664     // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop
 665     uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 666
 667     if (masterConfig.gyroSync) {
 668         while (1) {
 669             if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - currentTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
 670                 break;
 671             }
 672         }
 673     }
 674
 675     taskMainPidLoop();
 676 }
 677
 678 void taskHandleSerial(void)
 679 {
 680     handleSerial();
 681 }
 682
 683 void taskUpdateBeeper(void)
 684 {
 685     beeperUpdate();          //call periodic beeper handler
 686 }
 687
 688 void taskUpdateBattery(void)
 689 {
 690     static uint32_t vbatLastServiced = 0;
 691     static uint32_t ibatLastServiced = 0;
 692
 693     if (feature(FEATURE_VBAT)) {
 694         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
 695             uint32_t vbatTimeDelta = currentTime - vbatLastServiced;
 696             vbatLastServiced = currentTime;
 697             updateBattery(vbatTimeDelta);
 698         }
 699     }
 700
 701     if (feature(FEATURE_CURRENT_METER)) {
 702         int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
 703
 704         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
 705             ibatLastServiced = currentTime;
 706             updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 707         }
 708     }
 709 }
 710
 711 bool taskUpdateRxCheck(uint32_t currentDeltaTime)
 712 {
 713     UNUSED(currentDeltaTime);
 714     updateRx(currentTime);
 715     return shouldProcessRx(currentTime);
 716 }
 717
 718 void taskUpdateRxMain(void)
 719 {
 720     processRx();
 721     updatePIDCoefficients(&currentProfile->pidProfile, currentControlRateProfile, &masterConfig.rxConfig);
 722     isRXDataNew = true;
 723 }
 724
 725 #ifdef GPS
 726 void taskProcessGPS(void)
 727 {
 728     // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 729     // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 730     // change this based on available hardware
 731     if (feature(FEATURE_GPS)) {
 732         gpsThread();
 733     }
 734
 735     if (sensors(SENSOR_GPS)) {
 736         updateGpsIndicator(currentTime);
 737     }
 738 }
 739 #endif
 740
 741 #ifdef MAG
 742 void taskUpdateCompass(void)
 743 {
 744     if (sensors(SENSOR_MAG)) {
 745         updateCompass(&masterConfig.magZero);
 746     }
 747 }
 748 #endif
 749
 750 #ifdef BARO
 751 void taskUpdateBaro(void)
 752 {
 753     if (sensors(SENSOR_BARO)) {
 754         uint32_t newDeadline = baroUpdate();
 755         rescheduleTask(TASK_SELF, newDeadline);
 756     }
 757
 758     //updatePositionEstimator_BaroTopic(currentTime);
 759 }
 760 #endif
 761
 762 #ifdef SONAR
 763 void taskUpdateSonar(void)
 764 {
 765     if (sensors(SENSOR_SONAR)) {
 766         sonarUpdate();
 767     }
 768
 769     //updatePositionEstimator_SonarTopic(currentTime);
 770 }
 771 #endif
 772
 773 #ifdef DISPLAY
 774 void taskUpdateDisplay(void)
 775 {
 776     if (feature(FEATURE_DISPLAY)) {
 777         updateDisplay();
 778     }
 779 }
 780 #endif
 781
 782 #ifdef TELEMETRY
 783 void taskTelemetry(void)
 784 {
 785     telemetryCheckState();
 786
 787     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 788         telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 789     }
 790 }
 791 #endif
 792
 793 #ifdef LED_STRIP
 794 void taskLedStrip(void)
 795 {
 796     if (feature(FEATURE_LED_STRIP)) {
 797         updateLedStrip();
 798     }
 799 }
 800 #endif