          lcd.setCursor(0,0);
          lcd.print("LAT: ");
          lcd.print(flat);
-         lcd.print(" ");
-         lcd.print(compassHeading);
          lcd.setCursor(0,1);
          lcd.print("LON: ");
          lcd.print(flon);
+         delay(1000);
+       break;
+       case CMD_NAVIGATE:
+         lcd.clear();
+         lcd.setCursor(0,0);
+         lcd.print("MAG:");
+         lcd.print(compassHeading);
+         lcd.setCursor(0,1);
+         lcd.print("GPS:");
+         lcd.print(getHeading(0, flat, flon));
          lcd.print(" ");
+         lcd.print("M:");
          lcd.print(getDistance(0, flat, flon));
-         delay(500);
+         delay(100);
        break;
-       case CMD_DIRECTION:
-         
-       break;
-    }
-    
-  }
-  
+    }
+    
+  }
  
   void gpsdump(TinyGPS &gps){
     unsigned long age, date, time, chars;

  void setup(){
    lcd.begin(16, 2);              // start the library
    lcd.setCursor(0,0);
    lcd.print("NEW TESTING..."); // print a simple message
 
    //Setup compass
    slaveAddress = HMC6352Address >> 1;  
    Wire.begin();
    //Set up diagnostic output
    Serial.begin(115200);
    Serial.println("Booting ...");
    //Set gps
    mySerial.begin(4800);
    //Setup output led
    pinMode(led, OUTPUT);
    //Setup input buttons
    pinMode(buttons, INPUT);
    /* Initialise the compass */
    if(!mag.begin()){
      /* There was a problem detecting the HMC5883 ... check your connections */

    return distance;
  }
  
  /**
  * Returns heading to a waypoint in degrees
  * Pass in the wayPoint and current lattitude and longitude
  */
  float getHeading(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);
    
    double arg1 = sin(waypointLon-currentLon)*cos(waypointLat);
    double arg2 = cos(currentLat)*sin(waypointLat)-sin(currentLat)*cos(waypointLat)*cos(waypointLon-currentLon);
    float heading = atan2(arg1, arg2);//,2*3.1415926535;
    heading = heading*180/3.1415926535;  // convert from radians to degrees
    //if the heading is negative then add 360 to make it positive
    if(heading < 0){
      heading+=360;   
    }
    
    return heading;
  }

  // read the buttons
