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.
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
int Eradius = 6371; // mean radius of the earth
void setup()
{
float DestLat = asin(sin(radians(CurLat))*cos(Distance/Eradius)+cos(radians(CurLat))*sin(Distance/Eradius)*cos(radians(Bearing)));
float DestLon = radians(CurLon) + atan2(sin(radians(Bearing))*sin(Distance/Eradius)*cos(radians(CurLat)),cos(Distance/Eradius)-sin(radians(CurLat))*sin(DestLat));
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));
}
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.
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
int Eradius = 6371; // mean radius of the earth
void setup(void)
{
Serial.begin(9600);
while(!Serial);
delay(1000);
r_CurLon = radians(CurLon);
r_CurLat = radians(CurLat);
r_Bearing = radians(Bearing);
float DestLat = asin(sin(r_CurLat)*cos(Distance/Eradius)+cos(r_CurLat)*sin(Distance/Eradius)*cos(r_Bearing));
float DestLon = r_CurLon + atan2(sin(r_Bearing)*sin(Distance/Eradius)*cos(r_CurLat),cos(Distance/Eradius)-sin(r_CurLat)*sin(DestLat));
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
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 Degrees Minutes Seconds to/from Decimal Degrees | Federal Communications Commission 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