So.... I'm currently working on an Auto-Pilot based quadcopter using an Arduino Mega 2560 with a bunch of add-on modules. That will probably prompt its own thread at some point.

In the mean time, one of the things I needed to work out was distance and bearing calculations from the drone's current GPS coordinates to its destination GPS coordinates. I thought maybe it would be useful to others to share this here.

The reason I thought others might find this useful was for the bearing calculations. I found every wrong way possible to do this scattered online. To make matters worse, I'm not a math genius by any stretch of the imagination.

It took me decomposing the functions on the igismap website and doing the math by hand to figure out where I was going wrong. It took me the better part of a day to get to this solution.

This program uses the Haversine Formulas for both distance and bearing. The distance functions were derived from:

http://www.movable-type.co.uk/scripts/latlong.html and the bearing functions were derived from

http://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/.

`#include <math.h>`

double lat1;

double lat2;

double lon1;

double lon2;

double latR1;

double latR2;

double lonR1;

double lonR2;

double dlon;

double dlat;

double a;

double e;

double d;

double R = 6371.00;

double toDegrees = 57.295779;

char sb[10];

/*-----------------------------Setup---------------------------------*/

void setup() {

// put your setup code here, to run once:

Serial.begin(115200);

}

/*-----------------------------Main Loop---------------------------------*/

void loop() {

//Get Starting GPS Coordinates

Serial.println("\nInput Start Latitude: ");

while(Serial.available() == 0){}

Serial.readBytes(sb, 10);

lat1 = atof(sb);

Serial.println("Input Start Longitude: \n");

while(Serial.available() == 0){}

Serial.readBytes(sb, 10);

lon1 = atof(sb);

String Start = "GPS Starting Location: " + String(lat1, 6) + ", " + String(lon1, 6);

Serial.print(Start);

//Get Destination GPS Coordinates

Serial.println("\nInput Destination Latitude: ");

while(Serial.available() == 0){}

Serial.readBytes(sb, 10);

lat2 = atof(sb);

Serial.println("Input Destination Longitude: \n");

while(Serial.available() == 0){}

Serial.readBytes(sb, 10);

lon2 = atof(sb);

String Dest = "GPS Destination Location: " + String(lat2, 6) + ", " + String(lon2, 6);

Serial.print(Dest);

Serial.println();

calcDist(); //Call the distance and bearing calculation function

}

/*-----------------------------Distance & Bearing Calculator---------------------------------*/

void calcDist(){ //This is a haversine based distance calculation formula

//This portion converts the current and destination GPS coords from decDegrees to Radians

lonR1 = lon1*(PI/180);

lonR2 = lon2*(PI/180);

latR1 = lat1*(PI/180);

latR2 = lat2*(PI/180);

//This portion calculates the differences for the Radian latitudes and longitudes and saves them to variables

dlon = lonR2 - lonR1;

dlat = latR2 - latR1;

//This portion is the Haversine Formula for distance between two points. Returned value is in KM

a = (sq(sin(dlat/2))) + cos(latR1) * cos(latR2) * (sq(sin(dlon/2)));

e = 2 * atan2(sqrt(a), sqrt(1-a)) ;

d = R * e;

Serial.println();

Serial.print("Distance to destination(KM): ");

//Serial.println(a);

//Serial.println(e);

Serial.println(d, 6);

Serial.println();

//This portion is the Haversine Formula for required bearing between current location and destination. Returned value is in Degrees

double x = cos(latR2)*sin(lonR2-lonR1); //calculate x

Serial.print("X = ");

Serial.println(x, 6);

double y = cos(latR1)*sin(latR2)-sin(latR1)*cos(latR2)*cos(lonR2-lonR1); //calculate y

Serial.print("Y = ");

Serial.println(y, 6);

float brRad = atan2(x, y); //return atan2 result for bearing. Result at this point is in Radians

Serial.print("atan2(x, y) (Radians) = ");

Serial.println(brRad, 6);

float reqBear = toDegrees*brRad;

Serial.print("Bearing: ");

Serial.println(reqBear, 4);

}

A side note about the bearing calculations. atan2(x, y) returns a value in Radians. Not only that, but the Start and End GPS Coordinates must be converted from Decimal Degrees in to Radians before they're plugged in to the equation. The GIS site says nothing about that and I spent 2 hours using their equations as written on their site and kept coming out with the wrong answers. After I figured out that I needed to convert to Radians and then back from Radians, it was a pretty quick task.

P.S. Entering GPS coordinates to 6 decimal places is not accurate. The Serial Output of the coordinates does not match. I know it's because double is not truly double and it's rounding as a result. Despite that, I've still found distance to be accurate within about 15 feet and bearing within about .001 degrees on average. For my uses, it'll be close enough.

Hopefully, this will save someone time in the future.