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