   * Pass in the current Latitude and Longitude
   */
   int getDistance(int wayPoint, float currentLat, float currentLon){
-    //Convert everthing to radians
-    float waypointLat = radians(mLat[wayPoint]);
-    float waypointLon = radians(mLon[wayPoint]);
-    currentLat = radians(currentLat);
-    currentLon = radians(currentLon);
+    //Convert to radians
+    float waypointLat = mLat[wayPoint];
+    float waypointLon = mLon[wayPoint];
+    
+    //currentLon = radians(currentLon);
     
     //Find the differance of lat an lon
     float diffLat = waypointLat - currentLat;
-    float diffLon = waypointLon - currentLon;
+    currentLat = radians(currentLat);
+    waypointLat = radians(waypointLat);
+    float diffLon = radians(waypointLon - currentLon);
+    
     
     //Calculate the distance
     float distance = (sin(diffLat/2.0)*sin(diffLat/2.0));

          mMenuPosition++;
        }
      break;
      case btnSELECT:
        executeCommand(mMenuPosition);
      break;
      default:
        //No buttons pressed

    distance2 *= sin(diffLon/2.0);
    distance2 *= sin(diffLon/2.0);
    distance += distance2;
    distance = 2 * atan2(sqrt(distance), sqrt(1.0-distance));
    
    //Convert to meters
    distance *= 6371000.0;
