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 #include "common/filter.h"
  29
  30 #include "drivers/sensor.h"
  31 #include "drivers/accgyro.h"
  32 #include "drivers/compass.h"
  33 #include "drivers/light_led.h"
  34
  35 #include "drivers/gpio.h"
  36 #include "drivers/system.h"
  37 #include "drivers/serial.h"
  38 #include "drivers/timer.h"
  39 #include "drivers/pwm_rx.h"
  40 #include "drivers/gyro_sync.h"
  41
  42 #include "sensors/sensors.h"
  43 #include "sensors/boardalignment.h"
  44 #include "sensors/sonar.h"
  45 #include "sensors/compass.h"
  46 #include "sensors/acceleration.h"
  47 #include "sensors/barometer.h"
  48 #include "sensors/gyro.h"
  49 #include "sensors/battery.h"
  50
  51 #include "io/beeper.h"
  52 #include "io/display.h"
  53 #include "io/escservo.h"
  54 #include "io/rc_controls.h"
  55 #include "io/rc_curves.h"
  56 #include "io/gimbal.h"
  57 #include "io/gps.h"
  58 #include "io/ledstrip.h"
  59 #include "io/serial.h"
  60 #include "io/serial_cli.h"
  61 #include "io/serial_msp.h"
  62 #include "io/statusindicator.h"
  63
  64 #include "rx/rx.h"
  65 #include "rx/msp.h"
  66
  67 #include "telemetry/telemetry.h"
  68 #include "blackbox/blackbox.h"
  69
  70 #include "flight/mixer.h"
  71 #include "flight/pid.h"
  72 #include "flight/imu.h"
  73 #include "flight/altitudehold.h"
  74 #include "flight/failsafe.h"
  75 #include "flight/autotune.h"
  76 #include "flight/navigation.h"
  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 /* VBAT monitoring interval (in microseconds) - 1s*/
  92 #define VBATINTERVAL (6 * 3500)
  93 /* IBat monitoring interval (in microseconds) - 6 default looptimes */
  94 #define IBATINTERVAL (6 * 3500)
  95 #define GYRO_WATCHDOG_DELAY 500  // Watchdog for boards without interrupt for gyro
  96 #define LOOP_DEADBAND 400 // Dead band for loop to modify to rcInterpolationFactor in RC Filtering for unstable looptimes
  97
  98 uint32_t currentTime = 0;
  99 uint32_t previousTime = 0;
 100 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
 101 float dT;
 102
 103 int16_t magHold;
 104 int16_t headFreeModeHold;
 105
 106 uint8_t motorControlEnable = false;
 107
 108 int16_t telemTemperature1;      // gyro sensor temperature
 109 static uint32_t disarmAt;     // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
 110
 111 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 112
 113 static bool isRXdataNew;
 114
 115 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
 116         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);            // pid controller function prototype
 117
 118 extern pidControllerFuncPtr pid_controller;
 119
 120 void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
 121 {
 122     currentProfile->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
 123     currentProfile->accelerometerTrims.values.pitch += rollAndPitchTrimsDelta->values.pitch;
 124
 125     saveConfigAndNotify();
 126 }
 127
 128 #ifdef AUTOTUNE
 129
 130 void updateAutotuneState(void)
 131 {
 132     static bool landedAfterAutoTuning = false;
 133     static bool autoTuneWasUsed = false;
 134
 135     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {
 136         if (!FLIGHT_MODE(AUTOTUNE_MODE)) {
 137             if (ARMING_FLAG(ARMED)) {
 138                 if (isAutotuneIdle() || landedAfterAutoTuning) {
 139                     autotuneReset();
 140                     landedAfterAutoTuning = false;
 141                 }
 142                 autotuneBeginNextPhase(&currentProfile->pidProfile);
 143                 ENABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 144                 autoTuneWasUsed = true;
 145             } else {
 146                 if (havePidsBeenUpdatedByAutotune()) {
 147                     saveConfigAndNotify();
 148                     autotuneReset();
 149                 }
 150             }
 151         }
 152         return;
 153     }
 154
 155     if (FLIGHT_MODE(AUTOTUNE_MODE)) {
 156         autotuneEndPhase();
 157         DISABLE_FLIGHT_MODE(AUTOTUNE_MODE);
 158     }
 159
 160     if (!ARMING_FLAG(ARMED) && autoTuneWasUsed) {
 161         landedAfterAutoTuning = true;
 162     }
 163 }
 164 #endif
 165
 166 bool isCalibrating()
 167 {
 168 #ifdef BARO
 169     if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
 170         return true;
 171     }
 172 #endif
 173
 174     // Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
 175
 176     return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 177 }
 178
 179 void annexCode(void)
 180 {
 181     int32_t tmp, tmp2;
 182     int32_t axis, prop1 = 0, prop2;
 183
 184     static uint32_t vbatLastServiced = 0;
 185     static uint32_t ibatLastServiced = 0;
 186     // PITCH & ROLL only dynamic PID adjustment,  depending on throttle value
 187     if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
 188         prop2 = 100;
 189     } else {
 190         if (rcData[THROTTLE] < 2000) {
 191             prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
 192         } else {
 193             prop2 = 100 - currentControlRateProfile->dynThrPID;
 194         }
 195     }
 196
 197     for (axis = 0; axis < 3; axis++) {
 198         tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 199         if (axis == ROLL || axis == PITCH) {
 200             if (currentProfile->rcControlsConfig.deadband) {
 201                 if (tmp > currentProfile->rcControlsConfig.deadband) {
 202                     tmp -= currentProfile->rcControlsConfig.deadband;
 203                 } else {
 204                     tmp = 0;
 205                 }
 206             }
 207
 208             tmp2 = tmp / 100;
 209             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 210             prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
 211             prop1 = (uint16_t)prop1 * prop2 / 100;
 212         } else if (axis == YAW) {
 213             if (currentProfile->rcControlsConfig.yaw_deadband) {
 214                 if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
 215                     tmp -= currentProfile->rcControlsConfig.yaw_deadband;
 216                 } else {
 217                     tmp = 0;
 218                 }
 219             }
 220             tmp2 = tmp / 100;
 221             rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
 222             prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
 223         }
 224         // FIXME axis indexes into pids.  use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
 225         dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
 226         dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
 227         dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
 228
 229         // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
 230         if (axis == YAW) {
 231             PIDweight[axis] = 100;
 232         }
 233         else {
 234             PIDweight[axis] = prop2;
 235         }
 236
 237         if (rcData[axis] < masterConfig.rxConfig.midrc)
 238             rcCommand[axis] = -rcCommand[axis];
 239     }
 240
 241     tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 242     tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
 243     tmp2 = tmp / 100;
 244     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 245
 246     if (FLIGHT_MODE(HEADFREE_MODE)) {
 247         float radDiff = degreesToRadians(heading - headFreeModeHold);
 248         float cosDiff = cos_approx(radDiff);
 249         float sinDiff = sin_approx(radDiff);
 250         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 251         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 252         rcCommand[PITCH] = rcCommand_PITCH;
 253     }
 254
 255     if (feature(FEATURE_VBAT)) {
 256         if ((int32_t)(currentTime - vbatLastServiced) >= VBATINTERVAL) {
 257             vbatLastServiced = currentTime;
 258             updateBattery();
 259         }
 260     }
 261
 262     if (feature(FEATURE_CURRENT_METER)) {
 263         int32_t ibatTimeSinceLastServiced = (int32_t) (currentTime - ibatLastServiced);
 264
 265         if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
 266             ibatLastServiced = currentTime;
 267             updateCurrentMeter((ibatTimeSinceLastServiced / 1000), &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 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 // Gyro Low Pass
 690 void filterGyro(void) {
 691     int axis;
 692     static float dTGyro;
 693     static filterStatePt1_t gyroADCState[XYZ_AXIS_COUNT];
 694
 695     if (!dTGyro) {
 696         dTGyro = (float)targetLooptime * 0.000001f;
 697     }
 698
 699     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 700         gyroADC[axis] = filterApplyPt1(gyroADC[axis], &gyroADCState[axis], currentProfile->pidProfile.gyro_cut_hz, dTGyro);
 701     }
 702 }
 703 void getArmingChannel(modeActivationCondition_t *modeActivationConditions, uint8_t *armingChannel) {
 704     for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) {
 705         modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index];
 706         if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
 707             *armingChannel = modeActivationCondition->auxChannelIndex +  NON_AUX_CHANNEL_COUNT;
 708             break;
 709         }
 710     }
 711 }
 712
 713 void filterRc(void){
 714     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
 715     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
 716     static int16_t loop[5] = { 0, 0, 0, 0, 0 };
 717     static int16_t factor, rcInterpolationFactor, loopAvg;
 718     static uint32_t rxRefreshRate;
 719     static int16_t lastAux, deltaAux;                            // last arming AUX position and delta for arming AUX
 720         static uint8_t auxChannelToFilter;                           // AUX channel used for arming needs filtering when used
 721         static int loopCount;
 722
 723     // Set RC refresh rate for sampling and channels to filter
 724     if (!rxRefreshRate) {
 725         if (feature(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM)) {
 726             rxRefreshRate = 20000;
 727
 728             // AUX Channels to filter to replace PPM/PWM averaging
 729             getArmingChannel(currentProfile->modeActivationConditions,&auxChannelToFilter);
 730
 731         }
 732
 733         // TODO Are there more different refresh rates?
 734         else {
 735             switch (masterConfig.rxConfig.serialrx_provider) {
 736                 case SERIALRX_SPEKTRUM1024:
 737                     rxRefreshRate = 22000;
 738                     break;
 739                 case SERIALRX_SPEKTRUM2048:
 740                     rxRefreshRate = 11000;
 741                     break;
 742                 case SERIALRX_SBUS:
 743                     rxRefreshRate = 11000;
 744                     break;
 745                 default:
 746                     rxRefreshRate = 10000;
 747                     break;
 748             }
 749         }
 750
 751         rcInterpolationFactor = 1; // Initial Factor before looptime average is calculated
 752
 753     }
 754
 755     // Averaging of cycleTime for more precise sampling
 756     loop[loopCount] = cycleTime;
 757     loopCount++;
 758
 759     // Start recalculating new rcInterpolationFactor every 5 loop iterations
 760     if (loopCount > 4) {
 761         uint16_t tmp = (loop[0] + loop[1] + loop[2] + loop[3] + loop[4]) / 5;
 762
 763         // Jitter tolerance to prevent rcInterpolationFactor jump too much
 764         if (tmp > (loopAvg + LOOP_DEADBAND) || tmp < (loopAvg - LOOP_DEADBAND))  {
 765             loopAvg = tmp;
 766             rcInterpolationFactor = rxRefreshRate / loopAvg + 1;
 767         }
 768
 769         loopCount = 0;
 770      }
 771
 772     if (isRXdataNew) {
 773         for (int channel=0; channel < 4; channel++) {
 774                 deltaRC[channel] = rcData[channel] -  (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
 775             lastCommand[channel] = rcData[channel];
 776         }
 777
 778         // Read AUX channel (arm/disarm guard enhancement)
 779         if (auxChannelToFilter) {
 780             deltaAux = rcData[auxChannelToFilter] - (lastAux - deltaAux * factor/rcInterpolationFactor);
 781             lastAux = rcData[auxChannelToFilter];
 782         }
 783
 784         isRXdataNew = false;
 785         factor = rcInterpolationFactor - 1;
 786         }
 787
 788     else {
 789         factor--;
 790     }
 791
 792     // Interpolate steps of rcData
 793     if (factor > 0) {
 794         for (int channel=0; channel < 4; channel++) {
 795             rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
 796          }
 797
 798         // Interpolate steps of Aux
 799         if (auxChannelToFilter) {
 800             rcData[auxChannelToFilter] = lastAux - deltaAux * factor/rcInterpolationFactor;
 801         }
 802     }
 803
 804     else {
 805         factor = 0;
 806     }
 807 }
 808
 809 // Function for loop trigger
 810 bool runLoop(uint32_t loopTime) {
 811         bool loopTrigger = false;
 812
 813     if (masterConfig.syncGyroToLoop) {
 814         if (ARMING_FLAG(ARMED)) {
 815             if (gyroSyncCheckUpdate() || (int32_t)(currentTime - (loopTime + GYRO_WATCHDOG_DELAY)) >= 0) {
 816                 loopTrigger = true;
 817             }
 818         }
 819         // Blheli arming workaround (stable looptime prior to arming)
 820         else if (!ARMING_FLAG(ARMED) && ((int32_t)(currentTime - loopTime) >= 0)) {
 821                 loopTrigger = true;
 822         }
 823     }
 824
 825     else if ((int32_t)(currentTime - loopTime) >= 0){
 826         loopTrigger = true;
 827     }
 828
 829     return loopTrigger;
 830 }
 831
 832 void loop(void)
 833 {
 834     static uint32_t loopTime;
 835 #if defined(BARO) || defined(SONAR)
 836     static bool haveProcessedAnnexCodeOnce = false;
 837 #endif
 838
 839     updateRx(currentTime);
 840
 841     if (shouldProcessRx(currentTime)) {
 842         processRx();
 843         isRXdataNew = true;
 844
 845 #ifdef BARO
 846         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 847         if (haveProcessedAnnexCodeOnce) {
 848             if (sensors(SENSOR_BARO)) {
 849                 updateAltHoldState();
 850             }
 851         }
 852 #endif
 853
 854 #ifdef SONAR
 855         // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
 856         if (haveProcessedAnnexCodeOnce) {
 857             if (sensors(SENSOR_SONAR)) {
 858                 updateSonarAltHoldState();
 859             }
 860         }
 861 #endif
 862
 863     } else {
 864         // not processing rx this iteration
 865         executePeriodicTasks();
 866
 867         // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 868         // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 869         // change this based on available hardware
 870 #ifdef GPS
 871         if (feature(FEATURE_GPS)) {
 872             gpsThread();
 873         }
 874 #endif
 875     }
 876
 877     currentTime = micros();
 878     if (runLoop(loopTime)) {
 879
 880         loopTime = currentTime + targetLooptime;
 881
 882         imuUpdate(&currentProfile->accelerometerTrims);
 883
 884         // Measure loop rate just after reading the sensors
 885         currentTime = micros();
 886         cycleTime = (int32_t)(currentTime - previousTime);
 887         previousTime = currentTime;
 888
 889         dT = (float)cycleTime * 0.000001f;
 890
 891         if (currentProfile->pidProfile.gyro_cut_hz) {
 892             filterGyro();
 893         }
 894
 895         if (masterConfig.rcSmoothing) {
 896             filterRc();
 897         }
 898
 899         annexCode();
 900 #if defined(BARO) || defined(SONAR)
 901         haveProcessedAnnexCodeOnce = true;
 902 #endif
 903
 904 #ifdef AUTOTUNE
 905         updateAutotuneState();
 906 #endif
 907
 908 #ifdef MAG
 909         if (sensors(SENSOR_MAG)) {
 910                 updateMagHold();
 911         }
 912 #endif
 913
 914 #if defined(BARO) || defined(SONAR)
 915         if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
 916             if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
 917                 applyAltHold(&masterConfig.airplaneConfig);
 918             }
 919         }
 920 #endif
 921
 922         // If we're armed, at minimum throttle, and we do arming via the
 923         // sticks, do not process yaw input from the rx.  We do this so the
 924         // motors do not spin up while we are trying to arm or disarm.
 925         // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
 926         if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck
 927 #ifndef USE_QUAD_MIXER_ONLY
 928                 && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo)
 929                 && masterConfig.mixerMode != MIXER_AIRPLANE
 930                 && masterConfig.mixerMode != MIXER_FLYING_WING
 931 #endif
 932         ) {
 933             rcCommand[YAW] = 0;
 934         }
 935
 936
 937         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
 938             rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value);
 939         }
 940
 941 #ifdef GPS
 942         if (sensors(SENSOR_GPS)) {
 943             if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) {
 944                 updateGpsStateForHomeAndHoldMode();
 945             }
 946         }
 947 #endif
 948
 949         // PID - note this is function pointer set by setPIDController()
 950         pid_controller(
 951             &currentProfile->pidProfile,
 952             currentControlRateProfile,
 953             masterConfig.max_angle_inclination,
 954             &currentProfile->accelerometerTrims,
 955             &masterConfig.rxConfig
 956         );
 957
 958         mixTable();
 959
 960 #ifdef USE_SERVOS
 961         filterServos();
 962         writeServos();
 963 #endif
 964
 965         if (motorControlEnable) {
 966             writeMotors();
 967         }
 968
 969 #ifdef BLACKBOX
 970         if (!cliMode && feature(FEATURE_BLACKBOX)) {
 971             handleBlackbox();
 972         }
 973 #endif
 974     }
 975
 976 #ifdef TELEMETRY
 977     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 978         telemetryProcess(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
 979     }
 980 #endif
 981
 982 #ifdef LED_STRIP
 983     if (feature(FEATURE_LED_STRIP)) {
 984         updateLedStrip();
 985     }
 986 #endif
 987 }