     if (!GPS.parse(GPS.lastNMEA()))
       return;
   }
+
+  payloadGPS.parse("$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47");
+  Serial.print("Payload GPS: ");
+  Serial.print(payloadGPS.latitudeDegrees,7);
+  Serial.print(',');
+  Serial.print(payloadGPS.longitudeDegrees,7);
+  Serial.print(',');
+  Serial.println((int)(payloadGPS.altitude));
 
   // Display GPS info about every 2 seconds
   if (millis() - gpsTimer > 2000){

/*
 * Author: Dylan Trafford (EE/CpE), Trevor Gahl (CpE), Gabe Gordon (MSGC MAP Student). Adapted from example code from Adafruit.com
 * Modified for Linn-Benton Community College by Levi Willmeth
 * Developed for use by MSGC BOREALIS Program
 * Purpose: To transmit data from a GPS and IMU unit to a computer for ground station positional data.
 * Note: Sends comma seperated data lead by a '~'. When the recieving computer sees a tilda, it knows it is the beginning of the line.
 */

//Included Libraries (some libraries are imported even though they are included in the base packages)
#include <Adafruit_GPS.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

//Defines (string replace constants)
#define GPSECHO false

//Instance Initializations
//Initializes an instance of the BNO055 called bno with an I2C address of 55
Adafruit_BNO055 bno = Adafruit_BNO055(55);
//Initializes an instance of SoftwareSerial called mySerial with RX and TX on pins 8 and 7
SoftwareSerial mySerial(8, 7);
//Initializes an instance of Adafruit_GPS called GPS using the mySerial instance
Adafruit_GPS GPS(&mySerial);

//Global Intializations
boolean usingInterrupt = true;
boolean calibrated = true;

float findBearing(float tLat, float tLon, float pLat, float pLon)
{


void setup()
{
  Serial.begin(115200);
  while(!Serial){ ; }
  Serial.println("Linn-Benton Community College, Eclipse 2017 payload tracker starting up..");
  
  // Set up BNO055 IMU
  if(!bno.begin()){
    Serial.println("Ooops, could not find BNO055... Check your wiring or I2C ADDR!");
//    while(1);
  } else {
    Serial.println("BNO055 IMU detected..");
  }
//  bno.setExtCrystalUse(true);                     //Use the external clock in the IMU
//  bno.setMode(bno.OPERATION_MODE_NDOF);

  // Set up Adafruit Ultimate GPS Shield
  GPS.begin(9600);
  // Set GPS to update position once per second
  GPS.sendCommand(PMTK_SET_NMEA_UPDATE_1HZ);
  GPS.sendCommand(PMTK_API_SET_FIX_CTL_1HZ);
  // Specify GPS output style
  GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCGGA);
  //GPS.sendCommand(PGCMD_ANTENNA);
  delay(100);
}

void loop()
{
