   return bearing;
 }
 
+int parseSerial()
+{
+  char c;
+  uint8_t i = 0;
+  // Wait for a leading tilde
+  while(Serial.available()){
+    if(Serial.read() == '~') break;
+  }
+  // Process the rest of the line
+  while(Serial.available() && i<INPUT_BUF_SIZE-1){
+    c = Serial.read();
+    
+  }
+}
+
 float degToRad(float deg)
 {
   // Takes degrees, returns radians


void setup()
{
  //Launches a serial connection with a 115200 baud rate
  Serial.begin(115200);
  while(!Serial){ ; }
  Serial.println("Linn-Benton Community College, Eclipse 2017 payload tracker starting up..");
  //Launches the IMU. It returns a true value if it successfully launches.
  if(!bno.begin()){
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.println("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
    while(1);
  } else {
    Serial.println("BNO055 IMU detected..");
  }
  GPS.begin(9600);
  delay(500);
  bno.setExtCrystalUse(true);                     //Use the external clock in the IMU
  bno.setMode(bno.OPERATION_MODE_NDOF);
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);   //String formatting on the GPS
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);      //GPS packet dump rate
  GPS.sendCommand(PGCMD_ANTENNA);
  useInterrupt(usingInterrupt);                   //Set to use or not use the interrupt for GPS parcing
  delay(100);
}

// Interrupt is called once a millisecond, looks for any new GPS data, and stores it
SIGNAL(TIMER0_COMPA_vect)
{
  char c = GPS.read();
  // if you want to debug, this is a good time to do it!
  #ifdef UDR0
    if (GPSECHO)
      if (c) UDR0 = c;
      // writing direct to UDR0 is much much faster than Serial.print
      // but only one character can be written at a time.
  #endif
}

void useInterrupt(boolean v)
{
  // Set up the interrupt, intended to be run once on startup
  if (v) {
    // Timer0 is already used for millis() - we'll just interrupt somewhere
    // in the middle and call the "Compare A" function above
    OCR0A = 0xAF;
    TIMSK0 |= _BV(OCIE0A);
    usingInterrupt = true;
  } else {
    // do not call the interrupt function COMPA anymore
    TIMSK0 &= ~_BV(OCIE0A);
    usingInterrupt = false;
  }
}

uint32_t gpsTimer = millis();
uint32_t imuTimer = gpsTimer;
sensors_event_t event;           //Create a new local event instance
uint8_t sys, gyro, accel, mag;   //Create local variables gyro, accel, mag

void loop()
{

      Serial.println(GPS.fixquality);
    }
  }
//  // Display IMU info about 10 times per second
//  if (millis() - imuTimer > 100){
//    imuTimer = millis(); // reset the timer
//    //Read the current calibration values from the IMU
//    bno.getCalibration(&sys, &gyro, &accel, &mag);
//    //Read the current positional values
//    bno.getEvent(&event);
//
//    // Using quaternions
//    imu::Quaternion q = bno.getQuat();
//    q.normalize();
//    float temp = q.x();  q.x() = -q.y();  q.y() = temp;
//    q.z() = -q.z();
//    // Converted back to eulers
//    imu::Vector<3> euler = q.toEuler();
//    Serial.print("[IMU]");
//    Serial.print(-180/M_PI * euler.x());  // heading, nose-right is positive, z-axis points up
//    Serial.print(',');
//    Serial.print(-180/M_PI * euler.y());  // roll, rightwing-up is positive, y-axis points forward
//    Serial.print(',');
//    Serial.print(-180/M_PI * euler.z());  // pitch, nose-down is positive, x-axis points right
//    /*
//    Serial.print(event.orientation.x,2);
//    Serial.print(",");
//    Serial.print(event.orientation.y,2);
//    Serial.print(",");
//    Serial.print(event.orientation.z,2);
//    */
//    Serial.print(',');
//    Serial.print(sys);
//    Serial.print(',');
//    Serial.print(gyro);
//    Serial.print(',');
//    Serial.print(accel);
//    Serial.print(',');
//    Serial.println(mag);
//  }
}
