/**
  * This file uses Rhumb Line Navigation to calculate the heading 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.
  * North West hemishpere is assumed!
  *
**/
void calc_heading()
{
   //---Local Variables---
   float tc=0;    //true calculated course
   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
   float part1=0; //break expression up
   float part2=0; //break expression up
   //---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;
   //---Perform Expression in Parts---
   part1=log(tan(lat2/2+pi/4)/tan(lat1/2+pi/4));
   part2=atan2(lon1-lon2,part1);
   tc = fmod(part2,2*pi);
   //---Set Desired Heading to true course in degrees---
   hCONTROL.desired = tc*180/pi;
   }
}