# GPS code destination point given distance and bearing from start point

Hi all
I have seen plenty of code t navigate between waypoints or calculate distance etc but I am looking for some code to calculate a destination point using a given distance and bearing from my start point.

So if I have a known set of coordinates I want to be able to enter the distance and bearing and determine the new coordinates for my next waypoint.

I think I have found the math for it here using this calculator but I want to do it in code
http://www.movable-type.co.uk/scripts/latlong.html
I have also found a possible solution in java and several other pages that are also in java.
There may also be some usable code in the Ardupilot project.

``````JavaScript:
var lat2 = Math.asin( Math.sin(lat1)*Math.cos(d/R) +
Math.cos(lat1)*Math.sin(d/R)*Math.cos(brng) );
var lon2 = lon1 + Math.atan2(Math.sin(brng)*Math.sin(d/R)*Math.cos(lat1),
Math.cos(d/R)-Math.sin(lat1)*Math.sin(lat2));
Excel:	lat2: =ASIN(SIN(lat1)*COS(d/R) + COS(lat1)*SIN(d/R)*COS(brng))
lon2: =lon1 + ATAN2(COS(d/R)-SIN(lat1)*SIN(lat2), SIN(brng)*SIN(d/R)*COS(lat1))
* Remember that Excel reverses the arguments to ATAN2 – see notes below
``````

Basically just wondering if its doable or if anyone knows of a project or sketch that may contain the maths I am looking for.

Thanks

Apparently this is the code using processing but I do not know how to change to code for the arduino. Googloing that now

``````  float CurLon = 0.000;
float CurLat = 52.000;
float Bearing = 45; // Bearing of travel
float Distance = 10; // km per update

void setup()
{
DestLon = (DestLon+3*PI)%(2*PI) - PI;  // normalise to -180..+180º
System.out.printf("starting at a Longitude of %f and a Latitude of %f ",CurLon,CurLat);
System.out.printf("if we travel %f km on a bearing of %f degrees ",Distance,Bearing);
System.out.printf("we end up at Longitude of %f and a Latitude of %f ",degrees(DestLon),degrees(DestLat));
}
``````

A convenient and complete reference on the math (for very accurate calculations) is given here : http://www.icsm.gov.au/gda/gdatm/gdav2.3.pdf

For your particular question, see Vincenty's Direct formulae in section 4. Keep in mind that the Arduino performs only single-precision floating point operations, which really is not sufficient for this purpose.

Thanks. I will use that to confirm the maths is correct

I fixed up the code to run on an arduino:

``````float CurLon = 0.000;
float CurLat = 52.000;
float r_CurLon;
float r_CurLat;
float Bearing = 45; // Bearing of travel
float r_Bearing;
float Distance = 10; // km per update

void setup(void)
{
Serial.begin(9600);
while(!Serial);
delay(1000);
DestLon = (DestLon+3*PI)/(2*PI);
int i = DestLon;
DestLon = (DestLon - i) * (2*PI) - PI;  // normalise to -180..+180º

//System.out.printf("starting at a Longitude of %f and a Latitude of %f ",CurLon,CurLat);
Serial.print("starting at a Longitude of ");
Serial.print(CurLon,6);
Serial.print(" and a Latitude of ");
Serial.println(CurLat,6);
//System.out.printf("if we travel %f km on a bearing of %f degrees ",Distance,Bearing);
Serial.print("if we travel ");
Serial.print(Distance,6);
Serial.print(" km on a bearing of ");
Serial.print(Bearing,6);
Serial.println(" degrees");
//System.out.printf("we end up at Longitude of %f and a Latitude of %f ",degrees(DestLon),degrees(DestLat));
Serial.print("we end up at a Longitude of ");
Serial.print(degrees(DestLon),6);
Serial.print(" and a Latitude of ");
Serial.println(degrees(DestLat),6);
}

void loop(void)
{
}
``````

which outputs:

``````starting at a Longitude of 0.000000 and a Latitude of 52.000000
if we travel 10.000000 km on a bearing of 45.000000 degrees
we end up at a Longitude of 0.103423 and a Latitude of 52.063541
``````

Is this result what you were expecting?

Pete

According to the this site, that worked quite well. Calculate distance and bearing between two Latitude/Longitude points using haversine formula in JavaScript

Thanks very much for doing that. Appreciate it

Providing I entered it correctly on that website using 0 degrees 0 minutes 0 seconds North latitude and 52 degrees 0 minutes 0 seconds East longitude as both North and East are positive numbers then I get back a destination of
0 degrees 3 minutes 49 seconds N and 052 degrees 3 minus 49 seconds E which converted to decimal using the following page
http://transition.fcc.gov/mb/audio/bickel/DDDMMSS-decimal.html is 0.063611 latitude and 52.063611 longitude

Or am I using the script wrong and converting incorrectly?

Does the bearing also get entered as 45 degrees 0 minutes 0 seconds?

EDIT - Found another bit of code for this function but its not functional. I think its a snippet from his full project found by searching for jimthree HABsim

JimthreeHABsim.ino (6.69 KB)

You've reversed the Lat and Long. Your code uses Lat 52N, long 0E.

Pete

Lol. And I thought I had double checked that.

It works perfectly.