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