src/mw.c

   1 #include <stdbool.h>
   2 #include <stdlib.h>
   3 #include <stdint.h>
   4 #include <math.h>
   5
   6 #include "platform.h"
   7
   8 #include "common/maths.h"
   9 #include "common/axis.h"
  10
  11 #include "drivers/accgyro_common.h"
  12 #include "drivers/light_ledring.h"
  13 #include "drivers/light_led.h"
  14 #include "drivers/gpio_common.h"
  15 #include "drivers/system_common.h"
  16 #include "drivers/serial_common.h"
  17
  18 #include "boardalignment.h"
  19 #include "battery.h"
  20 #include "buzzer.h"
  21 #include "escservo.h"
  22 #include "failsafe.h"
  23 #include "flight_imu.h"
  24 #include "flight_common.h"
  25 #include "flight_mixer.h"
  26 #include "gimbal.h"
  27 #include "gps_common.h"
  28 #include "sensors_common.h"
  29 #include "sensors_sonar.h"
  30 #include "sensors_compass.h"
  31 #include "sensors_acceleration.h"
  32 #include "sensors_barometer.h"
  33 #include "sensors_gyro.h"
  34 #include "serial_cli.h"
  35 #include "serial_common.h"
  36 #include "statusindicator.h"
  37 #include "rc_controls.h"
  38 #include "rc_curves.h"
  39 #include "rx_common.h"
  40 #include "telemetry_common.h"
  41
  42 #include "runtime_config.h"
  43 #include "config.h"
  44 #include "config_profile.h"
  45 #include "config_master.h"
  46
  47 // June 2013     V2.2-dev
  48
  49 enum {
  50     ALIGN_GYRO = 0,
  51     ALIGN_ACCEL = 1,
  52     ALIGN_MAG = 2
  53 };
  54
  55 /* for VBAT monitoring frequency */
  56 #define VBATFREQ 6        // to read battery voltage - nth number of loop iterations
  57
  58 int16_t debug[4];
  59 uint32_t currentTime = 0;
  60 uint32_t previousTime = 0;
  61 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
  62 int16_t headFreeModeHold;
  63
  64 int16_t telemTemperature1;      // gyro sensor temperature
  65
  66 uint16_t rssi;                  // range: [0;1023]
  67
  68 extern uint8_t dynP8[3], dynI8[3], dynD8[3];
  69 extern failsafe_t *failsafe;
  70
  71 typedef void (* pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *accelerometerTrims);
  72
  73 extern pidControllerFuncPtr pid_controller;
  74
  75 // Automatic ACC Offset Calibration
  76 bool AccInflightCalibrationArmed = false;
  77 bool AccInflightCalibrationMeasurementDone = false;
  78 bool AccInflightCalibrationSavetoEEProm = false;
  79 bool AccInflightCalibrationActive = false;
  80 uint16_t InflightcalibratingA = 0;
  81
  82 void annexCode(void)
  83 {
  84     static uint32_t calibratedAccTime;
  85     int32_t tmp, tmp2;
  86     int32_t axis, prop1, prop2;
  87
  88     static uint8_t batteryWarningEnabled = false;
  89
  90     static uint8_t vbatTimer = 0;
  91
  92     // PITCH & ROLL only dynamic PID adjustemnt,  depending on throttle value
  93     if (rcData[THROTTLE] < currentProfile.tpa_breakpoint) {
  94         prop2 = 100;
  95     } else {
  96         if (rcData[THROTTLE] < 2000) {
  97             prop2 = 100 - (uint16_t)currentProfile.dynThrPID * (rcData[THROTTLE] - currentProfile.tpa_breakpoint) / (2000 - currentProfile.tpa_breakpoint);
  98         } else {
  99             prop2 = 100 - currentProfile.dynThrPID;
 100         }
 101     }
 102
 103     for (axis = 0; axis < 3; axis++) {
 104         tmp = min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500);
 105         if (axis != 2) {        // ROLL & PITCH
 106             if (currentProfile.deadband) {
 107                 if (tmp > currentProfile.deadband) {
 108                     tmp -= currentProfile.deadband;
 109                 } else {
 110                     tmp = 0;
 111                 }
 112             }
 113
 114             tmp2 = tmp / 100;
 115             rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
 116             prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.rollPitchRate * tmp / 500;
 117             prop1 = (uint16_t)prop1 * prop2 / 100;
 118         } else {                // YAW
 119             if (currentProfile.yaw_deadband) {
 120                 if (tmp > currentProfile.yaw_deadband) {
 121                     tmp -= currentProfile.yaw_deadband;
 122                 } else {
 123                     tmp = 0;
 124                 }
 125             }
 126             rcCommand[axis] = tmp * -masterConfig.yaw_control_direction;
 127             prop1 = 100 - (uint16_t)currentProfile.controlRateConfig.yawRate * abs(tmp) / 500;
 128         }
 129         dynP8[axis] = (uint16_t)currentProfile.pidProfile.P8[axis] * prop1 / 100;
 130         dynI8[axis] = (uint16_t)currentProfile.pidProfile.I8[axis] * prop1 / 100;
 131         dynD8[axis] = (uint16_t)currentProfile.pidProfile.D8[axis] * prop1 / 100;
 132         if (rcData[axis] < masterConfig.rxConfig.midrc)
 133             rcCommand[axis] = -rcCommand[axis];
 134     }
 135
 136     tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
 137     tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck);       // [MINCHECK;2000] -> [0;1000]
 138     tmp2 = tmp / 100;
 139     rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100;    // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
 140
 141     if (f.HEADFREE_MODE) {
 142         float radDiff = degreesToRadians(heading - headFreeModeHold);
 143         float cosDiff = cosf(radDiff);
 144         float sinDiff = sinf(radDiff);
 145         int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
 146         rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
 147         rcCommand[PITCH] = rcCommand_PITCH;
 148     }
 149
 150     if (feature(FEATURE_VBAT)) {
 151         if (!(++vbatTimer % VBATFREQ)) {
 152             updateBatteryVoltage();
 153
 154             batteryWarningEnabled = shouldSoundBatteryAlarm();
 155         }
 156     }
 157
 158     buzzer(batteryWarningEnabled); // external buzzer routine that handles buzzer events globally now
 159
 160     if ((!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete())) {      // Calibration phasis
 161         LED0_TOGGLE;
 162     } else {
 163         if (f.ACC_CALIBRATED)
 164             LED0_OFF;
 165         if (f.ARMED)
 166             LED0_ON;
 167
 168         checkTelemetryState();
 169     }
 170
 171 #ifdef LEDRING
 172     if (feature(FEATURE_LED_RING)) {
 173         static uint32_t LEDTime;
 174         if ((int32_t)(currentTime - LEDTime) >= 0) {
 175             LEDTime = currentTime + 50000;
 176             ledringState(f.ARMED, angle[AI_PITCH], angle[AI_ROLL]);
 177         }
 178     }
 179 #endif
 180
 181     if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
 182         if (!f.SMALL_ANGLE) {
 183             f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
 184             LED0_TOGGLE;
 185             calibratedAccTime = currentTime + 500000;
 186         } else {
 187             f.ACC_CALIBRATED = 1;
 188         }
 189     }
 190
 191     handleSerial();
 192
 193     if (!cliMode && feature(FEATURE_TELEMETRY)) {
 194         handleTelemetry();
 195     }
 196
 197     if (sensors(SENSOR_GPS)) {
 198         updateGpsIndicator(currentTime);
 199     }
 200
 201     // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
 202     if (gyro.temperature)
 203         gyro.temperature(&telemTemperature1);
 204     else {
 205         // TODO MCU temp
 206     }
 207 }
 208
 209 static void mwArm(void)
 210 {
 211     if (isGyroCalibrationComplete() && f.ACC_CALIBRATED) {
 212         // TODO: feature(FEATURE_FAILSAFE) && failsafeCnt < 2
 213         // TODO: && ( !feature || ( feature && ( failsafecnt > 2) )
 214         if (!f.ARMED) {         // arm now!
 215             f.ARMED = 1;
 216             headFreeModeHold = heading;
 217         }
 218     } else if (!f.ARMED) {
 219         blinkLedAndSoundBeeper(2, 255, 1);
 220     }
 221 }
 222
 223 static void mwVario(void)
 224 {
 225
 226 }
 227
 228 void loop(void)
 229 {
 230     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
 231     static uint8_t rcSticks;            // this hold sticks position for command combos
 232     uint8_t stTmp = 0;
 233     int i;
 234     static uint32_t rcTime = 0;
 235 #ifdef BARO
 236     static int16_t initialThrottleHold;
 237 #endif
 238     static uint32_t loopTime;
 239     uint16_t auxState = 0;
 240     bool isThrottleLow = false;
 241     bool rcReady = false;
 242
 243     // calculate rc stuff from serial-based receivers (spek/sbus)
 244     if (feature(FEATURE_SERIALRX)) {
 245         rcReady = isSerialRxFrameComplete(&masterConfig.rxConfig);
 246     }
 247
 248     if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
 249         rcReady = false;
 250         rcTime = currentTime + 20000;
 251         computeRC(&masterConfig.rxConfig, &rxRuntimeConfig);
 252
 253         // in 3D mode, we need to be able to disarm by switch at any time
 254         if (feature(FEATURE_3D)) {
 255             if (!rcOptions[BOXARM])
 256                 mwDisarm();
 257         }
 258
 259         // Read value of AUX channel as rssi
 260         // 0 is disable, 1-4 is AUX{1..4}
 261         if (masterConfig.rssi_aux_channel > 0) {
 262             const int16_t rssiChannelData = rcData[AUX1 + masterConfig.rssi_aux_channel - 1];
 263             // Range of rssiChannelData is [1000;2000]. rssi should be in [0;1023];
 264             rssi = (uint16_t)((constrain(rssiChannelData - 1000, 0, 1000) / 1000.0f) * 1023.0f);
 265         }
 266
 267         if (feature(FEATURE_FAILSAFE)) {
 268             failsafe->vTable->updateState();
 269         }
 270
 271         // ------------------ STICKS COMMAND HANDLER --------------------
 272         // checking sticks positions
 273         for (i = 0; i < 4; i++) {
 274             stTmp >>= 2;
 275             if (rcData[i] > masterConfig.rxConfig.mincheck)
 276                 stTmp |= 0x80;  // check for MIN
 277             if (rcData[i] < masterConfig.rxConfig.maxcheck)
 278                 stTmp |= 0x40;  // check for MAX
 279         }
 280         if (stTmp == rcSticks) {
 281             if (rcDelayCommand < 250)
 282                 rcDelayCommand++;
 283         } else
 284             rcDelayCommand = 0;
 285         rcSticks = stTmp;
 286
 287         // perform actions
 288         if (feature(FEATURE_3D) && (rcData[THROTTLE] > (masterConfig.rxConfig.midrc - masterConfig.flight3DConfig.deadband3d_throttle) && rcData[THROTTLE] < (masterConfig.rxConfig.midrc + masterConfig.flight3DConfig.deadband3d_throttle)))
 289             isThrottleLow = true;
 290         else if (!feature(FEATURE_3D) && (rcData[THROTTLE] < masterConfig.rxConfig.mincheck))
 291             isThrottleLow = true;
 292         if (isThrottleLow) {
 293             resetErrorAngle();
 294             resetErrorGyro();
 295             if (currentProfile.activate[BOXARM] > 0) { // Arming/Disarming via ARM BOX
 296                 if (rcOptions[BOXARM] && f.OK_TO_ARM)
 297                     mwArm();
 298                 else if (f.ARMED)
 299                     mwDisarm();
 300             }
 301         }
 302
 303         if (rcDelayCommand == 20) {
 304             if (f.ARMED) {      // actions during armed
 305                 // Disarm on throttle down + yaw
 306                 if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE))
 307                     mwDisarm();
 308                 // Disarm on roll (only when retarded_arm is enabled)
 309                 if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_LO))
 310                     mwDisarm();
 311             } else {            // actions during not armed
 312                 i = 0;
 313                 // GYRO calibration
 314                 if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
 315                     gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
 316                     if (feature(FEATURE_GPS))
 317                         GPS_reset_home_position();
 318 #ifdef BARO
 319                     if (sensors(SENSOR_BARO))
 320                         baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
 321 #endif
 322                     if (!sensors(SENSOR_MAG))
 323                         heading = 0; // reset heading to zero after gyro calibration
 324                 // Inflight ACC Calibration
 325                 } else if (feature(FEATURE_INFLIGHT_ACC_CAL) && (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_HI)) {
 326                     if (AccInflightCalibrationMeasurementDone) {        // trigger saving into eeprom after landing
 327                         AccInflightCalibrationMeasurementDone = false;
 328                         AccInflightCalibrationSavetoEEProm = true;
 329                     } else {
 330                         AccInflightCalibrationArmed = !AccInflightCalibrationArmed;
 331                         if (AccInflightCalibrationArmed) {
 332                             queueConfirmationBeep(2);
 333                         } else {
 334                             queueConfirmationBeep(3);
 335                         }
 336                     }
 337                 }
 338
 339                 // Multiple configuration profiles
 340                 if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
 341                     i = 1;
 342                 else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE)     // PITCH up   -> Profile 2
 343                     i = 2;
 344                 else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_HI)     // ROLL right -> Profile 3
 345                     i = 3;
 346                 if (i) {
 347                     masterConfig.current_profile_index = i - 1;
 348                     writeEEPROM();
 349                     readEEPROM();
 350                     blinkLedAndSoundBeeper(2, 40, i);
 351                     // TODO alarmArray[0] = i;
 352                 }
 353
 354                 // Arm via YAW
 355                 if (currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE))
 356                     mwArm();
 357                 // Arm via ROLL
 358                 else if (masterConfig.retarded_arm && currentProfile.activate[BOXARM] == 0 && (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI))
 359                     mwArm();
 360                 // Calibrating Acc
 361                 else if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE)
 362                     accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
 363                 // Calibrating Mag
 364                 else if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE)
 365                     f.CALIBRATE_MAG = 1;
 366                 i = 0;
 367                 // Acc Trim
 368                 if (rcSticks == THR_HI + YAW_CE + PIT_HI + ROL_CE) {
 369                     currentProfile.accelerometerTrims.trims.pitch += 2;
 370                     i = 1;
 371                 } else if (rcSticks == THR_HI + YAW_CE + PIT_LO + ROL_CE) {
 372                     currentProfile.accelerometerTrims.trims.pitch -= 2;
 373                     i = 1;
 374                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_HI) {
 375                     currentProfile.accelerometerTrims.trims.roll += 2;
 376                     i = 1;
 377                 } else if (rcSticks == THR_HI + YAW_CE + PIT_CE + ROL_LO) {
 378                     currentProfile.accelerometerTrims.trims.roll -= 2;
 379                     i = 1;
 380                 }
 381                 if (i) {
 382                     copyCurrentProfileToProfileSlot(masterConfig.current_profile_index);
 383                     writeEEPROM();
 384                     readEEPROMAndNotify();
 385                     rcDelayCommand = 0; // allow autorepetition
 386                 }
 387             }
 388         }
 389
 390         if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
 391             if (AccInflightCalibrationArmed && f.ARMED && rcData[THROTTLE] > masterConfig.rxConfig.mincheck && !rcOptions[BOXARM]) {   // Copter is airborne and you are turning it off via boxarm : start measurement
 392                 InflightcalibratingA = 50;
 393                 AccInflightCalibrationArmed = false;
 394             }
 395             if (rcOptions[BOXCALIB]) {      // Use the Calib Option to activate : Calib = TRUE Meausrement started, Land and Calib = 0 measurement stored
 396                 if (!AccInflightCalibrationActive && !AccInflightCalibrationMeasurementDone)
 397                     InflightcalibratingA = 50;
 398                     AccInflightCalibrationActive = true;
 399             } else if (AccInflightCalibrationMeasurementDone && !f.ARMED) {
 400                 AccInflightCalibrationMeasurementDone = false;
 401                 AccInflightCalibrationSavetoEEProm = true;
 402             }
 403         }
 404
 405         // Check AUX switches
 406         for (i = 0; i < 4; i++)
 407             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);
 408         for (i = 0; i < CHECKBOX_ITEM_COUNT; i++)
 409             rcOptions[i] = (auxState & currentProfile.activate[i]) > 0;
 410
 411         if ((rcOptions[BOXANGLE] || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {
 412             // bumpless transfer to Level mode
 413             if (!f.ANGLE_MODE) {
 414                 resetErrorAngle();
 415                 f.ANGLE_MODE = 1;
 416             }
 417         } else {
 418             f.ANGLE_MODE = 0; // failsafe support
 419         }
 420
 421         if (rcOptions[BOXHORIZON]) {
 422             f.ANGLE_MODE = 0;
 423             if (!f.HORIZON_MODE) {
 424                 resetErrorAngle();
 425                 f.HORIZON_MODE = 1;
 426             }
 427         } else {
 428             f.HORIZON_MODE = 0;
 429         }
 430
 431         if ((rcOptions[BOXARM]) == 0)
 432             f.OK_TO_ARM = 1;
 433         if (f.ANGLE_MODE || f.HORIZON_MODE) {
 434             LED1_ON;
 435         } else {
 436             LED1_OFF;
 437         }
 438
 439 #ifdef BARO
 440         if (sensors(SENSOR_BARO)) {
 441             // Baro alt hold activate
 442             if (rcOptions[BOXBARO]) {
 443                 if (!f.BARO_MODE) {
 444                     f.BARO_MODE = 1;
 445                     AltHold = EstAlt;
 446                     initialThrottleHold = rcCommand[THROTTLE];
 447                     errorAltitudeI = 0;
 448                     BaroPID = 0;
 449                 }
 450             } else {
 451                 f.BARO_MODE = 0;
 452             }
 453             // Vario signalling activate
 454             if (feature(FEATURE_VARIO)) {
 455                 if (rcOptions[BOXVARIO]) {
 456                     if (!f.VARIO_MODE) {
 457                         f.VARIO_MODE = 1;
 458                     }
 459                 } else {
 460                     f.VARIO_MODE = 0;
 461                 }
 462             }
 463         }
 464 #endif
 465
 466 #ifdef  MAG
 467         if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
 468             if (rcOptions[BOXMAG]) {
 469                 if (!f.MAG_MODE) {
 470                     f.MAG_MODE = 1;
 471                     magHold = heading;
 472                 }
 473             } else {
 474                 f.MAG_MODE = 0;
 475             }
 476             if (rcOptions[BOXHEADFREE]) {
 477                 if (!f.HEADFREE_MODE) {
 478                     f.HEADFREE_MODE = 1;
 479                 }
 480             } else {
 481                 f.HEADFREE_MODE = 0;
 482             }
 483             if (rcOptions[BOXHEADADJ]) {
 484                 headFreeModeHold = heading; // acquire new heading
 485             }
 486         }
 487 #endif
 488
 489         if (sensors(SENSOR_GPS)) {
 490             updateGpsWaypointsAndMode();
 491         }
 492
 493         if (rcOptions[BOXPASSTHRU]) {
 494             f.PASSTHRU_MODE = 1;
 495         } else {
 496             f.PASSTHRU_MODE = 0;
 497         }
 498
 499         if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
 500             f.HEADFREE_MODE = 0;
 501         }
 502     } else {                    // not in rc loop
 503         static int taskOrder = 0;    // never call all function in the same loop, to avoid high delay spikes
 504         switch (taskOrder) {
 505         case 0:
 506             taskOrder++;
 507 #ifdef MAG
 508             if (sensors(SENSOR_MAG) && compassGetADC(&masterConfig.magZero))
 509                 break;
 510 #endif
 511         case 1:
 512             taskOrder++;
 513 #ifdef BARO
 514             if (sensors(SENSOR_BARO) && baroUpdate(currentTime) != BAROMETER_ACTION_NOT_READY)
 515                 break;
 516 #endif
 517         case 2:
 518             taskOrder++;
 519 #ifdef BARO
 520             if (sensors(SENSOR_BARO) && getEstimatedAltitude())
 521                 break;
 522 #endif
 523         case 3:
 524             // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
 525             // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
 526             // change this based on available hardware
 527             taskOrder++;
 528             if (feature(FEATURE_GPS)) {
 529                 gpsThread();
 530                 break;
 531             }
 532         case 4:
 533             taskOrder = 0;
 534 #ifdef SONAR
 535             if (sensors(SENSOR_SONAR)) {
 536                 Sonar_update();
 537             }
 538 #endif
 539             if (feature(FEATURE_VARIO) && f.VARIO_MODE)
 540                 mwVario();
 541             break;
 542         }
 543     }
 544
 545     currentTime = micros();
 546     if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
 547         loopTime = currentTime + masterConfig.looptime;
 548
 549         computeIMU();
 550         annexCode();
 551         // Measure loop rate just afer reading the sensors
 552         currentTime = micros();
 553         cycleTime = (int32_t)(currentTime - previousTime);
 554         previousTime = currentTime;
 555
 556 #ifdef MAG
 557         if (sensors(SENSOR_MAG)) {
 558             if (abs(rcCommand[YAW]) < 70 && f.MAG_MODE) {
 559                 int16_t dif = heading - magHold;
 560                 if (dif <= -180)
 561                     dif += 360;
 562                 if (dif >= +180)
 563                     dif -= 360;
 564                 dif *= -masterConfig.yaw_control_direction;
 565                 if (f.SMALL_ANGLE)
 566                     rcCommand[YAW] -= dif * currentProfile.pidProfile.P8[PIDMAG] / 30;    // 18 deg
 567             } else
 568                 magHold = heading;
 569         }
 570 #endif
 571
 572 #ifdef BARO
 573         if (sensors(SENSOR_BARO)) {
 574             if (f.BARO_MODE) {
 575                 static uint8_t isAltHoldChanged = 0;
 576                 static int16_t AltHoldCorr = 0;
 577                 if (!f.FIXED_WING) {
 578                     // multirotor alt hold
 579                     if (currentProfile.alt_hold_fast_change) {
 580                         // rapid alt changes
 581                         if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
 582                             errorAltitudeI = 0;
 583                             isAltHoldChanged = 1;
 584                             rcCommand[THROTTLE] += (rcCommand[THROTTLE] > initialThrottleHold) ? -currentProfile.alt_hold_throttle_neutral : currentProfile.alt_hold_throttle_neutral;
 585                         } else {
 586                             if (isAltHoldChanged) {
 587                                 AltHold = EstAlt;
 588                                 isAltHoldChanged = 0;
 589                             }
 590                             rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
 591                         }
 592                     } else {
 593                         // slow alt changes for apfags
 594                         if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile.alt_hold_throttle_neutral) {
 595                             // Slowly increase/decrease AltHold proportional to stick movement ( +100 throttle gives ~ +50 cm in 1 second with cycle time about 3-4ms)
 596                             AltHoldCorr += rcCommand[THROTTLE] - initialThrottleHold;
 597                             AltHold += AltHoldCorr / 2000;
 598                             AltHoldCorr %= 2000;
 599                             isAltHoldChanged = 1;
 600                         } else if (isAltHoldChanged) {
 601                             AltHold = EstAlt;
 602                             AltHoldCorr = 0;
 603                             isAltHoldChanged = 0;
 604                         }
 605                         rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle + 100, masterConfig.escAndServoConfig.maxthrottle);
 606                     }
 607                 } else {
 608                     // handle fixedwing-related althold. UNTESTED! and probably wrong
 609                     // most likely need to check changes on pitch channel and 'reset' althold similar to
 610                     // how throttle does it on multirotor
 611                     rcCommand[PITCH] += BaroPID * masterConfig.fixedwing_althold_dir;
 612                 }
 613             }
 614         }
 615 #endif
 616
 617         if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) {
 618             rcCommand[THROTTLE] += throttleAngleCorrection;
 619         }
 620
 621         if (sensors(SENSOR_GPS)) {
 622             if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
 623                 updateGpsStateForHomeAndHoldMode();
 624             }
 625         }
 626
 627         // PID - note this is function pointer set by setPIDController()
 628         pid_controller(&currentProfile.pidProfile, &currentProfile.controlRateConfig, masterConfig.max_angle_inclination, &currentProfile.accelerometerTrims);
 629
 630         mixTable();
 631         writeServos();
 632         writeMotors();
 633     }
 634 }