   }
 
   void executeCommand(int cmd){
+    float heading;
+    int distance;
     switch(cmd){
        case CMD_SHOW_POSITION:
          //Display current position

         lcd.print(compassHeading);
         lcd.setCursor(0,1);
         lcd.print("G:");
         lcd.print(getHeading(mCurrentWayPoint, flat, flon));
         lcd.print(" ");
         lcd.print("D:");
         lcd.print(formatDistance(getDistance(mCurrentWayPoint, flat, flon)));
         //delay(100);
       break;
       case CMD_ADD_WAY_POINT:
         mLat[mCurrentWayPoint] = flat;

    }
    
  }
 
 void forward(){
 
 }
 
 void left(){
   
 }
 
 void right(){
   
 }
 
 float formatDistance(int meters){
  return meters / (float) 1000;
