Bearing using two sets of coordinates (Latitude and Longitude) in C

Question

I have began working on an autonomous rc helicopter in c. I need help finding a way to calculate the bearing acuratley. I am using two sets of coordinates (latitude and longitude), one is the current location and the other is the destination point. I have converted the latitude and longitude into decimal format so....

40°58'19.86"N = 40.972183
74°14'52.74"W = 74.247983

Can anyone show me code in c to find the bearing or a formula i can use?

i have looked at: http://www.movable-type.co.uk/scripts/latlong.html and when i transfer it to c, the results make no sense.

This is what i have tried:

 double x = Sin(Longitude2 - Longitude1) * Cos(Latitude2);
 double y = Cos(Latitude1) * Sin(Latitude2) - Sin(Latitude1) * Cos(Latitude2) * Cos(Longitude2 - Longitude1);
 double heading = (Atan2(x, y) % 2 * 3.14159265) (180/3.14159265);

Answer 1

Have you converted your coordinates from degrees to radians before calculations ?

angleRad = 3.14159265 * angleDeg / 180; And

bearing = Atan2(y, x); in that website. The bearing should be converted from rad to deg in turn:

bearing = 180 * bearing / 3.14159265; and in case of negative value eventually:

bearing = bearing + 360;

I don't know how to write this using the convention above (with "%").

Latitude1 and Longitude1 are coordinates of the observer Latitude2 and Longitude2 are coordinates of destination point.

Formulas from that website work well.