/**
  * This file uses Rhumb Line Navigation to calculate the distance based on the current position and the desired position
  * Rhumb Line Navigation is used instead of great circle navigation because it provides a constant heading and within
  * short distances any inefficiencies compared with great circle are negligable.
  * point1 is current location, point 2 is destination
**/
void calc_distance()
{
   //---Local Variables---
   float q=0; //calcualation parameter
   float d=0; //distance between points
   //----Variables to break expression into parts---
   float part1=0;
   float part2=0;
   float lon1=0;  //current longitude in radians
   float lon2=0;  //destination longitude in radians
   float lat1=0;  //current latitude in radians
   float lat2=0;  //destination latitude in radians
   //---Check for Autonomous Mode---
   if(PARAMS.mode == 'A')
   {
   //----Initialize Variables--
   lon1 = GPS.lon*pi/180;
   lon2 = WAYPOINTS.point[WAYPOINTS.current].lon*pi/180;
   lat1 = GPS.lat*pi/180;
   lat2 = WAYPOINTS.point[WAYPOINTS.current].lat*pi/180;
         if (abs(lat2-lat1) < .00000001){
             q=cos(lat1);
         } else {
             part1=tan(lat2/2+pi/4);
             part2=log(part1/tan(lat1/2+pi/4));
             q= (lat2-lat1)/part2;
         }
         part1=q*q*(lon2-lon1)*(lon2-lon1);
         part2=(lat2-lat1)*(lat2-lat1);
         d=sqrt(part2+ part1);
         //---Assign Calculated Distance--- Nautical Miles
         WAYPOINTS.distance = d*((180*60)/pi);
         
   }

}