          lcd.print(getHeading(mCurrentWayPoint, flat, flon));
          lcd.print(" ");
          lcd.print("D:");
-         lcd.print(getDistance(mCurrentWayPoint, flat, flon));
+         lcd.print(formatDistance(getDistance(mCurrentWayPoint, flat, flon)));
          //delay(100);
        break;
        case CMD_ADD_WAY_POINT:

    }
    
  }
 
  void gpsdump(TinyGPS &gps){
    unsigned long age, date, time, chars;

  bool feedgps(){
    bool newData = false;
    while (mySerial.available()){
      char data = mySerial.read();
      Serial.print(data);
      if (gps.encode(data))
        newData = true;
    }
    return newData;
