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