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
Statement. Well that will never print because the return statement ends the function.
Try this code. Substitute your own way points in the Trackwpts switch statement.
Code: Select all#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 = (latitude*alpha)+ (1-alpha)* last_latitude;
current_longitude = (longitude*alpha)+ (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 ;
}