   orientation.heading = -orientation.heading;
   return orientation;
 }
+sensors_vec_t getGyro() {
+  sensors_event_t gyroEvent;
+  gyro.getEvent(&gyroEvent);
+  gyroEvent.gyro.x = -gyroEvent.gyro.x;
+  gyroEvent.gyro.y = -gyroEvent.gyro.y;
+  gyroEvent.gyro.z = -gyroEvent.gyro.z;
+  return gyroEvent.gyro;
+}
 float getAltitude() {
   sensors_event_t baroEvent;
   bmp.getEvent(&baroEvent);

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

  Serial.begin(115200);

  // Initialize sensors
  if (!accel.begin() || !mag.begin() || !bmp.begin() || !gyro.begin()) {
    fatalError();
    while(true);
  }
  // Enable auto-ranging
  gyro.enableAutoRange(true);
  
  ESC1.attach(5);
  ESC2.attach(6);
