void setup(void)
{
board.init();
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// initialize our RC, IMU, mixer, and PID controller
rc.init(&board);
imu.init(&board);
pid.init();
mixer.init(&board, &rc, &pid);
msp.init(&board, &imu, &mixer, &rc);
// set initial time
previousTime = board.getMicros();
// always do gyro calibration at startup
calibratingG = CONFIG_CALIBRATING_GYRO_CYCLES;
// trigger accelerometer calibration requirement
haveSmallAngle = true;
} // setup
void setup(void)
{
uint32_t calibratingGyroMsec;
// Get particulars for board
board.init(imuLooptimeUsec, calibratingGyroMsec);
// sleep for 100ms
board.delayMilliseconds(100);
// flash the LEDs to indicate startup
board.ledRedOn();
board.ledGreenOff();
for (uint8_t i = 0; i < 10; i++) {
board.ledRedToggle();
board.ledGreenToggle();
board.delayMilliseconds(50);
}
board.ledRedOff();
board.ledGreenOff();
// compute cycles for calibration based on board's time constant
calibratingGyroCycles = (uint16_t)(1000. * calibratingGyroMsec / imuLooptimeUsec);
calibratingAccCycles = (uint16_t)(1000. * CONFIG_CALIBRATING_ACC_MSEC / imuLooptimeUsec);
// initializing timing tasks
imuTask.init(imuLooptimeUsec);
rcTask.init(CONFIG_RC_LOOPTIME_MSEC * 1000);
accelCalibrationTask.init(CONFIG_CALIBRATE_ACCTIME_MSEC * 1000);
altitudeEstimationTask.init(CONFIG_ALTITUDE_UPDATE_MSEC * 1000);
// initialize our external objects with objects they need
rc.init(&board);
stab.init(&rc, &imu);
imu.init(&board, calibratingGyroCycles, calibratingAccCycles);
mixer.init(&board, &rc, &stab);
msp.init(&board, &imu, &nav, &mixer, &rc);
nav.init(&board, &imu, &baro, &rc);
// always do gyro calibration at startup
calibratingG = calibratingGyroCycles;
// assume shallow angle (no accelerometer calibration needed)
haveSmallAngle = true;
// ensure not armed
armed = false;
// attempt to initialize barometer
baro.init(&board);
} // setup
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()
