   sensorError.roll /= trials;
   sensorError.elevation /= trials;
   calibrating = false;
+
+  sensorData = getOrientation();
+  attitude.pitch = sensorData.pitch;
+  attitude.roll = sensorData.roll;
+  attitude.yaw = sensorData.heading;
+  lastAttitudeUpdate = millis();
   
   digitalWrite(armLED, HIGH);
 }

SensorError sensorError = {0, 0, 0};
boolean calibrating = true;

Adafruit_10DOF                dof   = Adafruit_10DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);

  }
  // Calibrate accelerometer
  const unsigned short trials = 20;
  sensors_vec_t sensorData;
  for (int trial = 0; trial < trials; trial++) {
    sensorData = getOrientation();
    sensorError.pitch += (float)sensorData.pitch;
    sensorError.roll += (float)sensorData.roll;
    sensorError.elevation += getAltitude();
