Can anyone please help me with some aurdino code to determine that a GPS waypoint has been reached, and to set a radius within which the waypoint is legitimate.
I am using the UNO and EMS406 GPS module.
Many thanks.
Here is the code I use.
float Distance( float Lat1, float Lon1, float Lat2, float Lon2)
{
float dLat = radians( Lat2 - Lat1 );
float dLon = radians( Lon2 - Lon1 );
float a = sin( dLat / 2.0f ) * sin( dLat / 2.0f ) +
cos( radians( Lat1 ) ) * cos( radians( Lat2 ) ) *
sin( dLon / 2.0f ) * sin( dLon / 2.0f );
float d = 2.0f * atan2( sqrt( a ), sqrt( 1.0f - a ) );
return d * EARTH_RADIUS_METERS ;
}
And then.
if (Distance( current_latitude, current_longitude, dest_latitude, dest_longitude) < 10) // ten meters
//Do whatever you need once you reach the waypoint
All you need is you lat/long and the waypoint lat/long
Evil Genius jr.,
Do you know where this formula came from? In the few minutes I had to look at it, I could not make sense of it. At first it looked like a law of cosines. Then it looked like a great circle equation, but none seem to match. “d” is the result of an ATAN calculation and hence an angle in radians. This is multiplied by 2 and then a radius leading to a circumferance. So ‘a’ must somehow be associated with the angle sustained by the two waypoints through the center of the earth. Those angles must be very small and I wonder about roundoff issue.
My method would assume a localized flat earth and simply account for the foreshortening on longitude angles by dividing by the cosine of the latitude. Less technical but perhaps faster and less prone to rounding issues? Comments welcome.
It came from here http://www.sparkfun.com/tutorials/204
Sorry in the above code I forgot to include the defines. Hope this helps.
Also the Wiring NMEA library has the function built in.
Haversine formula. Interesting. Must investigate…
Thanks for the code evilgenius.
I have tried building it into my sketch without success. Get errors about function definition not being allowed. I think it refers to the line : float Distance( float Lat1, float Lon1, float Lat2, float Lon2, float unit_conversion )".
I have copied some of my sketch below in the hope that someone can spot my error(s). Thanks.
void loop()
{if (nss.available() > 0 ) {
// read incoming character from GPS
char c = nss.read();
// check if the character completes a valid GPS sentence
if (gps.decode(c)) {
// check if GPS positioning was active
if (gps.gprmc_status() == ‘A’) {
if (pset==0){ //set home lat & lon once only
hlat=gps.gprmc_latitude();
hlon=gps.gprmc_longitude();
pset=1; }
{Lat1=gps.gprmc_latitude(); // Set Lat1 & Lon1 to current latitude & longitude
Lon1=gps.gprmc_longitude();}
if (Q==1) {wp1lat = hlat+dxlat;} //waypoint 1 is the home position plus some
if (Q==1) {wp1lon = hlon+dylon;}
if (Q==2) {wp1lat = hlat-dxlat;}
if (Q==2) {wp1lon = hlon-dylon;}
if (Q==3) {wp1lat = hlat+dxlat;}
if (Q==3) {wp1lon = hlon+dylon;}
if (Q==4) {wp1lat = hlat-dxlat;}
if (Q==4) {wp1lon = hlon-dylon;}
Lat2= wp1lat; // set Lat2 & Lon2 to the waypoint target
Lon2=wp1lon;
{Serial.print("Destination lat first loop: ");Serial.println(wp1lat,7);
Serial.print("Destination lon first loop: ");Serial.println(wp1lon,7);
z=1;
Serial.print("First setting of z should be =1 z= ");Serial.println(z);}
}}}
//Distance calculation function //Distance calculation function //Distance calculation function
float Distance( float Lat1, float Lon1, float Lat2, float Lon2, float unit_conversion )
{
float dLat = radians( wp1lat - Lat1 );
float dLon = radians( wp1lon - Lon1 );
float a = sin( dLat / 2.0f ) * sin( dLat / 2.0f ) +
cos( radians( clat ) ) * cos( radians( wp1lat ) ) *
sin( dLon / 2.0f ) * sin( dLon / 2.0f );
float d = 2.0f * atan2( sqrt( a ), sqrt( 1.0f - a ) );
return d * EARTH_RADIUS_METERS ;
Serial.print("d in distance calculation= "); Serial.println(d); }
if(distance(Lat1,Lon1,Lat2,Lon2) < 10)
{
Serial.println(“WP reached”);
}
}
Ok spotted a couple of errors. First you did not have enough brackets in you main loop. Secondly You never called the distance function in the main loop. And the last one I found was in the distance code you print something after the ```
Return
Try this code. Substitute your own way points in the Trackwpts switch statement.
#include <TinyGPS.h>
float dest_latitude;
float dest_longitude;
float latitude, longitude;
double current_latitude, current_longitude;
double last_latitude,last_longitude;
;
float alpha = .8;
int wpt = 0;
TinyGPS gps;
#define EARTH_RADIUS_METERS 6372795.0f
void setup()
{
Serial.begin(4800);
delay(10000);
gps.f_get_position(&latitude, &longitude);
last_latitude = latitude;
last_longitude = longitude;
}
void loop()
{
GPSFilter();
trackWpts();
Serial.print(“destination Longitude:”);
Serial.println(dest_longitude,6);
Serial.print(“destination Latitude:”);
Serial.println(dest_latitude,6);
}
float GPSFilter()
{
gps.f_get_position(&latitude, &longitude);
current_latitude = (latitudealpha)+ (1-alpha) last_latitude;
current_longitude = (longitudealpha)+ (1-alpha) last_longitude;
last_latitude = current_latitude;
last_longitude = current_longitude;
}
void trackWpts() {
switch(wpt) {
case 0:
dest_latitude =00.982541;
dest_longitude = -00.307675;
break;
case 1:
dest_latitude = 00.98251;
dest_longitude = -00.307621;
break;
case 2:
dest_latitude = 00.982488;
dest_longitude = -00.307666;
break;
case 3:
dest_latitude = 00.982534;
dest_longitude = -00.307716;
break;
case 4:
dest_latitude = 00.00000;
dest_longitude = 00.0000;
break;
}
if (Distance( current_latitude, current_longitude, dest_latitude, dest_longitude) < 10)
wpt++;
}
float Distance( float Lat1, float Lon1, float Lat2, float Lon2)
{
float dLat = radians( Lat2 - Lat1 );
float dLon = radians( Lon2 - Lon1 );
float a = sin( dLat / 2.0f ) * sin( dLat / 2.0f ) +
cos( radians( Lat1 ) ) * cos( radians( Lat2 ) ) *
sin( dLon / 2.0f ) * sin( dLon / 2.0f );
float d = 2.0f * atan2( sqrt( a ), sqrt( 1.0f - a ) );
return d * EARTH_RADIUS_METERS ;
}