 
 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.
+  
+  // Set up BNO055 IMU
   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);
+    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);
-  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
+  // 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);
 }
-
-// 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()
 {

    if (!GPS.parse(GPS.lastNMEA()))
      return;
  }

  // Display GPS info about every 2 seconds
  if (millis() - gpsTimer > 2000){

#include <SoftwareSerial.h>
#include <Wire.h>
#include <SPI.h>
//#include <Adafruit_GPS.h>
#include <TinyGPS++.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <math.h>

#define DEBUGGING true
#define INPUT_BUF_SIZE 128

// Initializes an instance of the BNO055 called bno with an I2C address of 55
Adafruit_BNO055 bno = Adafruit_BNO055(55);

// Ultimate GPS shield uses RX and TX on pins 8 and 7
SoftwareSerial mySerial(8, 7);
Adafruit_GPS GPS(&mySerial);
Adafruit_GPS payloadGPS = new Adafruit_GPS();

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
char inputBuffer[INPUT_BUF_SIZE];


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