src/main/fc/fc_core.c

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