void loop(void) { static bool accCalibrated; static uint16_t calibratingA; static uint32_t currentTime; static uint32_t disarmTime; if (rcTask.checkAndUpdate(currentTime)) { // update RC channels rc.update(); // when landed, reset integral component of PID if (rc.throttleIsDown()) stab.resetIntegral(); if (rc.changed()) { if (armed) { // actions during armed // Disarm on throttle down + yaw if (rc.sticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) { if (armed) { armed = false; // Reset disarm time so that it works next time we arm the board. if (disarmTime != 0) disarmTime = 0; } } } else { // actions during not armed // gyro calibration if (rc.sticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) calibratingG = calibratingGyroCycles; // Arm via throttle-low / yaw-right if (rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) if (calibratingG == 0 && accCalibrated) if (!rc.auxState()) // aux switch must be in zero position if (!armed) armed = true; // accel calibration if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA = calibratingAccCycles; } // not armed } // rc.changed() // Switch to alt-hold when switch moves to position 1 or 2 nav.checkSwitch(); } else { // not in rc loop static int taskOrder; // never call all functions in the same loop, to avoid high delay spikes switch (taskOrder) { case 0: if (baro.available()) baro.update(); taskOrder++; break; case 1: if (baro.available() && altitudeEstimationTask.checkAndUpdate(currentTime)) { nav.updateAltitudePid(armed); } taskOrder++; break; case 2: taskOrder++; break; case 3: taskOrder++; break; case 4: taskOrder = 0; break; } } currentTime = board.getMicros(); if (imuTask.checkAndUpdate(currentTime)) { imu.update(currentTime, armed, calibratingA, calibratingG); haveSmallAngle = abs(imu.angle[0]) < CONFIG_SMALL_ANGLE && abs(imu.angle[1]) < CONFIG_SMALL_ANGLE; // measure loop rate just afer reading the sensors currentTime = board.getMicros(); // compute exponential RC commands rc.computeExpo(); // use LEDs to indicate calibration status if (calibratingA > 0 || calibratingG > 0) board.ledGreenOn(); else { if (accCalibrated) board.ledGreenOff(); if (armed) board.ledRedOn(); else board.ledRedOff(); } // periodically update accelerometer calibration status if (accelCalibrationTask.check(currentTime)) { if (!haveSmallAngle) { accCalibrated = false; board.ledGreenToggle(); accelCalibrationTask.update(currentTime); } else { accCalibrated = true; } } // handle serial communications msp.update(armed); // perform navigation tasks (alt-hold etc.) nav.perform(); // update stability PID controller stab.update(); // update mixer mixer.update(armed); } // IMU update } // loop()