src/mw.c

   1 #include "board.h"
   2 #include "mw.h"
   3
   4 // June 2013     V2.2-dev
   5
   6 flags_t f;
   7 int16_t debug[4];
   8 uint8_t toggleBeep = 0;
   9 uint32_t currentTime = 0;
  10 uint32_t previousTime = 0;
  11 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
  12 int16_t headFreeModeHold;
  13
  14 int16_t annex650_overrun_count = 0;
  15 uint8_t vbat;                   // battery voltage in 0.1V steps
  16 int16_t telemTemperature1;      // gyro sensor temperature
  17
  18 int16_t failsafeCnt = 0;
  19 int16_t failsafeEvents = 0;
  20 int16_t rcData[8] = { 1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502 }; // interval [1000;2000]
  21 int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
  22 int16_t lookupPitchRollRC[6];   // lookup table for expo & RC rate PITCH+ROLL
  23 int16_t lookupThrottleRC[11];   // lookup table for expo & mid THROTTLE
  24 uint16_t rssi;                  // range: [0;1023]
  25 rcReadRawDataPtr rcReadRawFunc = NULL;  // receive data from default (pwm/ppm) or additional (spek/sbus/?? receiver drivers)
  26
  27 static void pidMultiWii(void);
  28 static void pidRewrite(void);
  29 pidControllerFuncPtr pid_controller = pidMultiWii; // which pid controller are we using, defaultMultiWii
  30
  31 uint8_t dynP8[3], dynI8[3], dynD8[3];
  32 uint8_t rcOptions[CHECKBOXITEMS];
  33
  34 int16_t axisPID[3];
  35
  36 // **********************
  37 // GPS
  38 // **********************
  39 int32_t GPS_coord[2];
  40 int32_t GPS_home[2];
  41 int32_t GPS_hold[2];
  42 uint8_t GPS_numSat;
  43 uint16_t GPS_distanceToHome;        // distance to home point in meters
  44 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
  45 uint16_t GPS_altitude, GPS_speed;   // altitude in 0.1m and speed in 0.1m/s
  46 uint8_t GPS_update = 0;             // it's a binary toogle to distinct a GPS position update
  47 int16_t GPS_angle[2] = { 0, 0 };    // it's the angles that must be applied for GPS correction
  48 uint16_t GPS_ground_course = 0;     // degrees * 10
  49 uint8_t GPS_Present = 0;            // Checksum from Gps serial
  50 uint8_t GPS_Enable = 0;
  51 int16_t nav[2];
  52 int16_t nav_rated[2];               // Adding a rate controller to the navigation to make it smoother
  53 int8_t nav_mode = NAV_MODE_NONE;    // Navigation mode
  54 uint8_t  GPS_numCh;                 // Number of channels
  55 uint8_t  GPS_svinfo_chn[16];        // Channel number
  56 uint8_t  GPS_svinfo_svid[16];       // Satellite ID
  57 uint8_t  GPS_svinfo_quality[16];    // Bitfield Qualtity
  58 uint8_t  GPS_svinfo_cno[16];        // Carrier to Noise Ratio (Signal Strength)
  59
  60 // Automatic ACC Offset Calibration
  61 uint16_t InflightcalibratingA = 0;
  62 int16_t AccInflightCalibrationArmed;
  63 uint16_t AccInflightCalibrationMeasurementDone = 0;
  64 uint16_t AccInflightCalibrationSavetoEEProm = 0;
  65 uint16_t AccInflightCalibrationActive = 0;
  66
  67 // Battery monitoring stuff
  68 uint8_t batteryCellCount = 3;       // cell count
  69 uint16_t batteryWarningVoltage;     // annoying buzzer after this one, battery ready to be dead
  70
  71 void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
  72 {
  73     uint8_t i, r;
  74
  75     for (r = 0; r < repeat; r++) {
  76         for (i = 0; i < num; i++) {
  77             LED0_TOGGLE;            // switch LEDPIN state
  78             BEEP_ON;
  79             delay(wait);
  80             BEEP_OFF;
  81         }
  82         delay(60);
  83     }
  84 }
  85
  86 #define BREAKPOINT 1500
  87
  88 // this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
  89 void annexCode(void)
  90 {
  91     static uint32_t calibratedAccTime;
  92     uint16_t tmp, tmp2;
  93     static uint8_t buzzerFreq;  //delay between buzzer ring
  94     static uint8_t vbatTimer = 0;
  95     uint8_t axis, prop1, prop2;
  96     static uint8_t ind = 0;
  97     uint16_t vbatRaw = 0;
  98     static uint16_t vbatRawArray[8];
  99     uint8_t i;
 100
 101     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
 102     if (rcData[THROTTLE] < BREAKPOINT) {
 103         prop2 = 100;
 104     } else {
 105         if (rcData[THROTTLE] < 2000) {
 106             prop2 = 100 - (uint16_t) cfg.dynThrPID * (rcData[THROTTLE] - BREAKPOINT) / (2000 - BREAKPOINT);
 107         } else {
 108             prop2 = 100 - cfg.dynThrPID;
 109         }
 110     }
 111
 112     for (axis = 0; axis < 3; axis++) {
 113         tmp = min(abs(rcData[axis] - mcfg.midrc), 500);
 114         if (axis != 2) {        // ROLL & PITCH
 115             if (cfg.deadband) {
 116                 if (tmp > cfg.deadband) {
 117                     tmp -= cfg.deadband;
 118                 } else {
 119                     tmp = 0;
 120                 }
 121             }
 122
 123             tmp2 = tmp / 100;
 124             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 125             prop1 = 100 - (uint16_t) cfg.rollPitchRate * tmp / 500;
 126             prop1 = (uint16_t) prop1 *prop2 / 100;
 127         } else {                // YAW
 128             if (cfg.yawdeadband) {
 129                 if (tmp > cfg.yawdeadband) {
 130                     tmp -= cfg.yawdeadband;
 131                 } else {
 132                     tmp = 0;
 133                 }
 134             }
 135             rcCommand[axis] = tmp;
 136             prop1 = 100 - (uint16_t) cfg.yawRate * tmp / 500;
 137         }
 138         dynP8[axis] = (uint16_t) cfg.P8[axis] * prop1 / 100;
 139         dynI8[axis] = (uint16_t) cfg.I8[axis] * prop1 / 100;
 140         dynD8[axis] = (uint16_t) cfg.D8[axis] * prop1 / 100;
 141         if (rcData[axis] < mcfg.midrc)
 142             rcCommand[axis] = -rcCommand[axis];
 143     }
 144
 145     tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000);
 146     tmp = (uint32_t) (tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck);       // [MINCHECK;2000] -> [0;1000]
 147     tmp2 = tmp / 100;
 148     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 149
 150     if(f.HEADFREE_MODE) {
 151         float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
 152         float cosDiff = cosf(radDiff);
 153         float sinDiff = sinf(radDiff);
 154         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 155         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 156         rcCommand[PITCH] = rcCommand_PITCH;
 157     }
 158
 159     if (feature(FEATURE_VBAT)) {
 160         if (!(++vbatTimer % VBATFREQ)) {
 161             vbatRawArray[(ind++) % 8] = adcGetChannel(ADC_BATTERY);
 162             for (i = 0; i < 8; i++)
 163                 vbatRaw += vbatRawArray[i];
 164             vbat = batteryAdcToVoltage(vbatRaw / 8);
 165         }
 166         if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
 167             buzzerFreq = 0;
 168         } else
 169             buzzerFreq = 4;     // low battery
 170     }
 171
 172     buzzer(buzzerFreq);         // external buzzer routine that handles buzzer events globally now
 173
 174     if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) {      // Calibration phasis
 175         LED0_TOGGLE;
 176     } else {
 177         if (f.ACC_CALIBRATED)
 178             LED0_OFF;
 179         if (f.ARMED)
 180             LED0_ON;
 181         // This will switch to/from 9600 or 115200 baud depending on state. Of course, it should only do it on changes.
 182         if (feature(FEATURE_TELEMETRY))
 183             initTelemetry(f.ARMED);
 184     }
 185
 186 #ifdef LEDRING
 187     if (feature(FEATURE_LED_RING)) {
 188         static uint32_t LEDTime;
 189         if ((int32_t)(currentTime - LEDTime) >= 0) {
 190             LEDTime = currentTime + 50000;
 191             ledringState();
 192         }
 193     }
 194 #endif
 195
 196     if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
 197         if (!f.SMALL_ANGLES_25) {
 198             f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
 199             LED0_TOGGLE;
 200             calibratedAccTime = currentTime + 500000;
 201         } else {
 202             f.ACC_CALIBRATED = 1;
 203         }
 204     }
 205
 206     serialCom();
 207
 208     if (sensors(SENSOR_GPS)) {
 209         static uint32_t GPSLEDTime;
 210         if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) {
 211             GPSLEDTime = currentTime + 150000;
 212             LED1_TOGGLE;
 213         }
 214     }
 215
 216     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 217     if (gyro.temperature)
 218         gyro.temperature(&telemTemperature1);
 219     else {
 220         // TODO MCU temp
 221     }
 222 }
 223
 224 uint16_t pwmReadRawRC(uint8_t chan)
 225 {
 226     uint16_t data;
 227
 228     data = pwmRead(mcfg.rcmap[chan]);
 229     if (data < 750 || data > 2250)
 230         data = mcfg.midrc;
 231
 232     return data;
 233 }
 234
 235 void computeRC(void)
 236 {
 237     static int16_t rcData4Values[8][4], rcDataMean[8];
 238     static uint8_t rc4ValuesIndex = 0;
 239     uint8_t chan, a;
 240
 241     rc4ValuesIndex++;
 242     for (chan = 0; chan < 8; chan++) {
 243         rcData4Values[chan][rc4ValuesIndex % 4] = rcReadRawFunc(chan);
 244         rcDataMean[chan] = 0;
 245         for (a = 0; a < 4; a++)
 246             rcDataMean[chan] += rcData4Values[chan][a];
 247
 248         rcDataMean[chan] = (rcDataMean[chan] + 2) / 4;
 249         if (rcDataMean[chan] < rcData[chan] - 3)
 250             rcData[chan] = rcDataMean[chan] + 2;
 251         if (rcDataMean[chan] > rcData[chan] + 3)
 252             rcData[chan] = rcDataMean[chan] - 2;
 253     }
 254 }
 255
 256 static void mwArm(void)
 257 {
 258     if (calibratingG == 0 && f.ACC_CALIBRATED) {
 259         // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
 260         // TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
 261         if (!f.ARMED) {         // arm now!
 262             f.ARMED = 1;
 263             headFreeModeHold = heading;
 264         }
 265     } else if (!f.ARMED) {
 266         blinkLED(2, 255, 1);
 267     }
 268 }
 269
 270 static void mwDisarm(void)
 271 {
 272     if (f.ARMED)
 273         f.ARMED = 0;
 274 }
 275
 276 static void mwVario(void)
 277 {
 278
 279 }
 280
 281 static int32_t errorGyroI[3] = { 0, 0, 0 };
 282 static int32_t errorAngleI[2] = { 0, 0 };
 283
 284 static void pidMultiWii(void)
 285 {
 286     int axis, prop;
 287     int32_t error, errorAngle;
 288     int32_t PTerm, ITerm, PTermACC, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
 289     static int16_t lastGyro[3] = { 0, 0, 0 };
 290     static int32_t delta1[3], delta2[3];
 291     int32_t deltaSum;
 292     int32_t delta;
 293
 294     // **** PITCH & ROLL & YAW PID ****
 295     prop = max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])); // range [0;500]
 296     for (axis = 0; axis < 3; axis++) {
 297         if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2) { // MODE relying on ACC
 298             // 50 degrees max inclination
 299             errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis];
 300             PTermACC = errorAngle * cfg.P8[PIDLEVEL] / 100; // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
 301             PTermACC = constrain(PTermACC, -cfg.D8[PIDLEVEL] * 5, +cfg.D8[PIDLEVEL] * 5);
 302
 303             errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp
 304             ITermACC = (errorAngleI[axis] * cfg.I8[PIDLEVEL]) >> 12;
 305         }
 306         if (!f.ANGLE_MODE || f.HORIZON_MODE || axis == 2) { // MODE relying on GYRO or YAW axis
 307             error = (int32_t)rcCommand[axis] * 10 * 8 / cfg.P8[axis];
 308             error -= gyroData[axis];
 309
 310             PTermGYRO = rcCommand[axis];
 311
 312             errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000); // WindUp
 313             if (abs(gyroData[axis]) > 640)
 314                 errorGyroI[axis] = 0;
 315             ITermGYRO = (errorGyroI[axis] / 125 * cfg.I8[axis]) >> 6;
 316         }
 317         if (f.HORIZON_MODE && axis < 2) {
 318             PTerm = (PTermACC * (500 - prop) + PTermGYRO * prop) / 500;
 319             ITerm = (ITermACC * (500 - prop) + ITermGYRO * prop) / 500;
 320         } else {
 321             if (f.ANGLE_MODE && axis < 2) {
 322                 PTerm = PTermACC;
 323                 ITerm = ITermACC;
 324             } else {
 325                 PTerm = PTermGYRO;
 326                 ITerm = ITermGYRO;
 327             }
 328         }
 329
 330         PTerm -= (int32_t)gyroData[axis] * dynP8[axis] / 10 / 8; // 32 bits is needed for calculation
 331         delta = gyroData[axis] - lastGyro[axis];
 332         lastGyro[axis] = gyroData[axis];
 333         deltaSum = delta1[axis] + delta2[axis] + delta;
 334         delta2[axis] = delta1[axis];
 335         delta1[axis] = delta;
 336         DTerm = (deltaSum * dynD8[axis]) / 32;
 337         axisPID[axis] = PTerm + ITerm - DTerm;
 338     }
 339 }
 340
 341 #define GYRO_I_MAX 256
 342
 343 static void pidRewrite(void)
 344 {
 345     int32_t errorAngle;
 346     int axis;
 347     int32_t delta, deltaSum;
 348     static int32_t delta1[3], delta2[3];
 349     int32_t PTerm, ITerm, DTerm;
 350     static int32_t lastError[3] = { 0, 0, 0 };
 351     int32_t AngleRateTmp, RateError;
 352
 353     // ----------PID controller----------
 354     for (axis = 0; axis < 3; axis++) {
 355         // -----Get the desired angle rate depending on flight mode
 356         if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis < 2 ) { // MODE relying on ACC
 357             // calculate error and limit the angle to 50 degrees max inclination
 358             errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -500, +500) - angle[axis] + cfg.angleTrim[axis]; // 16 bits is ok here
 359         }
 360         if (axis == 2) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
 361             AngleRateTmp = (((int32_t)(cfg.yawRate + 27) * rcCommand[2]) >> 5);
 362          } else {
 363             if (!f.ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
 364                 AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
 365                 if (f.HORIZON_MODE) {
 366                     // mix up angle error to desired AngleRateTmp to add a little auto-level feel
 367                     AngleRateTmp += (errorAngle * cfg.I8[PIDLEVEL]) >> 8;
 368                 }
 369             } else { // it's the ANGLE mode - control is angle based, so control loop is needed
 370                 AngleRateTmp = (errorAngle * cfg.P8[PIDLEVEL]) >> 4;
 371             }
 372         }
 373
 374         // --------low-level gyro-based PID. ----------
 375         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
 376         // -----calculate scaled error.AngleRates
 377         // multiplication of rcCommand corresponds to changing the sticks scaling here
 378         RateError = AngleRateTmp - gyroData[axis];
 379
 380         // -----calculate P component
 381         PTerm = (RateError * cfg.P8[axis]) >> 7;
 382         // -----calculate I component
 383         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
 384         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
 385         // Time correction (to avoid different I scaling for different builds based on average cycle time)
 386         // is normalized to cycle time = 2048.
 387         errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * cfg.I8[axis];
 388
 389         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 390         // I coefficient (I8) moved before integration to make limiting independent from PID settings
 391         errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t)-GYRO_I_MAX << 13, (int32_t)+GYRO_I_MAX << 13);
 392         ITerm = errorGyroI[axis] >> 13;
 393
 394         //-----calculate D-term
 395         delta = RateError - lastError[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
 396         lastError[axis] = RateError;
 397
 398         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
 399         // would be scaled by different dt each time. Division by dT fixes that.
 400         delta = (delta * ((uint16_t)0xFFFF / (cycleTime >> 4))) >> 6;
 401         // add moving average here to reduce noise
 402         deltaSum = delta1[axis] + delta2[axis] + delta;
 403         delta2[axis] = delta1[axis];
 404         delta1[axis] = delta;
 405         DTerm = (deltaSum * cfg.D8[axis]) >> 8;
 406
 407         // -----calculate total PID output
 408         axisPID[axis] = PTerm + ITerm + DTerm;
 409     }
 410 }
 411
 412 void setPIDController(int type)
 413 {
 414     switch (type) {
 415         case 0:
 416         default:
 417             pid_controller = pidMultiWii;
 418             break;
 419         case 1:
 420             pid_controller = pidRewrite;
 421             break;
 422     }
 423 }
 424
 425 void loop(void)
 426 {
 427     static uint8_t rcDelayCommand;      // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
 428     static uint8_t rcSticks;            // this hold sticks position for command combos
 429     uint8_t stTmp = 0;
 430     int i;
 431     static uint32_t rcTime = 0;
 432     static int16_t initialThrottleHold;
 433     static uint32_t loopTime;
 434     uint16_t auxState = 0;
 435     static uint8_t GPSNavReset = 1;
 436
 437     // this will return false if spektrum is disabled. shrug.
 438     if (spektrumFrameComplete())
 439         computeRC();
 440
 441     if ((int32_t)(currentTime - rcTime) >= 0) { // 50Hz
 442         rcTime = currentTime + 20000;
 443         // TODO clean this up. computeRC should handle this check
 444         if (!feature(FEATURE_SPEKTRUM))
 445             computeRC();
 446
 447         // Failsafe routine
 448         if (feature(FEATURE_FAILSAFE)) {
 449             if (failsafeCnt > (5 * cfg.failsafe_delay) && f.ARMED) { // Stabilize, and set Throttle to specified level
 450                 for (i = 0; i < 3; i++)
 451                     rcData[i] = mcfg.midrc;      // after specified guard time after RC signal is lost (in 0.1sec)
 452                 rcData[THROTTLE] = cfg.failsafe_throttle;
 453                 if (failsafeCnt > 5 * (cfg.failsafe_delay + cfg.failsafe_off_delay)) {  // Turn OFF motors after specified Time (in 0.1sec)
 454                     mwDisarm();             // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
 455                     f.OK_TO_ARM = 0;        // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
 456                 }
 457                 failsafeEvents++;
 458             }
 459             if (failsafeCnt > (5 * cfg.failsafe_delay) && !f.ARMED) {  // Turn off "Ok To arm to prevent the motors from spinning after repowering the RX with low throttle and aux to arm
 460                 mwDisarm();         // This will prevent the copter to automatically rearm if failsafe shuts it down and prevents
 461                 f.OK_TO_ARM = 0;    // to restart accidentely by just reconnect to the tx - you will have to switch off first to rearm
 462             }
 463             failsafeCnt++;
 464         }
 465         // end of failsafe routine - next change is made with RcOptions setting
 466
 467         // ------------------ STICKS COMMAND HANDLER --------------------
 468         // checking sticks positions
 469         for (i = 0; i < 4; i++) {
 470             stTmp >>= 2;
 471             if (rcData[i] > mcfg.mincheck)
 472                 stTmp |= 0x80;  // check for MIN
 473             if (rcData[i] < mcfg.maxcheck)
 474                 stTmp |= 0x40;  // check for MAX
 475         }
 476         if (stTmp == rcSticks) {
 477             if (rcDelayCommand < 250)
 478                 rcDelayCommand++;
 479         } else
 480             rcDelayCommand = 0;
 481         rcSticks = stTmp;
 482
 483         // perform actions
 484         if (rcData[THROTTLE] < mcfg.mincheck) {
 485             errorGyroI[ROLL] = 0;
 486             errorGyroI[PITCH] = 0;
 487             errorGyroI[YAW] = 0;
 488             errorAngleI[ROLL] = 0;
 489             errorAngleI[PITCH] = 0;
 490             if (cfg.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
 491                 if (rcOptions[BOXARM] && f.OK_TO_ARM)
 492                     mwArm();
 493                 else if (f.ARMED)
 494                     mwDisarm();
 495             }
 496         }
 497
 498         if (rcDelayCommand == 20) {
 499             if (f.ARMED) {      // actions during armed
 500                 // Disarm on throttle down + yaw
 501                 if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
 502                     mwDisarm();
 503                 // Disarm on roll (only when retarded_arm is enabled)
 504                 if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
 505                     mwDisarm();
 506             } else {            // actions during not armed
 507                 i = 0;
 508                 // GYRO calibration
 509                 if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 510                     calibratingG = 1000;
 511                     if (feature(FEATURE_GPS))
 512                         GPS_reset_home_position();
 513                     if (sensors(SENSOR_BARO))
 514                         calibratingB = 10; // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 515                 // Inflight ACC Calibration
 516                 } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 517                     if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
 518                         AccInflightCalibrationMeasurementDone = 0;
 519                         AccInflightCalibrationSavetoEEProm = 1;
 520                     } else {
 521                         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 522                         if (AccInflightCalibrationArmed) {
 523                             toggleBeep = 2;
 524                         } else {
 525                             toggleBeep = 3;
 526                         }
 527                     }
 528                 }
 529
 530                 // Multiple configuration profiles
 531                 if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 532                     i = 1;
 533                 else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 534                     i = 2;
 535                 else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 536                     i = 3;
 537                 if (i) {
 538                     mcfg.current_profile = i - 1;
 539                     writeEEPROM(0, false);
 540                     blinkLED(2, 40, i);
 541                     // TODO alarmArray[0] = i;
 542                 }
 543
 544                 // Arm via YAW
 545                 if (cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
 546                     mwArm();
 547                 // Arm via ROLL
 548                 else if (mcfg.retarded_arm && cfg.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
 549                     mwArm();
 550                 // Calibrating Acc
 551                 else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
 552                     calibratingA = 400;
 553                 // Calibrating Mag
 554                 else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
 555                     f.CALIBRATE_MAG = 1;
 556                 i = 0;
 557                 // Acc Trim
 558                 if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
 559                     cfg.angleTrim[PITCH] += 2;
 560                     i = 1;
 561                 } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
 562                     cfg.angleTrim[PITCH] -= 2;
 563                     i = 1;
 564                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
 565                     cfg.angleTrim[ROLL] += 2;
 566                     i = 1;
 567                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
 568                     cfg.angleTrim[ROLL] -= 2;
 569                     i = 1;
 570                 }
 571                 if (i) {
 572                     writeEEPROM(1, true);
 573                     rcDelayCommand = 0; // allow autorepetition
 574                 }
 575             }
 576         }
 577
 578         if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 579             if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > mcfg.mincheck && !rcOptions[BOXARM]) {   // Copter is airborne and you are turning it off via boxarm : start measurement
 580                 InflightcalibratingA = 50;
 581                 AccInflightCalibrationArmed = 0;
 582             }
 583             if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
 584                 if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 585                     InflightcalibratingA = 50;
 586             } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
 587                 AccInflightCalibrationMeasurementDone = 0;
 588                 AccInflightCalibrationSavetoEEProm = 1;
 589             }
 590         }
 591
 592         // Check AUX switches
 593         for (i = 0; i < 4; i++)
 594             auxState |= (rcData[AUX1 + i] < 1300) << (3 * i) | (1300 < rcData[AUX1 + i] && rcData[AUX1 + i] < 1700) << (3 * i + 1) | (rcData[AUX1 + i] > 1700) << (3 * i + 2);
 595         for (i = 0; i < CHECKBOXITEMS; i++)
 596             rcOptions[i] = (auxState & cfg.activate[i]) > 0;
 597
 598         // note: if FAILSAFE is disable, failsafeCnt > 5 * FAILSAVE_DELAY is always false
 599         if ((rcOptions[BOXANGLE] || (failsafeCnt > 5 * cfg.failsafe_delay)) && (sensors(SENSOR_ACC))) {
 600             // bumpless transfer to Level mode
 601             if (!f.ANGLE_MODE) {
 602                 errorAngleI[ROLL] = 0;
 603                 errorAngleI[PITCH] = 0;
 604                 f.ANGLE_MODE = 1;
 605             }
 606         } else {
 607             f.ANGLE_MODE = 0;        // failsave support
 608         }
 609
 610         if (rcOptions[BOXHORIZON]) {
 611             f.ANGLE_MODE = 0;
 612             if (!f.HORIZON_MODE) {
 613                 errorAngleI[ROLL] = 0;
 614                 errorAngleI[PITCH] = 0;
 615                 f.HORIZON_MODE = 1;
 616             }
 617         } else {
 618             f.HORIZON_MODE = 0;
 619         }
 620
 621         if ((rcOptions[BOXARM]) == 0)
 622             f.OK_TO_ARM = 1;
 623         if (f.ANGLE_MODE || f.HORIZON_MODE) {
 624             LED1_ON;
 625         } else {
 626             LED1_OFF;
 627         }
 628
 629 #ifdef BARO
 630         if (sensors(SENSOR_BARO)) {
 631             // Baro alt hold activate
 632             if (rcOptions[BOXBARO]) {
 633                 if (!f.BARO_MODE) {
 634                     f.BARO_MODE = 1;
 635                     AltHold = EstAlt;
 636                     initialThrottleHold = rcCommand[THROTTLE];
 637                     errorAltitudeI = 0;
 638                     BaroPID = 0;
 639                 }
 640             } else {
 641                 f.BARO_MODE = 0;
 642             }
 643             // Vario signalling activate
 644             if (feature(FEATURE_VARIO)) {
 645                 if (rcOptions[BOXVARIO]) {
 646                     if (!f.VARIO_MODE) {
 647                         f.VARIO_MODE = 1;
 648                     }
 649                 } else {
 650                     f.VARIO_MODE = 0;
 651                 }
 652             }
 653         }
 654 #endif
 655
 656 #ifdef  MAG
 657         if (sensors(SENSOR_MAG)) {
 658             if (rcOptions[BOXMAG]) {
 659                 if (!f.MAG_MODE) {
 660                     f.MAG_MODE = 1;
 661                     magHold = heading;
 662                 }
 663             } else {
 664                 f.MAG_MODE = 0;
 665             }
 666             if (rcOptions[BOXHEADFREE]) {
 667                 if (!f.HEADFREE_MODE) {
 668                     f.HEADFREE_MODE = 1;
 669                 }
 670             } else {
 671                 f.HEADFREE_MODE = 0;
 672             }
 673             if (rcOptions[BOXHEADADJ]) {
 674                 headFreeModeHold = heading; // acquire new heading
 675             }
 676         }
 677 #endif
 678
 679         if (sensors(SENSOR_GPS)) {
 680             if (f.GPS_FIX && GPS_numSat >= 5) {
 681                 // if both GPS_HOME & GPS_HOLD are checked => GPS_HOME is the priority
 682                 if (rcOptions[BOXGPSHOME]) {
 683                     if (!f.GPS_HOME_MODE) {
 684                         f.GPS_HOME_MODE = 1;
 685                         f.GPS_HOLD_MODE = 0;
 686                         GPSNavReset = 0;
 687                         GPS_set_next_wp(&GPS_home[LAT], &GPS_home[LON]);
 688                         nav_mode = NAV_MODE_WP;
 689                     }
 690                 } else {
 691                     f.GPS_HOME_MODE = 0;
 692                     if (rcOptions[BOXGPSHOLD] && abs(rcCommand[ROLL]) < cfg.ap_mode && abs(rcCommand[PITCH]) < cfg.ap_mode) {
 693                         if (!f.GPS_HOLD_MODE) {
 694                             f.GPS_HOLD_MODE = 1;
 695                             GPSNavReset = 0;
 696                             GPS_hold[LAT] = GPS_coord[LAT];
 697                             GPS_hold[LON] = GPS_coord[LON];
 698                             GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
 699                             nav_mode = NAV_MODE_POSHOLD;
 700                         }
 701                     } else {
 702                         f.GPS_HOLD_MODE = 0;
 703                         // both boxes are unselected here, nav is reset if not already done
 704                         if (GPSNavReset == 0) {
 705                             GPSNavReset = 1;
 706                             GPS_reset_nav();
 707                         }
 708                     }
 709                 }
 710             } else {
 711                 f.GPS_HOME_MODE = 0;
 712                 f.GPS_HOLD_MODE = 0;
 713                 nav_mode = NAV_MODE_NONE;
 714             }
 715         }
 716
 717         if (rcOptions[BOXPASSTHRU]) {
 718             f.PASSTHRU_MODE = 1;
 719         } else {
 720             f.PASSTHRU_MODE = 0;
 721         }
 722
 723         if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE) {
 724             f.HEADFREE_MODE = 0;
 725         }
 726     } else {                    // not in rc loop
 727         static int taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
 728         if (taskOrder > 3)
 729             taskOrder -= 4;
 730         switch (taskOrder) {
 731         case 0:
 732             taskOrder++;
 733 #ifdef MAG
 734             if (sensors(SENSOR_MAG) && Mag_getADC())
 735                 break;
 736 #endif
 737         case 1:
 738             taskOrder++;
 739 #ifdef BARO
 740             if (sensors(SENSOR_BARO) && Baro_update())
 741                 break;
 742 #endif
 743         case 2:
 744             taskOrder++;
 745 #ifdef BARO
 746             if (sensors(SENSOR_BARO) && getEstimatedAltitude())
 747             break;
 748 #endif
 749         case 3:
 750             taskOrder++;
 751 #ifdef SONAR
 752             if (sensors(SENSOR_SONAR)) {
 753                 Sonar_update();
 754                 debug[2] = sonarAlt;
 755             }
 756 #endif
 757             if (feature(FEATURE_VARIO) && f.VARIO_MODE)
 758                 mwVario();
 759             break;
 760         }
 761     }
 762
 763     currentTime = micros();
 764     if (mcfg.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
 765         loopTime = currentTime + mcfg.looptime;
 766
 767         computeIMU();
 768         // Measure loop rate just afer reading the sensors
 769         currentTime = micros();
 770         cycleTime = (int32_t)(currentTime - previousTime);
 771         previousTime = currentTime;
 772 #ifdef MPU6050_DMP
 773         mpu6050DmpLoop();
 774 #endif
 775
 776 #ifdef MAG
 777         if (sensors(SENSOR_MAG)) {
 778             if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
 779                 int16_t dif = heading - magHold;
 780                 if (dif <= -180)
 781                     dif += 360;
 782                 if (dif >= +180)
 783                     dif -= 360;
 784                 if (f.SMALL_ANGLES_25)
 785                     rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30;    // 18 deg
 786             } else
 787                 magHold = heading;
 788         }
 789 #endif
 790
 791 #ifdef BARO
 792         if (sensors(SENSOR_BARO)) {
 793             if (f.BARO_MODE) {
 794                 static uint8_t isAltHoldChanged = 0;
 795                 static int16_t AltHoldCorr = 0;
 796                 if (cfg.alt_hold_fast_change) {
 797                     // rapid alt changes
 798                     if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
 799                         errorAltitudeI = 0;
 800                         isAltHoldChanged = 1;
 801                         rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -cfg.alt_hold_throttle_neutral : cfg.alt_hold_throttle_neutral;
 802                     } else {
 803                         if (isAltHoldChanged) {
 804                             AltHold = EstAlt;
 805                             isAltHoldChanged = 0;
 806                         }
 807                         rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
 808                     }
 809                 } else {
 810                     // slow alt changes for apfags
 811                     if (abs(rcCommand[THROTTLE] - initialThrottleHold) > cfg.alt_hold_throttle_neutral) {
 812                         // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
 813                         AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
 814                         if (abs(AltHoldCorr) > 500) {
 815                             AltHold += AltHoldCorr / 500;
 816                             AltHoldCorr %= 500;
 817                         }
 818                         errorAltitudeI = 0;
 819                         isAltHoldChanged = 1;
 820                     } else if (isAltHoldChanged) {
 821                         AltHold = EstAlt;
 822                         isAltHoldChanged = 0;
 823                     }
 824                     rcCommand[THROTTLE] = initialThrottleHold + BaroPID;
 825                 }
 826             }
 827         }
 828 #endif
 829
 830         if (sensors(SENSOR_GPS)) {
 831             if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
 832                 float sin_yaw_y = sinf(heading * 0.0174532925f);
 833                 float cos_yaw_x = cosf(heading * 0.0174532925f);
 834                 if (cfg.nav_slew_rate) {
 835                     nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -cfg.nav_slew_rate, cfg.nav_slew_rate); // TODO check this on uint8
 836                     nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -cfg.nav_slew_rate, cfg.nav_slew_rate);
 837                     GPS_angle[ROLL] = (nav_rated[LON] * cos_yaw_x - nav_rated[LAT] * sin_yaw_y) / 10;
 838                     GPS_angle[PITCH] = (nav_rated[LON] * sin_yaw_y + nav_rated[LAT] * cos_yaw_x) / 10;
 839                 } else {
 840                     GPS_angle[ROLL] = (nav[LON] * cos_yaw_x - nav[LAT] * sin_yaw_y) / 10;
 841                     GPS_angle[PITCH] = (nav[LON] * sin_yaw_y + nav[LAT] * cos_yaw_x) / 10;
 842                 }
 843             }
 844         }
 845
 846         // PID - note this is function pointer set by setPIDController()
 847         pid_controller();
 848
 849         mixTable();
 850         writeServos();
 851         writeMotors();
 852     }
 853 }