The aim of my work is given in the algorithm below.
- Initialize the H/W; wait until GPS module has a valid fix and then start the motor.
- Get actual position from the GPS.
- Calculate the distance and heading information to the next waypoint.
- Compare the actual heading with the current heading angle and feed the difference to the motors.
- When the actual position is close to the first waypoint, switch to the next waypoint.
- If/when the last waypoint is reached, stop the motor.
- If not go to step 2.
I'm calculating the distance between a pre-defined coordinate and the current location using Haversine formula. I need heading angle for the same pair of coordinates. But when I use the atan2 formula and compile using the Arduino IDE, I am getting the angle as 0.05,0.03. Am I doing it wrong? can the heading angle be calculated for points within a proximity of 10m?
formula
double y = sin(dlon) * cos(lat2);
double x = ((cos(lat1) * sin(lat2)) - (sin(lat1) * cos(lat2) * cos(dlon)));
double brng = atan2(y, x);
