        case CMD_ADD_WAY_POINT:
          mLat[mCurrentWayPoint] = flat;
          mLon[mCurrentWayPoint] = flon;
+       break;
+       case CMD_NEXT_WAY_POINT:
+         if(mCurrentWayPoint < MAX_WAYPOINTS)
+           mCurrentWayPoint++;
+       break;
+       case CMD_LAST_WAY_POINT:
+         if(mCurrentWayPoint > 0)
+           mCurrentWayPoint--;
        break;
     }
     

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

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