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
  25 #include "common/maths.h"
  26 #include "common/axis.h"
  27 #include "common/color.h"
  28
  29 #include "drivers/sensor.h"
  30 #include "drivers/accgyro.h"
  31 #include "drivers/compass.h"
  32 #include "drivers/light_led.h"
  33
  34 #include "drivers/gpio.h"
  35 #include "drivers/system.h"
  36 #include "drivers/serial.h"
  37 #include "drivers/timer.h"
  38 #include "drivers/pwm_rx.h"
  39
  40 #include "sensors/sensors.h"
  41 #include "sensors/boardalignment.h"
  42 #include "sensors/sonar.h"
  43 #include "sensors/compass.h"
  44 #include "sensors/acceleration.h"
  45 #include "sensors/barometer.h"
  46 #include "sensors/gyro.h"
  47 #include "sensors/battery.h"
  48
  49 #include "io/beeper.h"
  50 #include "io/display.h"
  51 #include "io/escservo.h"
  52 #include "io/rc_controls.h"
  53 #include "io/rc_curves.h"
  54 #include "io/gimbal.h"
  55 #include "io/gps.h"
  56 #include "io/ledstrip.h"
  57 #include "io/serial.h"
  58 #include "io/serial_cli.h"
  59 #include "io/serial_msp.h"
  60 #include "io/statusindicator.h"
  61
  62 #include "rx/rx.h"
  63 #include "rx/msp.h"
  64
  65 #include "telemetry/telemetry.h"
  66 #include "blackbox/blackbox.h"
  67
  68 #include "flight/mixer.h"
  69 #include "flight/pid.h"
  70 #include "flight/imu.h"
  71 #include "flight/altitudehold.h"
  72 #include "flight/failsafe.h"
  73 #include "flight/autotune.h"
  74 #include "flight/navigation.h"
  75
  76
  77 #include "config/runtime_config.h"
  78 #include "config/config.h"
  79 #include "config/config_profile.h"
  80 #include "config/config_master.h"
  81
  82 // June 2013     V2.2-dev
  83
  84 enum {
  85     ALIGN_GYRO = 0,
  86     ALIGN_ACCEL = 1,
  87     ALIGN_MAG = 2
  88 };
  89
  90 /* for VBAT monitoring frequency */
  91 #define VBATFREQ 6        // to read battery voltage - nth number of loop iterations
  92
  93 int16_t debug[4];
  94 uint32_t currentTime = 0;
  95 uint32_t previousTime = 0;
  96 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
  97
  98 int16_t magHold;
  99 int16_t headFreeModeHold;
 100
 101 int16_t telemTemperature1;      // gyro sensor temperature
 102 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 103
 104 extern uint8_t dynP8[3], dynI8[3], dynD8[3];
 105 extern failsafe_t *failsafe;
 106
 107 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
 108         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
 109
 110 extern pidControllerFuncPtr pid_controller;
 111
 112 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 113 {
 114     currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 115     currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 116
 117     saveConfigAndNotify();
 118 }
 119
 120 #ifdef AUTOTUNE
 121
 122 void updateAutotuneState(void)
 123 {
 124     static bool landedAfterAutoTuning = false;
 125     static bool autoTuneWasUsed = false;
 126
 127     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
 128         if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
 129             if (ARMING_FLAG(ARMED)) {
 130                 if (isAutotuneIdle() || landedAfterAutoTuning) {
 131                     autotuneReset();
 132                     landedAfterAutoTuning = false;
 133                 }
 134                 autotuneBeginNextPhase(&currentProfile->pidProfile);
 135                 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 136                 autoTuneWasUsed = true;
 137             } else {
 138                 if (havePidsBeenUpdatedByAutotune()) {
 139                     saveConfigAndNotify();
 140                     autotuneReset();
 141                 }
 142             }
 143         }
 144         return;
 145     }
 146
 147     if (FLIGHT_MODE(AUTOTUNE_MODE)) {
 148         autotuneEndPhase();
 149         DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 150     }
 151
 152     if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
 153         landedAfterAutoTuning = true;
 154     }
 155 }
 156 #endif
 157
 158 bool isCalibrating()
 159 {
 160 #ifdef BARO
 161     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 162         return true;
 163     }
 164 #endif
 165
 166     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 167
 168     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 169 }
 170
 171 void annexCode(void)
 172 {
 173     int32_t tmp, tmp2;
 174     int32_t axis, prop1 = 0, prop2;
 175
 176     static batteryState_e batteryState = BATTERY_OK;
 177     static uint8_t vbatTimer = 0;
 178     static int32_t vbatCycleTime = 0;
 179
 180     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
 181     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 182         prop2 = 100;
 183     } else {
 184         if (rcData[THROTTLE] < 2000) {
 185             prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 186         } else {
 187             prop2 = 100 - currentControlRateProfile->dynThrPID;
 188         }
 189     }
 190
 191     for (axis = 0; axis < 3; axis++) {
 192         tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 193         if (axis == ROLL || axis == PITCH) {
 194             if (currentProfile->rcControlsConfig.deadband) {
 195                 if (tmp > currentProfile->rcControlsConfig.deadband) {
 196                     tmp -= currentProfile->rcControlsConfig.deadband;
 197                 } else {
 198                     tmp = 0;
 199                 }
 200             }
 201
 202             tmp2 = tmp / 100;
 203             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 204             prop1 = 100 - (uint16_t)currentControlRateProfile->rollPitchRate * tmp / 500;
 205             prop1 = (uint16_t)prop1 * prop2 / 100;
 206         } else if (axis == YAW) {
 207             if (currentProfile->rcControlsConfig.yaw_deadband) {
 208                 if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
 209                     tmp -= currentProfile->rcControlsConfig.yaw_deadband;
 210                 } else {
 211                     tmp = 0;
 212                 }
 213             }
 214             rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
 215             prop1 = 100 - (uint16_t)currentControlRateProfile->yawRate * ABS(tmp) / 500;
 216         }
 217         // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
 218         dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
 219         dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
 220         dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
 221
 222         if (rcData[axis] < masterConfig.rxConfig.midrc)
 223             rcCommand[axis] = -rcCommand[axis];
 224     }
 225
 226     tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 227     tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
 228     tmp2 = tmp / 100;
 229     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 230
 231     if (FLIGHT_MODE(HEADFREE_MODE)) {
 232         float radDiff = degreesToRadians(heading - headFreeModeHold);
 233         float cosDiff = cosf(radDiff);
 234         float sinDiff = sinf(radDiff);
 235         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 236         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 237         rcCommand[PITCH] = rcCommand_PITCH;
 238     }
 239
 240     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) {
 241         vbatCycleTime += cycleTime;
 242         if (!(++vbatTimer % VBATFREQ)) {
 243
 244             if (feature(FEATURE_VBAT)) {
 245                 updateBatteryVoltage();
 246                 batteryState = calculateBatteryState();
 247             }
 248
 249             if (feature(FEATURE_CURRENT_METER)) {
 250                 updateCurrentMeter(vbatCycleTime);
 251             }
 252             vbatCycleTime = 0;
 253         }
 254     }
 255
 256     beepcodeUpdateState(batteryState);
 257
 258     if (ARMING_FLAG(ARMED)) {
 259         LED0_ON;
 260     } else {
 261         if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
 262             ENABLE_ARMING_FLAG(OK_TO_ARM);
 263         }
 264
 265         if (isCalibrating()) {
 266             LED0_TOGGLE;
 267             DISABLE_ARMING_FLAG(OK_TO_ARM);
 268         }
 269
 270         if (!STATE(SMALL_ANGLE)) {
 271             DISABLE_ARMING_FLAG(OK_TO_ARM);
 272         }
 273
 274         if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
 275             DISABLE_ARMING_FLAG(OK_TO_ARM);
 276         }
 277
 278         if (ARMING_FLAG(OK_TO_ARM)) {
 279             disableWarningLed();
 280         } else {
 281             enableWarningLed(currentTime);
 282         }
 283
 284         updateWarningLed(currentTime);
 285     }
 286
 287 #ifdef TELEMETRY
 288     checkTelemetryState();
 289 #endif
 290
 291     handleSerial();
 292
 293 #ifdef GPS
 294     if (sensors(SENSOR_GPS)) {
 295         updateGpsIndicator(currentTime);
 296     }
 297 #endif
 298
 299     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 300     if (gyro.temperature)
 301         gyro.temperature(&telemTemperature1);
 302 }
 303
 304 void mwDisarm(void)
 305 {
 306     if (ARMING_FLAG(ARMED)) {
 307         DISABLE_ARMING_FLAG(ARMED);
 308
 309 #ifdef TELEMETRY
 310         if (feature(FEATURE_TELEMETRY)) {
 311             // the telemetry state must be checked immediately so that shared serial ports are released.
 312             checkTelemetryState();
 313             mspAllocateSerialPorts(&masterConfig.serialConfig);
 314         }
 315 #endif
 316
 317 #ifdef BLACKBOX
 318         if (feature(FEATURE_BLACKBOX)) {
 319             finishBlackbox();
 320         }
 321 #endif
 322     }
 323 }
 324
 325 #define TELEMETRY_FUNCTION_MASK (FUNCTION_FRSKY_TELEMETRY | FUNCTION_HOTT_TELEMETRY | FUNCTION_MSP_TELEMETRY | FUNCTION_SMARTPORT_TELEMETRY)
 326
 327 void mwArm(void)
 328 {
 329     if (ARMING_FLAG(OK_TO_ARM)) {
 330         if (ARMING_FLAG(ARMED)) {
 331             return;
 332         }
 333         if (!ARMING_FLAG(PREVENT_ARMING)) {
 334             ENABLE_ARMING_FLAG(ARMED);
 335             headFreeModeHold = heading;
 336
 337 #ifdef TELEMETRY
 338             if (feature(FEATURE_TELEMETRY)) {
 339
 340
 341                 serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 342                 while (sharedPort) {
 343                     mspReleasePortIfAllocated(sharedPort);
 344                     sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 345                 }
 346             }
 347 #endif
 348
 349 #ifdef BLACKBOX
 350             if (feature(FEATURE_BLACKBOX)) {
 351                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 352                 if (sharedBlackboxAndMspPort) {
 353                     mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
 354                 }
 355                 startBlackbox();
 356             }
 357 #endif
 358             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 359
 360             return;
 361         }
 362     }
 363
 364     if (!ARMING_FLAG(ARMED)) {
 365         blinkLedAndSoundBeeper(2, 255, 1);
 366     }
 367 }
 368
 369 // Automatic ACC Offset Calibration
 370 bool AccInflightCalibrationArmed = false;
 371 bool AccInflightCalibrationMeasurementDone = false;
 372 bool AccInflightCalibrationSavetoEEProm = false;
 373 bool AccInflightCalibrationActive = false;
 374 uint16_t InflightcalibratingA = 0;
 375
 376 void handleInflightCalibrationStickPosition(void)
 377 {
 378     if (AccInflightCalibrationMeasurementDone) {
 379         // trigger saving into eeprom after landing
 380         AccInflightCalibrationMeasurementDone = false;
 381         AccInflightCalibrationSavetoEEProm = true;
 382     } else {
 383         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 384         if (AccInflightCalibrationArmed) {
 385             queueConfirmationBeep(4);
 386         } else {
 387             queueConfirmationBeep(6);
 388         }
 389     }
 390 }
 391
 392 void updateInflightCalibrationState(void)
 393 {
 394     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
 395         InflightcalibratingA = 50;
 396         AccInflightCalibrationArmed = false;
 397     }
 398     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
 399         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 400             InflightcalibratingA = 50;
 401         AccInflightCalibrationActive = true;
 402     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 403         AccInflightCalibrationMeasurementDone = false;
 404         AccInflightCalibrationSavetoEEProm = true;
 405     }
 406 }
 407
 408 void updateMagHold(void)
 409 {
 410     if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
 411         int16_t dif = heading - magHold;
 412         if (dif <= -180)
 413             dif += 360;
 414         if (dif >= +180)
 415             dif -= 360;
 416         dif *= -masterConfig.yaw_control_direction;
 417         if (STATE(SMALL_ANGLE))
 418             rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg
 419     } else
 420         magHold = heading;
 421 }
 422
 423 typedef enum {
 424 #ifdef MAG
 425     UPDATE_COMPASS_TASK,
 426 #endif
 427 #ifdef BARO
 428     UPDATE_BARO_TASK,
 429 #endif
 430 #ifdef SONAR
 431     UPDATE_SONAR_TASK,
 432 #endif
 433 #if defined(BARO) || defined(SONAR)
 434     CALCULATE_ALTITUDE_TASK,
 435 #endif
 436     UPDATE_DISPLAY_TASK
 437 } periodicTasks;
 438
 439 #define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
 440
 441
 442 void executePeriodicTasks(void)
 443 {
 444     static int periodicTaskIndex = 0;
 445
 446     switch (periodicTaskIndex++) {
 447 #ifdef MAG
 448     case UPDATE_COMPASS_TASK:
 449         if (sensors(SENSOR_MAG)) {
 450             updateCompass(&masterConfig.magZero);
 451         }
 452         break;
 453 #endif
 454
 455 #ifdef BARO
 456     case UPDATE_BARO_TASK:
 457         if (sensors(SENSOR_BARO)) {
 458             baroUpdate(currentTime);
 459         }
 460         break;
 461 #endif
 462
 463 #if defined(BARO) || defined(SONAR)
 464     case CALCULATE_ALTITUDE_TASK:
 465
 466 #if defined(BARO) && !defined(SONAR)
 467         if (sensors(SENSOR_BARO) && isBaroReady()) {
 468 #endif
 469 #if defined(BARO) && defined(SONAR)
 470         if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) {
 471 #endif
 472 #if !defined(BARO) && defined(SONAR)
 473         if (sensors(SENSOR_SONAR)) {
 474 #endif
 475             calculateEstimatedAltitude(currentTime);
 476         }
 477         break;
 478 #endif
 479 #ifdef SONAR
 480     case UPDATE_SONAR_TASK:
 481         if (sensors(SENSOR_SONAR)) {
 482             sonarUpdate();
 483         }
 484         break;
 485 #endif
 486 #ifdef DISPLAY
 487     case UPDATE_DISPLAY_TASK:
 488         if (feature(FEATURE_DISPLAY)) {
 489             updateDisplay();
 490         }
 491         break;
 492 #endif
 493     }
 494
 495     if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
 496         periodicTaskIndex = 0;
 497     }
 498 }
 499
 500 void processRx(void)
 501 {
 502     calculateRxChannelsAndUpdateFailsafe(currentTime);
 503
 504     // in 3D mode, we need to be able to disarm by switch at any time
 505     if (feature(FEATURE_3D)) {
 506         if (!IS_RC_MODE_ACTIVE(BOXARM))
 507             mwDisarm();
 508     }
 509
 510     updateRSSI(currentTime);
 511
 512     if (feature(FEATURE_FAILSAFE)) {
 513
 514         if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafe->vTable->isEnabled()) {
 515             failsafe->vTable->enable();
 516         }
 517
 518         failsafe->vTable->updateState();
 519     }
 520
 521     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 522
 523     if (throttleStatus == THROTTLE_LOW) {
 524         resetErrorAngle();
 525         resetErrorGyro();
 526     }
 527     // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers.
 528     // mixTable constrains motor commands, so checking  throttleStatus is enough
 529     if (
 530         ARMING_FLAG(ARMED)
 531         && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING)
 532         && masterConfig.auto_disarm_delay != 0
 533         && isUsingSticksForArming()
 534     ) {
 535         if (throttleStatus == THROTTLE_LOW) {
 536             if ((int32_t)(disarmAt - millis()) < 0)  // delay is over
 537                 mwDisarm();
 538         } else {
 539             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // extend delay
 540         }
 541     }
 542
 543     processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
 544
 545     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 546         updateInflightCalibrationState();
 547     }
 548
 549     updateActivatedModes(currentProfile->modeActivationConditions);
 550
 551     if (!cliMode) {
 552         updateAdjustmentStates(currentProfile->adjustmentRanges);
 553         processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
 554     }
 555
 556     bool canUseHorizonMode = true;
 557
 558     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
 559         // bumpless transfer to Level mode
 560         canUseHorizonMode = false;
 561
 562         if (!FLIGHT_MODE(ANGLE_MODE)) {
 563             resetErrorAngle();
 564             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 565         }
 566     } else {
 567         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 568     }
 569
 570     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 571
 572         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 573
 574         if (!FLIGHT_MODE(HORIZON_MODE)) {
 575             resetErrorAngle();
 576             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 577         }
 578     } else {
 579         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 580     }
 581
 582     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 583         LED1_ON;
 584     } else {
 585         LED1_OFF;
 586     }
 587
 588 #ifdef  MAG
 589     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 590         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 591             if (!FLIGHT_MODE(MAG_MODE)) {
 592                 ENABLE_FLIGHT_MODE(MAG_MODE);
 593                 magHold = heading;
 594             }
 595         } else {
 596             DISABLE_FLIGHT_MODE(MAG_MODE);
 597         }
 598         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 599             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 600                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 601             }
 602         } else {
 603             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 604         }
 605         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 606             headFreeModeHold = heading; // acquire new heading
 607         }
 608     }
 609 #endif
 610
 611 #ifdef GPS
 612     if (sensors(SENSOR_GPS)) {
 613         updateGpsWaypointsAndMode();
 614     }
 615 #endif
 616
 617     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 618         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 619     } else {
 620         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 621     }
 622
 623     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
 624         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 625     }
 626 }
 627
 628 void loop(void)
 629 {
 630     static uint32_t loopTime;
 631 #if defined(BARO) || defined(SONAR)
 632     static bool haveProcessedAnnexCodeOnce = false;
 633 #endif
 634
 635     updateRx();
 636
 637     if (shouldProcessRx(currentTime)) {
 638         processRx();
 639
 640 #ifdef BARO
 641         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 642         if (haveProcessedAnnexCodeOnce) {
 643             if (sensors(SENSOR_BARO)) {
 644                 updateAltHoldState();
 645             }
 646         }
 647 #endif
 648
 649 #ifdef SONAR
 650         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 651         if (haveProcessedAnnexCodeOnce) {
 652             if (sensors(SENSOR_SONAR)) {
 653                 updateSonarAltHoldState();
 654             }
 655         }
 656 #endif
 657
 658     } else {
 659         // not processing rx this iteration
 660         executePeriodicTasks();
 661
 662         // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 663         // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 664         // change this based on available hardware
 665 #ifdef GPS
 666         if (feature(FEATURE_GPS)) {
 667             gpsThread();
 668         }
 669 #endif
 670     }
 671
 672     currentTime = micros();
 673     if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
 674         loopTime = currentTime + masterConfig.looptime;
 675
 676         imuUpdate(&currentProfile->accelerometerTrims, masterConfig.mixerMode);
 677
 678         // Measure loop rate just after reading the sensors
 679         currentTime = micros();
 680         cycleTime = (int32_t)(currentTime - previousTime);
 681         previousTime = currentTime;
 682
 683         annexCode();
 684 #if defined(BARO) || defined(SONAR)
 685         haveProcessedAnnexCodeOnce = true;
 686 #endif
 687
 688 #ifdef AUTOTUNE
 689         updateAutotuneState();
 690 #endif
 691
 692 #ifdef MAG
 693         if (sensors(SENSOR_MAG)) {
 694                 updateMagHold();
 695         }
 696 #endif
 697
 698 #if defined(BARO) || defined(SONAR)
 699         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 700             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 701                 applyAltHold(&masterConfig.airplaneConfig);
 702             }
 703         }
 704 #endif
 705
 706         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 707             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
 708         }
 709
 710 #ifdef GPS
 711         if (sensors(SENSOR_GPS)) {
 712             if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 713                 updateGpsStateForHomeAndHoldMode();
 714             }
 715         }
 716 #endif
 717
 718         // PID - note this is function pointer set by setPIDController()
 719         pid_controller(
 720             &currentProfile->pidProfile,
 721             currentControlRateProfile,
 722             masterConfig.max_angle_inclination,
 723             &currentProfile->accelerometerTrims,
 724             &masterConfig.rxConfig
 725         );
 726
 727         mixTable();
 728         writeServos();
 729         writeMotors();
 730
 731 #ifdef BLACKBOX
 732         if (!cliMode && feature(FEATURE_BLACKBOX)) {
 733             handleBlackbox();
 734         }
 735 #endif
 736     }
 737
 738 #ifdef TELEMETRY
 739     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 740         handleTelemetry();
 741     }
 742 #endif
 743
 744 #ifdef LED_STRIP
 745     if (feature(FEATURE_LED_STRIP)) {
 746         updateLedStrip();
 747     }
 748 #endif
 749 }