Hi guys,
Just mucking around with things at the moment, however for the life of me, I cant see what is the issue. I know it will be something simple, but im not picking it up.
I have a set of long, lat coordinates that come in from my GPS module, reading that is fine, no issues thus far.
I then take these coordinates and then attempt to project them a given distance at a given heading.
I know what/where the new coordinates should work out to be, however, my code ain't playing ball.
Here is what I have got so far
#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <math.h>
TinyGPS gps;
SoftwareSerial GPS(10,11);
void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
GPS.begin(9600);
}
void loop() {
// put your main code here, to run repeatedly:
float lat, lon;
static float lat1, lon1;
float brng;
float d, R;
brng = 90*(PI/180); //Bearing set (which will actually be compass heading)
d = 1000; //set distance
R = 6371000; //radius of earth
while(GPS.available()){ // check for gps data
if(gps.encode(GPS.read())){ // encode gps data
gps.f_get_position(&lat,&lon); // get latitude and longitude
// display position
Serial.print("Position: ");
Serial.print("lat: ");Serial.print(lat, 8);Serial.print(" ");// print latitude
Serial.print("lon: ");Serial.println(lon, 8); // print longitude
lat1 =asin(sin(lat)*cos(d/R) + cos(lat)*sin(d/R)*cos(brng));
lon1 = lon + atan2(cos(d/R)-sin(lat)*sin(lat), sin (brng)*sin(d/R)*sin(lat));
Serial.println ();
Serial.print("Waypoint Lat: ");Serial.print(lat1, 8);Serial.print(" ");// print new latitude
Serial.print("Waypoint Lon: ");Serial.println(lon1, 8); // print new longitude
Serial.println ();
Serial.println ();
delay(5000);
}
}
}
