          lcd.print(compassHeading);
          lcd.setCursor(0,1);
          lcd.print("GPS:");
-         lcd.print(getHeading(0, flat, flon));
+         lcd.print(getHeading(mCurrentWayPoint, flat, flon));
          lcd.print(" ");
          lcd.print("M:");
-         lcd.print(getDistance(0, flat, flon));
+         lcd.print(getDistance(mCurrentWayPoint, flat, flon));
          delay(100);
+       break;
+       case CMD_ADD_WAY_POINT:
+         mLat[mCurrentWayPoint] = flat;
+         mLon[mCurrentWayPoint] = flon;
        break;
     }
     


//Store Waypoints
#define MAX_WAYPOINTS 20
float mLat[MAX_WAYPOINTS]; //38.629983, -90.596550
float mLon[MAX_WAYPOINTS];

//Method definitions
void gpsdump(TinyGPS &gps);

      while(1);
    }
    //Setup test waypoint
    mLat[0] = 38.628967;
    mLon[0] = -90.270577;
    
    //Show initial menu
    displayMenu();
