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()
void loop(void) { static uint32_t rcTime = 0; static uint32_t loopTime; static uint32_t calibratedAccTime; static bool accCalibrated; static bool armed; static uint16_t calibratingA; static uint32_t currentTime; static uint32_t disarmTime = 0; if (check_and_update_timed_task(&rcTime, CONFIG_RC_LOOPTIME_USEC, currentTime)) { // update RC channels rc.update(); // when landed, reset integral component of PID if (rc.throttleIsDown()) pid.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 = CONFIG_CALIBRATING_GYRO_CYCLES; } // Arm via YAW if ((rc.sticks == THR_LO + YAW_HI + PIT_CE + ROL_CE)) { if (calibratingG == 0 && accCalibrated) { if (!armed) { // arm now! armed = true; } } else if (!armed) { blinkLED(2, 255, 1); } } // Calibrating Acc else if (rc.sticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) calibratingA = CONFIG_CALIBRATING_ACC_CYCLES; } // not armed } // rc.changed() } 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: taskOrder++; //sensorsGetBaro(); case 1: taskOrder++; //sensorsGetSonar(); case 2: taskOrder++; case 3: taskOrder++; case 4: taskOrder = 0; break; } } currentTime = board.getMicros(); if (check_and_update_timed_task(&loopTime, CONFIG_IMU_LOOPTIME_USEC, currentTime)) { imu.update(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(); previousTime = currentTime; // compute exponential RC commands rc.computeExpo(); // use LEDs to indicate calibration status if (calibratingA > 0 || calibratingG > 0) { board.ledGreenToggle(); } else { if (accCalibrated) board.ledGreenOff(); if (armed) board.ledRedOn(); else board.ledRedOff(); } if (check_timed_task(calibratedAccTime, currentTime)) { if (!haveSmallAngle) { accCalibrated = false; // accelerometer not calibrated or angle too steep board.ledGreenToggle(); update_timed_task(&calibratedAccTime, CONFIG_CALIBRATE_ACCTIME_USEC, currentTime); } else { accCalibrated = true; } } // handle serial communications msp.update(armed); // update PID controller pid.update(&rc, &imu); // update mixer mixer.update(armed); } // IMU update } // loop()