/* I have created Code to Calculate GPS Distances between Multiple Points on an Arduino Mega 2560 to 0.04 foot accuracy. (Should work on most Arduinos.)
Why did I do this? Because the Arduino is only accurate to 6 Decimal Places when 8 are needed for proper conversion and Havesine Calculations. This code does not convert the NMEA output.  Example 3912.10253 NMEA.
 There are 2 Parts: One for distance and angle between Base and Rover. The second Part is between Rover and the Next Point to go to.  You use saved "Next Points,(SavedLongitude)" to calculate both Distance and Angles from Rover to Next Point you are wanting to go to. The angles are not Global but in reference to each of the points. I could not get Global Angles calculates to any precision that why I used Angles calculation between points. The Base to Rover angles calculated are very close to Haversine Calculations.
Data is from Zed-f9d UART1 Protocol Out ( 0+1 - UBX+NMEA) Bob Brandt, Hartsburg MO. Will Also Work with C94-M8P UART1 Protocol Out. Calculates Feet to the nearest 0.04 Feet. Distance Between Points and Angles are Calculated with Reference to Base and Rover NOT Global Degrees.  */








 //  I am Using ZedF9p Base and Rover but should work with others.
 // On a Mega 2560 board
// This Sketch is to take care of Mega 2560 lack of Precision Only 6 decimal points. Using GPS 

#include <floatToString.h>
#include <math.h>
//Bob Brandt, Hartsburg MO. 
String PartWord;
char  inChar;
float Intermead;
String gpsData;
String  LattRover;
String  LongRover;  
// int     headingDegrees;
char SavedLongitude[20];
char SavedLatitude[20];

void setup() {  
Serial2.begin(38400);   
  Serial.begin(9600);  
}
void loop() { 
gpsData = "";
  while (Serial2.available()) {    
    inChar = Serial2.read();
    gpsData += inChar;
    if (inChar == '$') {
    gpsData = Serial2.readStringUntil('\n');      //Gps Data from Rover      
    }  }
  
  ///////////////////////////////////////////////////////////1st Part Distance and Angles ///////

  String sGPRMC;
  sGPRMC = gpsData.substring(0, 5);
  if (sGPRMC == "GNRMC") {   
    String  LattBase = "3840.3xxx1";    //Enter Your Base Lattitude Here Using NMEA String from GPS Base no Conversion
    String  LongBase = "09214.8xxx4";   //Enter Your Base Longitude Here Using NMEA String from GPS Base no Conversion    
    float   Angles_and_TotalFeet;
    float   LattDiff;
    float   LongDiff;
    int     Mins1;
    int     Mins2;      
    int     Deg1;
    int     Deg2;
    int     Deg_Lon;
    int     Deg_Lat;
    int     Min_Rov_Lon;
    int     Min_Rov_Lat;
    float    Secs1;
    float    Secs2;      
    float   LatRovSecs;  
    float   LonRovSecs;        
    String  a1st;  
    float    Ft_Per_Degree = 316800;    
   LattRover = gpsData.substring(18, 28);
   LongRover = gpsData.substring(31, 42);    
   int NS_POS = gpsData.substring(10, 35).indexOf('N');       
  
  NS_POS = LongBase.indexOf('.');
  a1st   = String(LongBase.substring(0,NS_POS ).toInt());
  Deg1 = (a1st.substring(0, a1st.length()-2 )).toInt(); 
  Mins1 = (a1st.substring(a1st.length()-2 )).toInt(); 
  PartWord = a1st.substring(a1st.length(),2);   
  Mins1 = PartWord.toInt();  
  Secs1  = (LongBase.substring(NS_POS)).toFloat();     
  Secs1 = Secs1 * 60; 
  Ft_Per_Degree = 316800;  
  
  NS_POS = LongRover.indexOf('.');
  a1st   = String(LongRover.substring(0,NS_POS).toInt());
  Deg2 = (a1st.substring(0, a1st.length()-2 )).toInt(); 
  Deg_Lon = Deg2;
  PartWord = a1st.substring(a1st.length(),2);   
  Mins2 = PartWord.toInt();
  Min_Rov_Lon  = Mins2;
  Secs2  = (LongRover.substring(NS_POS)).toFloat();   
  Secs2 = Secs2 * 60; 
  LonRovSecs = Secs2;
  LongDiff = (( (88 * Secs2)- (88 * Secs1)) + ((5280 * Mins2)-(5280 * Mins1)) + ((  Ft_Per_Degree * Deg2)-(Ft_Per_Degree * Deg1 ))  );  
  Ft_Per_Degree =  sin(Deg2 * (PI/180)) * 316800; 

  NS_POS = LattBase.indexOf('.');
  a1st   = String(LattBase.substring(0,NS_POS ));
  Deg1 = (a1st.substring(0, a1st.length()-2)).toInt();  
  PartWord = a1st.substring(a1st.length(),2);   
  Mins1 = PartWord.toInt();     
  Secs1  =(LattBase.substring(NS_POS)).toFloat(); 
  Secs1 = Secs1 * 60;   

  NS_POS = LattRover.indexOf('.');
  a1st   = String(LattRover.substring(0,NS_POS ).toInt());
  Deg2 = (a1st.substring(0, a1st.length()-2)).toInt();
  Deg_Lat = Deg2;
  PartWord = a1st.substring(a1st.length(),2);   
  Mins2 = PartWord.toInt();  
  Min_Rov_Lat = Mins2;
  Secs2  = (LattRover.substring(NS_POS)).toFloat(); 
  Secs2 = Secs2 * 60;  
  LatRovSecs = Secs2;
  LattDiff  = (((88 * Secs2)- (88 * Secs1)) + ((5280 * Mins2)- (5280 * Mins1)) + ((  Ft_Per_Degree * Deg2)-(Ft_Per_Degree * Deg1 ))   );  
  Angles_and_TotalFeet = sqrt(sq(LongDiff) + sq(LattDiff));   
  Serial.println(Angles_and_TotalFeet  + String(" Ft  "));
  
  if (LattDiff > 0 &&  LongDiff > 0) {
   Intermead = 360 + ((atan((LongDiff) / (LattDiff)) * 57.29578)* -1); }
  if ( LongDiff < 0 && LattDiff < 0) {
  Intermead =  90 + atan(( LongDiff ) / (LattDiff )) *  57.29578;  }
  if  (LongDiff  < 0 && LattDiff > 0) {
  Intermead  =  (atan((LongDiff) / (LattDiff)) * 57.29578) * -1;}
  if  (LongDiff  > 0 && LattDiff < 0) {
  Intermead  = 270 + (atan((LongDiff) / (LattDiff)) * 57.29578);   }  
  Serial.println(Intermead + String(" Dg")); 

  ///////////////////////////////////////////////////////////Second Part Distance and Angles Between Rover and Next Point///////
int value = (SavedLongitude[0]);
if ( value > 1 ) {  
  LongBase = "";
  LattBase = "";
  LongBase  = SavedLongitude;    
  NS_POS = LongBase.indexOf('.');
  a1st   = String(LongBase.substring(0,NS_POS ).toInt());
  Deg1 = (a1st.substring(0, a1st.length()-2 )).toInt(); 
  Mins1 = (a1st.substring(a1st.length()-2 )).toInt();  
  Secs1  = (LongBase.substring(NS_POS)).toFloat();     
  Secs1 = Secs1 * 60;    
  Ft_Per_Degree =  316800; 
 LongDiff = (((88 * LonRovSecs)- (88 * Secs1))  +  ((5280 * Min_Rov_Lon)- (5280 * Mins1)) + ((  Ft_Per_Degree * Deg_Lon)-(Ft_Per_Degree * Deg1)));
  LattBase  = SavedLatitude; 
  NS_POS = LattBase.indexOf('.');
  a1st   = String(LattBase.substring(0,NS_POS ));
  Deg2 = (a1st.substring(0, a1st.length()-2)).toInt();  
  Mins1 = (a1st.substring(a1st.length(),2 )).toInt();   
  Secs1  =(LattBase.substring(NS_POS)).toFloat(); 
  Secs1 = Secs1 * 60; 
  LattDiff = (((88 * LatRovSecs )- (88 * Secs1)) +  ((5280 * Min_Rov_Lat)- (5280 * Mins1)) + ((  Ft_Per_Degree * Deg_Lat)-(Ft_Per_Degree * Deg2 )));  
  Angles_and_TotalFeet = sqrt(sq(LongDiff) + sq(LattDiff));  

  Serial.println(Angles_and_TotalFeet + String("'") ); // mower and next point distance apart 
  
 if (LattDiff > 0 &&  LongDiff > 0) {
   Intermead =  360 + ((atan((LongDiff) / (LattDiff)) * 57.29578)* -1);   }
if ( LongDiff < 0 && LattDiff < 0) {
  Intermead =  90 + atan(( LongDiff ) / (LattDiff )) *  57.29578;   }
if  (LongDiff  < 0 && LattDiff > 0) {
  Intermead  =  (atan((LongDiff) / (LattDiff)) * 57.29578) * -1;  }
if  (LongDiff  > 0 && LattDiff < 0) {
  Intermead  = 270 + (atan((LongDiff) / (LattDiff)) * 57.29578);    } 
  
Serial.println(Intermead + String("D"));  
delay(10);    
  
   } 
    }    
     }

/////////////////////////////////////////////////////END OF GPS GUIDANCE//////////////////////////////////////////    
///////////////////////////////////////////////////////