   void displayMenu(){
       lcd.clear();
       lcd.setCursor(0,0);
+      lcd.print(mCurrentWayPoint);
       lcd.print(">");
       lcd.print(mMenu[mMenuPosition]);
       if(mMenuPosition < MENU_LENGTH){

       case CMD_ADD_WAY_POINT:
         mLat[mCurrentWayPoint] = flat;
         mLon[mCurrentWayPoint] = flon;
       break;
    }
    

#define CMD_NAVIGATE -1
#define CMD_SHOW_POSITION 0
#define CMD_ADD_WAY_POINT 1
#define CMD_NEXT_WAY_POINT 2
#define CMD_LAST_WAY_POINT 3
int mMenuPosition = CMD_SHOW_POSITION;
#define MENU_LENGTH 4
char* mMenu[]={
"Show Position",
"Add Way Point",
"Next Way Point",
"Last Way Point",
"Delete Way Point",
"Clear Way Points"};
#define MODE_MENU 0
