       break; 
       case btnDOWN:
         //Move menu down
-        if(mMenuPosition < MENU_LENGTH - 1){
+        if(mMenuPosition < MENU_LENGTH){
           mMenuPosition++;
         }
       break;

#define btnNONE   5

//Menu System
#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
#define MODE_NAVIGATE 1
#define NUMBER_OF_MODES 2
int mMode = MODE_MENU;

//Store Waypoints

    //Convert to radians
    float waypointLat = mLat[wayPoint];
    float waypointLon = mLon[wayPoint];
      
    return calc_dist(currentLat, currentLon, waypointLat, waypointLon);
  }
  
  /*************************************************************************
 * //Function to calculate the distance between two waypoints
 *************************************************************************/
float calc_dist(float flat1, float flon1, float flat2, float flon2)
{
  float dist_calc=0;
  float dist_calc2=0;
  float diflat=0;
  float diflon=0;
  
  //I've to spplit all the calculation in several steps. If i try to do it in a single line the arduino will explode.
  diflat=radians(flat2-flat1);
  flat1=radians(flat1);
  flat2=radians(flat2);
  diflon=radians((flon2)-(flon1));
  
  dist_calc = (sin(diflat/2.0)*sin(diflat/2.0));
  dist_calc2= cos(flat1);
  dist_calc2*=cos(flat2);
  dist_calc2*=sin(diflon/2.0);
  dist_calc2*=sin(diflon/2.0);
  dist_calc +=dist_calc2;
  
  dist_calc=(2*atan2(sqrt(dist_calc),sqrt(1.0-dist_calc)));
  
  dist_calc*=6371000.0; //Converting to meters
  //Serial.println(dist_calc);
  return dist_calc;
}


  /**
  * Returns heading to a waypoint in degrees
  * Pass in the wayPoint and current lattitude and longitude
