 #include <SoftwareSerial.h>
 #include <TinyGPS.h>  
 #include <Wire.h>
-#include <Adafruit_Sensor.h>
-#include <Adafruit_LSM303_U.h>
-#include <Adafruit_L3GD20_U.h>
-#include <Adafruit_9DOF.h>
+#include <LSM303.h>
 #include <SabertoothSimplified.h>
 #include <EasyTransfer.h>
 #include <Kalman.h>

SoftwareSerial mySerial(2, 255);    //used for gps rx and tx pins in use

//Compass Stuff
/* Assign a unique ID to the sensors */
Adafruit_9DOF                dof   = Adafruit_9DOF();
Adafruit_LSM303_Accel_Unified accel = Adafruit_LSM303_Accel_Unified(30301);
Adafruit_LSM303_Mag_Unified   mag   = Adafruit_LSM303_Mag_Unified(30302);

/* Update this with the correct SLP for accurate altitude measurements */
float seaLevelPressure = SENSORS_PRESSURE_SEALEVELHPA;

//Compass stuff
float compassHeading;
int mHeadingOffset = 0;
#define HEADING_ADJUST_PIN A2

    mySerial.begin(9600);

    /* Initialise the compass */
    Wire.begin();
    compass.init();
    compass.enableDefault();
    /*
    Calibration values; the default values of +/-32767 for each axis
    lead to an assumed magnetometer bias of 0. Use the Calibrate example
    program to determine appropriate values for your particular unit.
    */
    compass.m_min = (LSM303::vector<int16_t>){-767, -642, -379};
    compass.m_max = (LSM303::vector<int16_t>){+322, +647, +616};
  
    //Setup test waypoint. Start 38.631514, -90.271908
    mLat[0] = 38.631158;
    mLon[0] = -90.272172;
