How to calculate distance and bearing to a waypoint 03/06/2009Posted by aliasmrjones in The Build.
In the last post we got the LCD working and were properly receiving and parsing data from the gps. The next step was to be able to calculate the distance and bearing to a waypoint. This turned out to be harder than I thought, mostly because we’re dealing with an 8 bit microcontroller and not a modern 32 bit microprocessor. By changing formulas, it did eventually work.
After doing some googling, I found a website with many articles about how to do calculations with latitude and longitude. You can visit the site here. I read that with modern floating point libraries, one can use the fairly simple sperical law of cosines to compute distance between two points defined by latitude and longitude in radians. (You can convert from degrees to radians by using the formula degrees * PI / 180). I created a couple of simple functions to convert back and forth between degrees and radians.
//convert degrees to radians
float dtor(float degrees)
return(degrees * PI / 180);
float rtod(float radians)
return(radians * 180.0 / PI);
The spherical law of cosines can be expressed in Excel like this: =ACOS(SIN(lat1)*SIN(lat2)+COS(lat1)*COS(lat2)*COS(lon2-lon1))*6371. 6371 is the radius of the earth in kilometers. You can substitute 3,956 miles or even 20925656.2 feet to get distance in other units. Thanks to a coastal navigation class I took a few years ago, I know that 1 minute of latitude is equal to 1 nautical mile. 1 degree of latitude is, therefore, 60 nautical miles. I made a spreadsheet in Excel, put in my approximate location in lat/lon as point 1 and lat – 1/same lon as point 2. This should give me a distance of exactly 60 nautical miles or about 69 statute miles. Excel reported 69 miles – success!.
I translated the formula to C and changed my AVR program to calculate the distance between 2 points and display the result on the LCD. I put in the same 2 points of lat/lon and got the same 69ish miles as I got in Excel. Great! I then put in a new point 2 only about 20 feet away from point 1. The LCD showed…several miles distance. Hmmmm.
The problem is that at least in the free GCC compiler for AVR, there is no difference between float and double data types. Both are 4 byte floating point numbers. The spherical law of cosines works well for small distances with real 8 byte doubles, but not with 4 byte floats.
The haversine formula, however, does work well for small distances with 4 byte floats.
dlon = lon2 – lon1
dlat = lat2 - lat1 a = (sin(dlat/2))^2 + cos(lat1) * cos(lat2) * (sin(dlon/2))^2 c = 2 * atan2(sqrt(a), sqrt(1-a)) d = R * c
R is the radius of the earth like in the law of cosines formula. You can get the result in miles, kilometers or feet by using an appropriate value for R. In this case, I decided to use the value 20925656.2 to return the distance in feet. After changing the code for the calculate distance function, I got something close to 20 feet and figure this is close enough given the accuracy of gps. One thing you need to be careful of is making sure the atan2 function has the arguments in the right order. The Excel and the AVR GCC math implementations of atan2 take the arguments in the opposite order and I got bit by this at first.
So, now a bit more about printing to the LCD. The LCD I am using is 16 characters by 2 lines. The AVRLib includes some functions to send characters to the display, but also has a handy rprintf function that you can set to output to anything with a “print a single character” function.
After setting up the LCD with
You can set up rprintf to print to the LCD with
Now, you can print a string to the LCd with
rprintfStr(“Message to print”);
You can move the cursor to a specific spot on the LCD with lcdGotoXY(x,y). So, for example, to move the cursor to the beginning of the second line you would use
Printing a floating point number is easily done with:
lcdClear() does just what it sounds like. Putting it all together, to print latitude and heading on line 1 and longitude and speed on line 2, a kind of status dashboard:
Here is a pic showing the readout produced by the above code:
The rprintf library takes up a lot of memory and it isn’t always the most efficient way to display things on the LCD, but it is often the easiest way and, at least at the moment, we have plenty of flash program memory and cpu cycles to spare.
So, now we are getting the basic gps data we need and are able to figure out where we should head and how far it is to our destination. The next step is to create a way to store a series of waypoints in the Atmega32’s EEPROM and then head for each waypoint in sequence. That is the topic for our next post.