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 "scheduler.h"
  25 #include "debug.h"
  26
  27 #include "common/maths.h"
  28 #include "common/axis.h"
  29 #include "common/color.h"
  30 #include "common/utils.h"
  31 #include "common/filter.h"
  32
  33 #include "drivers/sensor.h"
  34 #include "drivers/accgyro.h"
  35 #include "drivers/compass.h"
  36 #include "drivers/light_led.h"
  37
  38 #include "drivers/gpio.h"
  39 #include "drivers/system.h"
  40 #include "drivers/serial.h"
  41 #include "drivers/timer.h"
  42 #include "drivers/pwm_rx.h"
  43 #include "drivers/gyro_sync.h"
  44
  45 #include "sensors/sensors.h"
  46 #include "sensors/boardalignment.h"
  47 #include "sensors/sonar.h"
  48 #include "sensors/compass.h"
  49 #include "sensors/acceleration.h"
  50 #include "sensors/barometer.h"
  51 #include "sensors/gyro.h"
  52 #include "sensors/battery.h"
  53
  54 #include "io/beeper.h"
  55 #include "io/display.h"
  56 #include "io/escservo.h"
  57 #include "io/rc_controls.h"
  58 #include "io/rc_curves.h"
  59 #include "io/gimbal.h"
  60 #include "io/gps.h"
  61 #include "io/ledstrip.h"
  62 #include "io/serial.h"
  63 #include "io/serial_cli.h"
  64 #include "io/serial_msp.h"
  65 #include "io/statusindicator.h"
  66 #include "io/asyncfatfs/asyncfatfs.h"
  67 #include "io/transponder_ir.h"
  68 #include "io/vtx.h"
  69
  70 #include "rx/rx.h"
  71 #include "rx/msp.h"
  72
  73 #include "telemetry/telemetry.h"
  74 #include "blackbox/blackbox.h"
  75
  76 #include "flight/mixer.h"
  77 #include "flight/pid.h"
  78 #include "flight/imu.h"
  79 #include "flight/altitudehold.h"
  80 #include "flight/failsafe.h"
  81 #include "flight/gtune.h"
  82 #include "flight/navigation.h"
  83
  84 #include "config/runtime_config.h"
  85 #include "config/config.h"
  86 #include "config/config_profile.h"
  87 #include "config/config_master.h"
  88
  89 // June 2013     V2.2-dev
  90
  91 enum {
  92     ALIGN_GYRO = 0,
  93     ALIGN_ACCEL = 1,
  94     ALIGN_MAG = 2
  95 };
  96
  97 /* VBAT monitoring interval (in microseconds) - 1s*/
  98 #define VBATINTERVAL (6 * 3500)
  99 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
 100 #define IBATINTERVAL (6 * 3500)
 101
 102 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
 103
 104 #define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
 105
 106 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
 107
 108 int16_t magHold;
 109 int16_t headFreeModeHold;
 110
 111 uint8_t motorControlEnable = false;
 112
 113 int16_t telemTemperature1;      // gyro sensor temperature
 114 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 115
 116 extern uint32_t currentTime;
 117 extern uint8_t PIDweight[3];
 118
 119 uint16_t filteredCycleTime;
 120 static bool isRXDataNew;
 121 static bool armingCalibrationWasInitialised;
 122
 123 extern pidControllerFuncPtr pid_controller;
 124
 125 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 126 {
 127     masterConfig.accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 128     masterConfig.accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 129
 130     saveConfigAndNotify();
 131 }
 132
 133 #ifdef GTUNE
 134
 135 void updateGtuneState(void)
 136 {
 137     static bool GTuneWasUsed = false;
 138
 139     if (IS_RC_MODE_ACTIVE(BOXGTUNE)) {
 140         if (!FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 141             ENABLE_FLIGHT_MODE(GTUNE_MODE);
 142             init_Gtune(&currentProfile->pidProfile);
 143             GTuneWasUsed = true;
 144         }
 145         if (!FLIGHT_MODE(GTUNE_MODE) && !ARMING_FLAG(ARMED) && GTuneWasUsed) {
 146             saveConfigAndNotify();
 147             GTuneWasUsed = false;
 148         }
 149     } else {
 150         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 151             DISABLE_FLIGHT_MODE(GTUNE_MODE);
 152         }
 153     }
 154 }
 155 #endif
 156
 157 bool isCalibrating()
 158 {
 159 #ifdef BARO
 160     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 161         return true;
 162     }
 163 #endif
 164
 165     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 166
 167     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 168 }
 169
 170 void filterRc(void)
 171 {
 172     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
 173     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
 174     static int16_t factor, rcInterpolationFactor;
 175     uint16_t rxRefreshRate;
 176
 177     // Set RC refresh rate for sampling and channels to filter
 178     initRxRefreshRate(&rxRefreshRate);
 179
 180     rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
 181
 182     if (isRXDataNew) {
 183         for (int channel=0; channel < 4; channel++) {
 184             deltaRC[channel] = rcCommand[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
 185             lastCommand[channel] = rcCommand[channel];
 186         }
 187
 188         isRXDataNew = false;
 189         factor = rcInterpolationFactor - 1;
 190     } else {
 191         factor--;
 192     }
 193
 194     // Interpolate steps of rcCommand
 195     if (factor > 0) {
 196         for (int channel=0; channel < 4; channel++) {
 197             rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
 198          }
 199     } else {
 200         factor = 0;
 201     }
 202 }
 203
 204 void scaleRcCommandToFpvCamAngle(void) {
 205     //recalculate sin/cos only when masterConfig.rxConfig.fpvCamAngleDegrees changed
 206     static uint8_t lastFpvCamAngleDegrees = 0;
 207     static float cosFactor = 1.0;
 208     static float sinFactor = 0.0;
 209
 210     if (lastFpvCamAngleDegrees != masterConfig.rxConfig.fpvCamAngleDegrees){
 211         lastFpvCamAngleDegrees = masterConfig.rxConfig.fpvCamAngleDegrees;
 212         cosFactor = cos_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
 213         sinFactor = sin_approx(masterConfig.rxConfig.fpvCamAngleDegrees * RAD);
 214     }
 215
 216     int16_t roll = rcCommand[ROLL];
 217     int16_t yaw = rcCommand[YAW];
 218     rcCommand[ROLL] = constrain(roll * cosFactor -  yaw * sinFactor, -500, 500);
 219     rcCommand[YAW]  = constrain(yaw  * cosFactor + roll * sinFactor, -500, 500);
 220 }
 221
 222 static void updateRcCommands(void)
 223 {
 224     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 225     int32_t prop;
 226     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 227         prop = 100;
 228     } else {
 229         if (rcData[THROTTLE] < 2000) {
 230             prop = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 231         } else {
 232             prop = 100 - currentControlRateProfile->dynThrPID;
 233         }
 234     }
 235
 236     for (int axis = 0; axis < 3; axis++) {
 237         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
 238         PIDweight[axis] = prop;
 239
 240         int32_t tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 241         if (axis == ROLL || axis == PITCH) {
 242             if (tmp > masterConfig.rcControlsConfig.deadband) {
 243                 tmp -= masterConfig.rcControlsConfig.deadband;
 244             } else {
 245                 tmp = 0;
 246             }
 247             rcCommand[axis] = rcLookupPitchRoll(tmp, currentControlRateProfile);
 248         } else if (axis == YAW) {
 249             if (tmp > masterConfig.rcControlsConfig.yaw_deadband) {
 250                 tmp -= masterConfig.rcControlsConfig.yaw_deadband;
 251             } else {
 252                 tmp = 0;
 253             }
 254             rcCommand[axis] = rcLookupYaw(tmp, currentControlRateProfile) * -masterConfig.yaw_control_direction;
 255         }
 256         if (rcData[axis] < masterConfig.rxConfig.midrc) {
 257             rcCommand[axis] = -rcCommand[axis];
 258         }
 259     }
 260
 261     int32_t tmp;
 262     if (feature(FEATURE_3D)) {
 263         tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX);
 264         tmp = (uint32_t)(tmp - PWM_RANGE_MIN);
 265     } else {
 266         tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 267         tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);
 268     }
 269     rcCommand[THROTTLE] = rcLookupThrottle(tmp);
 270
 271     if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3DDISABLESWITCH) && !failsafeIsActive()) {
 272         fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000);
 273         rcCommand[THROTTLE] = masterConfig.rxConfig.midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - masterConfig.rxConfig.midrc);
 274     }
 275
 276     if (FLIGHT_MODE(HEADFREE_MODE)) {
 277         const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
 278         const float cosDiff = cos_approx(radDiff);
 279         const float sinDiff = sin_approx(radDiff);
 280         const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 281         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 282         rcCommand[PITCH] = rcCommand_PITCH;
 283     }
 284
 285     // experimental scaling of RC command to FPV cam angle
 286     if (masterConfig.rxConfig.fpvCamAngleDegrees && !FLIGHT_MODE(HEADFREE_MODE)) {
 287         scaleRcCommandToFpvCamAngle();
 288     }
 289 }
 290
 291 static void updateLEDs(void)
 292 {
 293     if (ARMING_FLAG(ARMED)) {
 294         LED0_ON;
 295     } else {
 296         if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) {
 297             ENABLE_ARMING_FLAG(OK_TO_ARM);
 298         }
 299
 300         if (!STATE(SMALL_ANGLE)) {
 301             DISABLE_ARMING_FLAG(OK_TO_ARM);
 302         }
 303
 304         if (isCalibrating() || (averageSystemLoadPercent > 100)) {
 305             warningLedFlash();
 306             DISABLE_ARMING_FLAG(OK_TO_ARM);
 307         } else {
 308             if (ARMING_FLAG(OK_TO_ARM)) {
 309                 warningLedDisable();
 310             } else {
 311                 warningLedFlash();
 312             }
 313         }
 314
 315         warningLedUpdate();
 316     }
 317 }
 318
 319 void mwDisarm(void)
 320 {
 321     armingCalibrationWasInitialised = false;
 322
 323     if (ARMING_FLAG(ARMED)) {
 324         DISABLE_ARMING_FLAG(ARMED);
 325
 326 #ifdef BLACKBOX
 327         if (feature(FEATURE_BLACKBOX)) {
 328             finishBlackbox();
 329         }
 330 #endif
 331
 332         beeper(BEEPER_DISARMING);      // emit disarm tone
 333     }
 334 }
 335
 336 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
 337
 338 void releaseSharedTelemetryPorts(void) {
 339     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 340     while (sharedPort) {
 341         mspReleasePortIfAllocated(sharedPort);
 342         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 343     }
 344 }
 345
 346 void mwArm(void)
 347 {
 348     static bool firstArmingCalibrationWasCompleted;
 349
 350     if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
 351         gyroSetCalibrationCycles(calculateCalibratingCycles());
 352         armingCalibrationWasInitialised = true;
 353         firstArmingCalibrationWasCompleted = true;
 354     }
 355
 356     if (!isGyroCalibrationComplete()) return;  // prevent arming before gyro is calibrated
 357
 358     if (ARMING_FLAG(OK_TO_ARM)) {
 359         if (ARMING_FLAG(ARMED)) {
 360             return;
 361         }
 362         if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 363             return;
 364         }
 365         if (!ARMING_FLAG(PREVENT_ARMING)) {
 366             ENABLE_ARMING_FLAG(ARMED);
 367             ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 368             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 369
 370 #ifdef BLACKBOX
 371             if (feature(FEATURE_BLACKBOX)) {
 372                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 373                 if (sharedBlackboxAndMspPort) {
 374                     mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
 375                 }
 376                 startBlackbox();
 377             }
 378 #endif
 379             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 380
 381             //beep to indicate arming
 382 #ifdef GPS
 383             if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
 384                 beeper(BEEPER_ARMING_GPS_FIX);
 385             else
 386                 beeper(BEEPER_ARMING);
 387 #else
 388             beeper(BEEPER_ARMING);
 389 #endif
 390
 391             return;
 392         }
 393     }
 394
 395     if (!ARMING_FLAG(ARMED)) {
 396         beeperConfirmationBeeps(1);
 397     }
 398 }
 399
 400 // Automatic ACC Offset Calibration
 401 bool AccInflightCalibrationArmed = false;
 402 bool AccInflightCalibrationMeasurementDone = false;
 403 bool AccInflightCalibrationSavetoEEProm = false;
 404 bool AccInflightCalibrationActive = false;
 405 uint16_t InflightcalibratingA = 0;
 406
 407 void handleInflightCalibrationStickPosition(void)
 408 {
 409     if (AccInflightCalibrationMeasurementDone) {
 410         // trigger saving into eeprom after landing
 411         AccInflightCalibrationMeasurementDone = false;
 412         AccInflightCalibrationSavetoEEProm = true;
 413     } else {
 414         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 415         if (AccInflightCalibrationArmed) {
 416             beeper(BEEPER_ACC_CALIBRATION);
 417         } else {
 418             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 419         }
 420     }
 421 }
 422
 423 void updateInflightCalibrationState(void)
 424 {
 425     if (AccInflightCalibrationArmed && ARMING_FLAG(ARMED) && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !IS_RC_MODE_ACTIVE(BOXARM)) {   // Copter is airborne and you are turning it off via boxarm : start measurement
 426         InflightcalibratingA = 50;
 427         AccInflightCalibrationArmed = false;
 428     }
 429     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE measurement started, Land and Calib = 0 measurement stored
 430         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 431             InflightcalibratingA = 50;
 432         AccInflightCalibrationActive = true;
 433     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 434         AccInflightCalibrationMeasurementDone = false;
 435         AccInflightCalibrationSavetoEEProm = true;
 436     }
 437 }
 438
 439 void updateMagHold(void)
 440 {
 441     if (ABS(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
 442         int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
 443         if (dif <= -180)
 444             dif += 360;
 445         if (dif >= +180)
 446             dif -= 360;
 447         dif *= -masterConfig.yaw_control_direction;
 448         if (STATE(SMALL_ANGLE))
 449             rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg
 450     } else
 451         magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 452 }
 453
 454 void processRx(void)
 455 {
 456     static bool armedBeeperOn = false;
 457     static bool wasAirmodeIsActivated;
 458
 459     calculateRxChannelsAndUpdateFailsafe(currentTime);
 460
 461     // in 3D mode, we need to be able to disarm by switch at any time
 462     if (feature(FEATURE_3D)) {
 463         if (!IS_RC_MODE_ACTIVE(BOXARM))
 464             mwDisarm();
 465     }
 466
 467     updateRSSI(currentTime);
 468
 469     if (feature(FEATURE_FAILSAFE)) {
 470
 471         if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 472             failsafeStartMonitoring();
 473         }
 474
 475         failsafeUpdateState();
 476     }
 477
 478     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 479
 480     if (isAirmodeActive() && ARMING_FLAG(ARMED)) {
 481         if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) wasAirmodeIsActivated = true; // Prevent Iterm from being reset
 482     } else {
 483         wasAirmodeIsActivated = false;
 484     }
 485
 486     /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
 487      This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
 488     if (throttleStatus == THROTTLE_LOW && !wasAirmodeIsActivated) {
 489         pidResetErrorGyroState();
 490     }
 491
 492     // When armed and motors aren't spinning, do beeps and then disarm
 493     // board after delay so users without buzzer won't lose fingers.
 494     // mixTable constrains motor commands, so checking  throttleStatus is enough
 495     if (ARMING_FLAG(ARMED)
 496         && feature(FEATURE_MOTOR_STOP)
 497         && !STATE(FIXED_WING)
 498     ) {
 499         if (isUsingSticksForArming()) {
 500             if (throttleStatus == THROTTLE_LOW) {
 501                 if (masterConfig.auto_disarm_delay != 0
 502                     && (int32_t)(disarmAt - millis()) < 0
 503                 ) {
 504                     // auto-disarm configured and delay is over
 505                     mwDisarm();
 506                     armedBeeperOn = false;
 507                 } else {
 508                     // still armed; do warning beeps while armed
 509                     beeper(BEEPER_ARMED);
 510                     armedBeeperOn = true;
 511                 }
 512             } else {
 513                 // throttle is not low
 514                 if (masterConfig.auto_disarm_delay != 0) {
 515                     // extend disarm time
 516                     disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
 517                 }
 518
 519                 if (armedBeeperOn) {
 520                     beeperSilence();
 521                     armedBeeperOn = false;
 522                 }
 523             }
 524         } else {
 525             // arming is via AUX switch; beep while throttle low
 526             if (throttleStatus == THROTTLE_LOW) {
 527                 beeper(BEEPER_ARMED);
 528                 armedBeeperOn = true;
 529             } else if (armedBeeperOn) {
 530                 beeperSilence();
 531                 armedBeeperOn = false;
 532             }
 533         }
 534     }
 535
 536     processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.disarm_kill_switch);
 537
 538     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 539         updateInflightCalibrationState();
 540     }
 541
 542     updateActivatedModes(masterConfig.modeActivationConditions);
 543
 544     if (!cliMode) {
 545         updateAdjustmentStates(masterConfig.adjustmentRanges);
 546         processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
 547     }
 548
 549     bool canUseHorizonMode = true;
 550
 551     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
 552         // bumpless transfer to Level mode
 553         canUseHorizonMode = false;
 554
 555         if (!FLIGHT_MODE(ANGLE_MODE)) {
 556             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 557         }
 558     } else {
 559         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 560     }
 561
 562     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 563
 564         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 565
 566         if (!FLIGHT_MODE(HORIZON_MODE)) {
 567             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 568         }
 569     } else {
 570         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 571     }
 572
 573     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 574         LED1_ON;
 575     } else {
 576         LED1_OFF;
 577     }
 578
 579 #ifdef  MAG
 580     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 581         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 582             if (!FLIGHT_MODE(MAG_MODE)) {
 583                 ENABLE_FLIGHT_MODE(MAG_MODE);
 584                 magHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
 585             }
 586         } else {
 587             DISABLE_FLIGHT_MODE(MAG_MODE);
 588         }
 589         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 590             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 591                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 592             }
 593         } else {
 594             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 595         }
 596         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 597             headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading
 598         }
 599     }
 600 #endif
 601
 602 #ifdef GPS
 603     if (sensors(SENSOR_GPS)) {
 604         updateGpsWaypointsAndMode();
 605     }
 606 #endif
 607
 608     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 609         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 610     } else {
 611         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 612     }
 613
 614     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
 615         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 616     }
 617
 618 #ifdef TELEMETRY
 619     if (feature(FEATURE_TELEMETRY)) {
 620         if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
 621                 (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 622
 623             releaseSharedTelemetryPorts();
 624         } else {
 625             // the telemetry state must be checked immediately so that shared serial ports are released.
 626             telemetryCheckState();
 627             mspAllocateSerialPorts(&masterConfig.serialConfig);
 628         }
 629     }
 630 #endif
 631
 632 #ifdef VTX
 633     vtxUpdateActivatedChannel();
 634 #endif
 635 }
 636
 637 void subTaskPidController(void)
 638 {
 639     const uint32_t startTime = micros();
 640     // PID - note this is function pointer set by setPIDController()
 641     pid_controller(
 642         &currentProfile->pidProfile,
 643         currentControlRateProfile,
 644         masterConfig.max_angle_inclination,
 645         &masterConfig.accelerometerTrims,
 646         &masterConfig.rxConfig
 647     );
 648     if (debugMode == DEBUG_PIDLOOP) {debug[2] = micros() - startTime;}
 649 }
 650
 651 void subTaskMainSubprocesses(void) {
 652
 653     const uint32_t startTime = micros();
 654
 655     if (masterConfig.rxConfig.rcSmoothing || flightModeFlags) {
 656         filterRc();
 657     }
 658
 659     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 660     if (gyro.temperature) {
 661         gyro.temperature(&telemTemperature1);
 662     }
 663
 664     #ifdef MAG
 665             if (sensors(SENSOR_MAG)) {
 666                 updateMagHold();
 667             }
 668     #endif
 669
 670     #ifdef GTUNE
 671             updateGtuneState();
 672     #endif
 673
 674     #if defined(BARO) || defined(SONAR)
 675             if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 676                 if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 677                     applyAltHold(&masterConfig.airplaneConfig);
 678                 }
 679             }
 680     #endif
 681
 682         // If we're armed, at minimum throttle, and we do arming via the
 683         // sticks, do not process yaw input from the rx.  We do this so the
 684         // motors do not spin up while we are trying to arm or disarm.
 685         // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 686         if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 687     #ifndef USE_QUAD_MIXER_ONLY
 688     #ifdef USE_SERVOS
 689                     && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
 690     #endif
 691                     && masterConfig.mixerMode != MIXER_AIRPLANE
 692                     && masterConfig.mixerMode != MIXER_FLYING_WING
 693     #endif
 694         ) {
 695             rcCommand[YAW] = 0;
 696         }
 697
 698         if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 699             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value);
 700         }
 701
 702     #ifdef GPS
 703         if (sensors(SENSOR_GPS)) {
 704             if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 705                 updateGpsStateForHomeAndHoldMode();
 706             }
 707         }
 708     #endif
 709
 710     #ifdef USE_SDCARD
 711         afatfs_poll();
 712     #endif
 713
 714     #ifdef BLACKBOX
 715         if (!cliMode && feature(FEATURE_BLACKBOX)) {
 716             handleBlackbox();
 717         }
 718     #endif
 719
 720     #ifdef TRANSPONDER
 721         updateTransponder();
 722     #endif
 723     if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
 724 }
 725
 726 void subTaskMotorUpdate(void)
 727 {
 728     const uint32_t startTime = micros();
 729     if (debugMode == DEBUG_CYCLETIME) {
 730         static uint32_t previousMotorUpdateTime;
 731         const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime;
 732         debug[2] = currentDeltaTime;
 733         debug[3] = currentDeltaTime - targetPidLooptime;
 734         previousMotorUpdateTime = startTime;
 735     }
 736
 737     mixTable();
 738
 739 #ifdef USE_SERVOS
 740     filterServos();
 741     writeServos();
 742 #endif
 743
 744     if (motorControlEnable) {
 745         writeMotors(masterConfig.use_unsyncedPwm);
 746     }
 747     if (debugMode == DEBUG_PIDLOOP) {debug[3] = micros() - startTime;}
 748 }
 749
 750 uint8_t setPidUpdateCountDown(void) {
 751     if (masterConfig.gyro_soft_lpf_hz) {
 752             return masterConfig.pid_process_denom - 1;
 753     } else {
 754         return 1;
 755     }
 756 }
 757
 758 // Function for loop trigger
 759 void taskMainPidLoopCheck(void)
 760 {
 761     static uint32_t previousTime;
 762     static bool runTaskMainSubprocesses;
 763
 764     const uint32_t currentDeltaTime = getTaskDeltaTime(TASK_SELF);
 765
 766     cycleTime = micros() - previousTime;
 767     previousTime = micros();
 768
 769     if (debugMode == DEBUG_CYCLETIME) {
 770         debug[0] = cycleTime;
 771         debug[1] = averageSystemLoadPercent;
 772     }
 773
 774     const uint32_t startTime = micros();
 775     while (true) {
 776         if (gyroSyncCheckUpdate() || ((currentDeltaTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) {
 777             static uint8_t pidUpdateCountdown;
 778
 779             if (debugMode == DEBUG_PIDLOOP) {debug[0] = micros() - startTime;} // time spent busy waiting
 780             if (runTaskMainSubprocesses) {
 781                 subTaskMainSubprocesses();
 782                 runTaskMainSubprocesses = false;
 783             }
 784
 785             gyroUpdate();
 786
 787             if (pidUpdateCountdown) {
 788                 pidUpdateCountdown--;
 789             } else {
 790                 pidUpdateCountdown = setPidUpdateCountDown();
 791                 subTaskPidController();
 792                 subTaskMotorUpdate();
 793                 runTaskMainSubprocesses = true;
 794             }
 795
 796             break;
 797         }
 798     }
 799 }
 800
 801 void taskUpdateAccelerometer(void)
 802 {
 803     imuUpdateAccelerometer(&masterConfig.accelerometerTrims);
 804 }
 805
 806 void taskUpdateAttitude(void) {
 807     imuUpdateAttitude();
 808 }
 809
 810 void taskHandleSerial(void)
 811 {
 812     handleSerial();
 813 }
 814
 815 void taskUpdateBeeper(void)
 816 {
 817     beeperUpdate();          //call periodic beeper handler
 818 }
 819
 820 void taskUpdateBattery(void)
 821 {
 822     static uint32_t vbatLastServiced = 0;
 823     static uint32_t ibatLastServiced = 0;
 824
 825     if (feature(FEATURE_VBAT)) {
 826         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
 827             vbatLastServiced = currentTime;
 828             updateBattery();
 829         }
 830     }
 831
 832     if (feature(FEATURE_CURRENT_METER)) {
 833         int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
 834
 835         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
 836             ibatLastServiced = currentTime;
 837             updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 838         }
 839     }
 840 }
 841
 842 bool taskUpdateRxCheck(void)
 843 {
 844     updateRx(currentTime);
 845     return shouldProcessRx(currentTime);
 846 }
 847
 848 void taskUpdateRxMain(void)
 849 {
 850     processRx();
 851     isRXDataNew = true;
 852
 853     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
 854     updateRcCommands();
 855     updateLEDs();
 856
 857 #ifdef BARO
 858     if (sensors(SENSOR_BARO)) {
 859         updateAltHoldState();
 860     }
 861 #endif
 862
 863 #ifdef SONAR
 864     if (sensors(SENSOR_SONAR)) {
 865         updateSonarAltHoldState();
 866     }
 867 #endif
 868 }
 869
 870 #ifdef GPS
 871 void taskProcessGPS(void)
 872 {
 873     // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 874     // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 875     // change this based on available hardware
 876     if (feature(FEATURE_GPS)) {
 877         gpsThread();
 878     }
 879
 880     if (sensors(SENSOR_GPS)) {
 881         updateGpsIndicator(currentTime);
 882     }
 883 }
 884 #endif
 885
 886 #ifdef MAG
 887 void taskUpdateCompass(void)
 888 {
 889     if (sensors(SENSOR_MAG)) {
 890         updateCompass(&masterConfig.magZero);
 891     }
 892 }
 893 #endif
 894
 895 #ifdef BARO
 896 void taskUpdateBaro(void)
 897 {
 898     if (sensors(SENSOR_BARO)) {
 899         uint32_t newDeadline = baroUpdate();
 900         rescheduleTask(TASK_SELF, newDeadline);
 901     }
 902 }
 903 #endif
 904
 905 #ifdef SONAR
 906 void taskUpdateSonar(void)
 907 {
 908     if (sensors(SENSOR_SONAR)) {
 909         sonarUpdate();
 910     }
 911 }
 912 #endif
 913
 914 #if defined(BARO) || defined(SONAR)
 915 void taskCalculateAltitude(void)
 916 {
 917     if (false
 918 #if defined(BARO)
 919         || (sensors(SENSOR_BARO) && isBaroReady())
 920 #endif
 921 #if defined(SONAR)
 922         || sensors(SENSOR_SONAR)
 923 #endif
 924         ) {
 925         calculateEstimatedAltitude(currentTime);
 926     }}
 927 #endif
 928
 929 #ifdef DISPLAY
 930 void taskUpdateDisplay(void)
 931 {
 932     if (feature(FEATURE_DISPLAY)) {
 933         updateDisplay();
 934     }
 935 }
 936 #endif
 937
 938 #ifdef TELEMETRY
 939 void taskTelemetry(void)
 940 {
 941     telemetryCheckState();
 942
 943     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 944         telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 945     }
 946 }
 947 #endif
 948
 949 #ifdef LED_STRIP
 950 void taskLedStrip(void)
 951 {
 952     if (feature(FEATURE_LED_STRIP)) {
 953         updateLedStrip();
 954     }
 955 }
 956 #endif
 957
 958 #ifdef TRANSPONDER
 959 void taskTransponder(void)
 960 {
 961     if (feature(FEATURE_TRANSPONDER)) {
 962         updateTransponder();
 963     }
 964 }
 965 #endif