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 #include "flight/filter.h"
  76
  77
  78 #include "config/runtime_config.h"
  79 #include "config/config.h"
  80 #include "config/config_profile.h"
  81 #include "config/config_master.h"
  82
  83 // June 2013     V2.2-dev
  84
  85 enum {
  86     ALIGN_GYRO = 0,
  87     ALIGN_ACCEL = 1,
  88     ALIGN_MAG = 2
  89 };
  90
  91 /* for VBAT monitoring frequency */
  92 #define VBATFREQ 6        // to read battery voltage - nth number of loop iterations
  93
  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 uint8_t motorControlEnable = false;
 102
 103 int16_t telemTemperature1;      // gyro sensor temperature
 104 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 105
 106 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 107
 108 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
 109         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
 110
 111 extern pidControllerFuncPtr pid_controller;
 112
 113 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 114 {
 115     currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 116     currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 117
 118     saveConfigAndNotify();
 119 }
 120
 121 #ifdef AUTOTUNE
 122
 123 void updateAutotuneState(void)
 124 {
 125     static bool landedAfterAutoTuning = false;
 126     static bool autoTuneWasUsed = false;
 127
 128     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
 129         if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
 130             if (ARMING_FLAG(ARMED)) {
 131                 if (isAutotuneIdle() || landedAfterAutoTuning) {
 132                     autotuneReset();
 133                     landedAfterAutoTuning = false;
 134                 }
 135                 autotuneBeginNextPhase(&currentProfile->pidProfile);
 136                 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 137                 autoTuneWasUsed = true;
 138             } else {
 139                 if (havePidsBeenUpdatedByAutotune()) {
 140                     saveConfigAndNotify();
 141                     autotuneReset();
 142                 }
 143             }
 144         }
 145         return;
 146     }
 147
 148     if (FLIGHT_MODE(AUTOTUNE_MODE)) {
 149         autotuneEndPhase();
 150         DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 151     }
 152
 153     if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
 154         landedAfterAutoTuning = true;
 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 annexCode(void)
 173 {
 174     int32_t tmp, tmp2;
 175     int32_t axis, prop1 = 0, prop2;
 176
 177     static batteryState_e batteryState = BATTERY_OK;
 178     static uint8_t vbatTimer = 0;
 179     static int32_t vbatCycleTime = 0;
 180
 181     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 182     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 183         prop2 = 100;
 184     } else {
 185         if (rcData[THROTTLE] < 2000) {
 186             prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 187         } else {
 188             prop2 = 100 - currentControlRateProfile->dynThrPID;
 189         }
 190     }
 191
 192     for (axis = 0; axis < 3; axis++) {
 193         tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 194         if (axis == ROLL || axis == PITCH) {
 195             if (currentProfile->rcControlsConfig.deadband) {
 196                 if (tmp > currentProfile->rcControlsConfig.deadband) {
 197                     tmp -= currentProfile->rcControlsConfig.deadband;
 198                 } else {
 199                     tmp = 0;
 200                 }
 201             }
 202
 203             tmp2 = tmp / 100;
 204             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 205             prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
 206             prop1 = (uint16_t)prop1 * prop2 / 100;
 207         } else if (axis == YAW) {
 208             if (currentProfile->rcControlsConfig.yaw_deadband) {
 209                 if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
 210                     tmp -= currentProfile->rcControlsConfig.yaw_deadband;
 211                 } else {
 212                     tmp = 0;
 213                 }
 214             }
 215             tmp2 = tmp / 100;
 216             rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
 217             prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
 218         }
 219         // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
 220         dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
 221         dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
 222         dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
 223
 224         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
 225         if (axis == YAW) {
 226             PIDweight[axis] = 100;
 227         }
 228         else {
 229             PIDweight[axis] = prop2;
 230         }
 231
 232         if (rcData[axis] < masterConfig.rxConfig.midrc)
 233             rcCommand[axis] = -rcCommand[axis];
 234     }
 235
 236     tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 237     tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
 238     tmp2 = tmp / 100;
 239     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 240
 241     if (FLIGHT_MODE(HEADFREE_MODE)) {
 242         float radDiff = degreesToRadians(heading - headFreeModeHold);
 243         float cosDiff = cosf(radDiff);
 244         float sinDiff = sinf(radDiff);
 245         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 246         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 247         rcCommand[PITCH] = rcCommand_PITCH;
 248     }
 249
 250     if (feature(FEATURE_VBAT | FEATURE_CURRENT_METER)) {
 251         vbatCycleTime += cycleTime;
 252         if (!(++vbatTimer % VBATFREQ)) {
 253
 254             if (feature(FEATURE_VBAT)) {
 255                 updateBatteryVoltage();
 256                 batteryState = calculateBatteryState();
 257                 //handle beepers for battery levels
 258                 if (batteryState == BATTERY_CRITICAL)
 259                     beeper(BEEPER_BAT_CRIT_LOW);    //critically low battery
 260                 else if (batteryState == BATTERY_WARNING)
 261                     beeper(BEEPER_BAT_LOW);         //low battery
 262             }
 263
 264             if (feature(FEATURE_CURRENT_METER)) {
 265                 updateCurrentMeter(vbatCycleTime, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 266             }
 267             vbatCycleTime = 0;
 268         }
 269     }
 270
 271     beeperUpdate();          //call periodic beeper handler
 272
 273     if (ARMING_FLAG(ARMED)) {
 274         LED0_ON;
 275     } else {
 276         if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
 277             ENABLE_ARMING_FLAG(OK_TO_ARM);
 278         }
 279
 280         if (!STATE(SMALL_ANGLE)) {
 281             DISABLE_ARMING_FLAG(OK_TO_ARM);
 282         }
 283
 284         if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
 285             DISABLE_ARMING_FLAG(OK_TO_ARM);
 286         }
 287
 288         if (isCalibrating()) {
 289             warningLedFlash();
 290             DISABLE_ARMING_FLAG(OK_TO_ARM);
 291         } else {
 292             if (ARMING_FLAG(OK_TO_ARM)) {
 293                 warningLedDisable();
 294             } else {
 295                 warningLedFlash();
 296             }
 297         }
 298
 299         warningLedUpdate();
 300     }
 301
 302 #ifdef TELEMETRY
 303     telemetryCheckState();
 304 #endif
 305
 306     handleSerial();
 307
 308 #ifdef GPS
 309     if (sensors(SENSOR_GPS)) {
 310         updateGpsIndicator(currentTime);
 311     }
 312 #endif
 313
 314     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 315     if (gyro.temperature)
 316         gyro.temperature(&telemTemperature1);
 317 }
 318
 319 void mwDisarm(void)
 320 {
 321     if (ARMING_FLAG(ARMED)) {
 322         DISABLE_ARMING_FLAG(ARMED);
 323
 324 #ifdef BLACKBOX
 325         if (feature(FEATURE_BLACKBOX)) {
 326             finishBlackbox();
 327         }
 328 #endif
 329
 330         beeper(BEEPER_DISARMING);      // emit disarm tone
 331     }
 332 }
 333
 334 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_MSP | FUNCTION_TELEMETRY_SMARTPORT)
 335
 336 void releaseSharedTelemetryPorts(void) {
 337     serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 338     while (sharedPort) {
 339         mspReleasePortIfAllocated(sharedPort);
 340         sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
 341     }
 342 }
 343
 344 void mwArm(void)
 345 {
 346     if (ARMING_FLAG(OK_TO_ARM)) {
 347         if (ARMING_FLAG(ARMED)) {
 348             return;
 349         }
 350         if (!ARMING_FLAG(PREVENT_ARMING)) {
 351             ENABLE_ARMING_FLAG(ARMED);
 352             headFreeModeHold = heading;
 353
 354 #ifdef BLACKBOX
 355             if (feature(FEATURE_BLACKBOX)) {
 356                 serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
 357                 if (sharedBlackboxAndMspPort) {
 358                     mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
 359                 }
 360                 startBlackbox();
 361             }
 362 #endif
 363             disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;   // start disarm timeout, will be extended when throttle is nonzero
 364
 365             //beep to indicate arming
 366 #ifdef GPS
 367             if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
 368                 beeper(BEEPER_ARMING_GPS_FIX);
 369             else
 370                 beeper(BEEPER_ARMING);
 371 #else
 372             beeper(BEEPER_ARMING);
 373 #endif
 374
 375             return;
 376         }
 377     }
 378
 379     if (!ARMING_FLAG(ARMED)) {
 380         beeperConfirmationBeeps(1);
 381     }
 382 }
 383
 384 // Automatic ACC Offset Calibration
 385 bool AccInflightCalibrationArmed = false;
 386 bool AccInflightCalibrationMeasurementDone = false;
 387 bool AccInflightCalibrationSavetoEEProm = false;
 388 bool AccInflightCalibrationActive = false;
 389 uint16_t InflightcalibratingA = 0;
 390
 391 void handleInflightCalibrationStickPosition(void)
 392 {
 393     if (AccInflightCalibrationMeasurementDone) {
 394         // trigger saving into eeprom after landing
 395         AccInflightCalibrationMeasurementDone = false;
 396         AccInflightCalibrationSavetoEEProm = true;
 397     } else {
 398         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 399         if (AccInflightCalibrationArmed) {
 400             beeper(BEEPER_ACC_CALIBRATION);
 401         } else {
 402             beeper(BEEPER_ACC_CALIBRATION_FAIL);
 403         }
 404     }
 405 }
 406
 407 void updateInflightCalibrationState(void)
 408 {
 409     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
 410         InflightcalibratingA = 50;
 411         AccInflightCalibrationArmed = false;
 412     }
 413     if (IS_RC_MODE_ACTIVE(BOXCALIB)) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
 414         if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 415             InflightcalibratingA = 50;
 416         AccInflightCalibrationActive = true;
 417     } else if (AccInflightCalibrationMeasurementDone && !ARMING_FLAG(ARMED)) {
 418         AccInflightCalibrationMeasurementDone = false;
 419         AccInflightCalibrationSavetoEEProm = true;
 420     }
 421 }
 422
 423 void updateMagHold(void)
 424 {
 425     if (ABS(rcCommand[YAW]) < 70 && FLIGHT_MODE(MAG_MODE)) {
 426         int16_t dif = heading - magHold;
 427         if (dif <= -180)
 428             dif += 360;
 429         if (dif >= +180)
 430             dif -= 360;
 431         dif *= -masterConfig.yaw_control_direction;
 432         if (STATE(SMALL_ANGLE))
 433             rcCommand[YAW] -= dif * currentProfile->pidProfile.P8[PIDMAG] / 30;    // 18 deg
 434     } else
 435         magHold = heading;
 436 }
 437
 438 typedef enum {
 439 #ifdef MAG
 440     UPDATE_COMPASS_TASK,
 441 #endif
 442 #ifdef BARO
 443     UPDATE_BARO_TASK,
 444 #endif
 445 #ifdef SONAR
 446     UPDATE_SONAR_TASK,
 447 #endif
 448 #if defined(BARO) || defined(SONAR)
 449     CALCULATE_ALTITUDE_TASK,
 450 #endif
 451     UPDATE_DISPLAY_TASK
 452 } periodicTasks;
 453
 454 #define PERIODIC_TASK_COUNT (UPDATE_DISPLAY_TASK + 1)
 455
 456
 457 void executePeriodicTasks(void)
 458 {
 459     static int periodicTaskIndex = 0;
 460
 461     switch (periodicTaskIndex++) {
 462 #ifdef MAG
 463     case UPDATE_COMPASS_TASK:
 464         if (sensors(SENSOR_MAG)) {
 465             updateCompass(&masterConfig.magZero);
 466         }
 467         break;
 468 #endif
 469
 470 #ifdef BARO
 471     case UPDATE_BARO_TASK:
 472         if (sensors(SENSOR_BARO)) {
 473             baroUpdate(currentTime);
 474         }
 475         break;
 476 #endif
 477
 478 #if defined(BARO) || defined(SONAR)
 479     case CALCULATE_ALTITUDE_TASK:
 480
 481 #if defined(BARO) && !defined(SONAR)
 482         if (sensors(SENSOR_BARO) && isBaroReady()) {
 483 #endif
 484 #if defined(BARO) && defined(SONAR)
 485         if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) {
 486 #endif
 487 #if !defined(BARO) && defined(SONAR)
 488         if (sensors(SENSOR_SONAR)) {
 489 #endif
 490             calculateEstimatedAltitude(currentTime);
 491         }
 492         break;
 493 #endif
 494 #ifdef SONAR
 495     case UPDATE_SONAR_TASK:
 496         if (sensors(SENSOR_SONAR)) {
 497             sonarUpdate();
 498         }
 499         break;
 500 #endif
 501 #ifdef DISPLAY
 502     case UPDATE_DISPLAY_TASK:
 503         if (feature(FEATURE_DISPLAY)) {
 504             updateDisplay();
 505         }
 506         break;
 507 #endif
 508     }
 509
 510     if (periodicTaskIndex >= PERIODIC_TASK_COUNT) {
 511         periodicTaskIndex = 0;
 512     }
 513 }
 514
 515 void processRx(void)
 516 {
 517     static bool armedBeeperOn = false;
 518
 519     calculateRxChannelsAndUpdateFailsafe(currentTime);
 520
 521     // in 3D mode, we need to be able to disarm by switch at any time
 522     if (feature(FEATURE_3D)) {
 523         if (!IS_RC_MODE_ACTIVE(BOXARM))
 524             mwDisarm();
 525     }
 526
 527     updateRSSI(currentTime);
 528
 529     if (feature(FEATURE_FAILSAFE)) {
 530
 531         if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsMonitoring()) {
 532             failsafeStartMonitoring();
 533         }
 534
 535         failsafeUpdateState();
 536     }
 537
 538     throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 539
 540     if (throttleStatus == THROTTLE_LOW) {
 541         pidResetErrorAngle();
 542         pidResetErrorGyro();
 543     }
 544
 545     // When armed and motors aren't spinning, do beeps and then disarm
 546     // board after delay so users without buzzer won't lose fingers.
 547     // mixTable constrains motor commands, so checking  throttleStatus is enough
 548     if (ARMING_FLAG(ARMED)
 549         && feature(FEATURE_MOTOR_STOP)
 550         && !STATE(FIXED_WING)
 551     ) {
 552         if (isUsingSticksForArming()) {
 553             if (throttleStatus == THROTTLE_LOW) {
 554                 if (masterConfig.auto_disarm_delay != 0
 555                     && (int32_t)(disarmAt - millis()) < 0
 556                 ) {
 557                     // auto-disarm configured and delay is over
 558                     mwDisarm();
 559                     armedBeeperOn = false;
 560                 } else {
 561                     // still armed; do warning beeps while armed
 562                     beeper(BEEPER_ARMED);
 563                     armedBeeperOn = true;
 564                 }
 565             } else {
 566                 // throttle is not low
 567                 if (masterConfig.auto_disarm_delay != 0) {
 568                     // extend disarm time
 569                     disarmAt = millis() + masterConfig.auto_disarm_delay * 1000;
 570                 }
 571
 572                 if (armedBeeperOn) {
 573                     beeperSilence();
 574                     armedBeeperOn = false;
 575                 }
 576             }
 577         } else {
 578             // arming is via AUX switch; beep while throttle low
 579             if (throttleStatus == THROTTLE_LOW) {
 580                 beeper(BEEPER_ARMED);
 581                 armedBeeperOn = true;
 582             } else if (armedBeeperOn) {
 583                 beeperSilence();
 584                 armedBeeperOn = false;
 585             }
 586         }
 587     }
 588
 589     processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch);
 590
 591     if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 592         updateInflightCalibrationState();
 593     }
 594
 595     updateActivatedModes(currentProfile->modeActivationConditions);
 596
 597     if (!cliMode) {
 598         updateAdjustmentStates(currentProfile->adjustmentRanges);
 599         processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig);
 600     }
 601
 602     bool canUseHorizonMode = true;
 603
 604     if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeIsActive())) && (sensors(SENSOR_ACC))) {
 605         // bumpless transfer to Level mode
 606         canUseHorizonMode = false;
 607
 608         if (!FLIGHT_MODE(ANGLE_MODE)) {
 609             pidResetErrorAngle();
 610             ENABLE_FLIGHT_MODE(ANGLE_MODE);
 611         }
 612     } else {
 613         DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support
 614     }
 615
 616     if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) {
 617
 618         DISABLE_FLIGHT_MODE(ANGLE_MODE);
 619
 620         if (!FLIGHT_MODE(HORIZON_MODE)) {
 621             pidResetErrorAngle();
 622             ENABLE_FLIGHT_MODE(HORIZON_MODE);
 623         }
 624     } else {
 625         DISABLE_FLIGHT_MODE(HORIZON_MODE);
 626     }
 627
 628     if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 629         LED1_ON;
 630     } else {
 631         LED1_OFF;
 632     }
 633
 634 #ifdef  MAG
 635     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 636         if (IS_RC_MODE_ACTIVE(BOXMAG)) {
 637             if (!FLIGHT_MODE(MAG_MODE)) {
 638                 ENABLE_FLIGHT_MODE(MAG_MODE);
 639                 magHold = heading;
 640             }
 641         } else {
 642             DISABLE_FLIGHT_MODE(MAG_MODE);
 643         }
 644         if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) {
 645             if (!FLIGHT_MODE(HEADFREE_MODE)) {
 646                 ENABLE_FLIGHT_MODE(HEADFREE_MODE);
 647             }
 648         } else {
 649             DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 650         }
 651         if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) {
 652             headFreeModeHold = heading; // acquire new heading
 653         }
 654     }
 655 #endif
 656
 657 #ifdef GPS
 658     if (sensors(SENSOR_GPS)) {
 659         updateGpsWaypointsAndMode();
 660     }
 661 #endif
 662
 663     if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) {
 664         ENABLE_FLIGHT_MODE(PASSTHRU_MODE);
 665     } else {
 666         DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
 667     }
 668
 669     if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
 670         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
 671     }
 672
 673 #ifdef TELEMETRY
 674     if (feature(FEATURE_TELEMETRY)) {
 675         if ((!masterConfig.telemetryConfig.telemetry_switch && ARMING_FLAG(ARMED)) ||
 676                 (masterConfig.telemetryConfig.telemetry_switch && IS_RC_MODE_ACTIVE(BOXTELEMETRY))) {
 677
 678             releaseSharedTelemetryPorts();
 679         } else {
 680             // the telemetry state must be checked immediately so that shared serial ports are released.
 681             telemetryCheckState();
 682             mspAllocateSerialPorts(&masterConfig.serialConfig);
 683         }
 684     }
 685 #endif
 686
 687 }
 688
 689 void loop(void)
 690 {
 691     static uint32_t loopTime;
 692 #if defined(BARO) || defined(SONAR)
 693     static bool haveProcessedAnnexCodeOnce = false;
 694 #endif
 695
 696     updateRx(currentTime);
 697
 698     if (shouldProcessRx(currentTime)) {
 699         processRx();
 700
 701 #ifdef BARO
 702         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 703         if (haveProcessedAnnexCodeOnce) {
 704             if (sensors(SENSOR_BARO)) {
 705                 updateAltHoldState();
 706             }
 707         }
 708 #endif
 709
 710 #ifdef SONAR
 711         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 712         if (haveProcessedAnnexCodeOnce) {
 713             if (sensors(SENSOR_SONAR)) {
 714                 updateSonarAltHoldState();
 715             }
 716         }
 717 #endif
 718
 719     } else {
 720         // not processing rx this iteration
 721         executePeriodicTasks();
 722
 723         // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 724         // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 725         // change this based on available hardware
 726 #ifdef GPS
 727         if (feature(FEATURE_GPS)) {
 728             gpsThread();
 729         }
 730 #endif
 731     }
 732
 733     currentTime = micros();
 734     if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
 735         loopTime = currentTime + masterConfig.looptime;
 736
 737         imuUpdate(&currentProfile->accelerometerTrims);
 738
 739         // Measure loop rate just after reading the sensors
 740         currentTime = micros();
 741         cycleTime = (int32_t)(currentTime - previousTime);
 742         previousTime = currentTime;
 743
 744         // Gyro Low Pass
 745         if (currentProfile->pidProfile.gyro_cut_hz) {
 746             int axis;
 747             static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
 748
 749             for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 750                     gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz);
 751             }
 752         }
 753
 754         annexCode();
 755 #if defined(BARO) || defined(SONAR)
 756         haveProcessedAnnexCodeOnce = true;
 757 #endif
 758
 759 #ifdef AUTOTUNE
 760         updateAutotuneState();
 761 #endif
 762
 763 #ifdef MAG
 764         if (sensors(SENSOR_MAG)) {
 765                 updateMagHold();
 766         }
 767 #endif
 768
 769 #if defined(BARO) || defined(SONAR)
 770         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 771             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 772                 applyAltHold(&masterConfig.airplaneConfig);
 773             }
 774         }
 775 #endif
 776
 777         // If we're armed, at minimum throttle, and we do arming via the
 778         // sticks, do not process yaw input from the rx.  We do this so the
 779         // motors do not spin up while we are trying to arm or disarm.
 780         // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 781         if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 782 #ifndef USE_QUAD_MIXER_ONLY
 783                 && !(masterConfig.mixerMode == MIXER_TRI && masterConfig.mixerConfig.tri_unarmed_servo)
 784                 && masterConfig.mixerMode != MIXER_AIRPLANE
 785                 && masterConfig.mixerMode != MIXER_FLYING_WING
 786 #endif
 787         ) {
 788             rcCommand[YAW] = 0;
 789         }
 790
 791
 792         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 793             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
 794         }
 795
 796 #ifdef GPS
 797         if (sensors(SENSOR_GPS)) {
 798             if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 799                 updateGpsStateForHomeAndHoldMode();
 800             }
 801         }
 802 #endif
 803
 804         // PID - note this is function pointer set by setPIDController()
 805         pid_controller(
 806             &currentProfile->pidProfile,
 807             currentControlRateProfile,
 808             masterConfig.max_angle_inclination,
 809             &currentProfile->accelerometerTrims,
 810             &masterConfig.rxConfig
 811         );
 812
 813         mixTable();
 814
 815 #ifdef USE_SERVOS
 816         filterServos();
 817         writeServos();
 818 #endif
 819
 820         if (motorControlEnable) {
 821             writeMotors();
 822         }
 823
 824 #ifdef BLACKBOX
 825         if (!cliMode && feature(FEATURE_BLACKBOX)) {
 826             handleBlackbox();
 827         }
 828 #endif
 829     }
 830
 831 #ifdef TELEMETRY
 832     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 833         telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 834     }
 835 #endif
 836
 837 #ifdef LED_STRIP
 838     if (feature(FEATURE_LED_STRIP)) {
 839         updateLedStrip();
 840     }
 841 #endif
 842 }