     //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;
-    currentLat = radians(currentLat);
-    waypointLat = radians(waypointLat);
-    float diffLon = radians(waypointLon - currentLon);
-    
-    
-    //Calculate the distance
-    float distance = (sin(diffLat/2.0)*sin(diffLat/2.0));
-    float distance2 = cos(currentLat);
-    distance2 *= cos(waypointLat);
-    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;
-    
-    return distance;
-  }
-  
+      
+    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

    compassHeading = heading * 180/M_PI; 
  }
  
  void updateDisplay(){
    //Check if a button was pressed
    int btnPressed = read_LCD_buttons();
    

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