 #include <Adafruit_Sensor.h>
 #include <Adafruit_HMC5883_U.h>
 
-//Compass stuff
-int HMC6352Address = 0x42;
-int slaveAddress;
-byte headingData[2];
-int i, headingValue;
 // tiny gps library code
 float flat, flon;
-float heading=0;
-int headinggps;
+
 TinyGPS gps;
 SoftwareSerial mySerial(2, 3);    //used for gps rx and tx pins in use
 

  * Pass in the current Latitude and Longitude
  */
  int getDistance(int wayPoint, float currentLat, float currentLon){
    //Convert everthing to radians
    float waypointLat = radians(mLat[wayPoint]);
    float waypointLon = radians(mLon[wayPoint]);
    currentLat = radians(currentLat);
    currentLon = radians(currentLon);
    
    //Find the differance of lat an lon
    float diffLat = waypointLat - currentLat;
    float diffLon = waypointLon - currentLon;
    
    //Calculate the distance
    float distance = (sin(diffLat/2.0)*sin(diffLat/2.0));

          mMenuPosition++;
        }
      break;
      case btnRIGHT:
        //Next Mode
        if(mMode < NUMBER_OF_MODES - 1){
           mMode++; 
        }
      break;
      case btnLEFT:
        //Last Mode
        if(mMode > 0){
           mMode--; 
        }
      break;
      case btnSELECT:
        if(mMode == MODE_MENU){
          executeCommand(mMenuPosition);
        }
      break;
      default:
        //No buttons pressed
