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