Calculating Bearing and Distance to a Latitude Longitude Waypoint with AVR Atmega C

By aliasmrjones - Last updated: Tuesday, May 19, 2009 - Save & Share - One Comment

This post will show how to calculate bearing and distance to a latitude longitude waypoint using C on an Amtel AVR microcontroller. Deathpod3000 uses the formulas I gave in the last post, but with minor changes to adapt to C on the AVR. The code should be easy to adapt to other microcontrollers like PIC, or even other languages such as basic or java. So, let’s get to the real work – the code!

The first thing I’ll cover is calculating distance. As I mentioned in the last post, I used the haversine formula to calculate distance because it works well with the 4 byte floating point numbers the AVR C libraries use even for distances as small as 1 meter. The spherical law of cosines is another popular formula and works well in Excell and on modern processors with 8 byte floating point numbers, but it break down at small distances with 4 byte floating points like the AVR ATmega line. The error is too great with only 7 decimal places of accuracy so the spherical law of cosines will work great for calculating something like 60 miles, but will be very inaccurate for distances like 20 feet, which is what we really care about for navigating with a small rc car.

First, for the distance and bearing formulas to work, latitude and longitude must be in radians, not degrees. It is very easy to convert between degrees and radians and I created a function to convert from degrees to radians and a function to convert from radians to degrees. I like to display latitude and longitude in degrees even when storing and calculating with radians.

//convert degrees to radians
float dtor(float degrees)
{
return(degrees * PI / 180);
}

//Convert radians to degrees
float rtod(float radians)
{
return(radians * 180.0 / PI);
}

Now, for the haversine formula calculation…

//Calculate distance form lat1/lon1 to lat2/lon2 using haversine formula
//Note lat1/lon1/lat2/lon2 must be in radians
//Returns float distance in feet
float calcDistance(float lat1, float lon1, float lat2, float lon2)
{
float dlon, dlat, a, c;
float dist = 0.0;
lon1 = lon1 * -1.0; //make west = positive
lon2 = lon2 * -1.0;
dlon = lon2 – lon1;
dlat = lat2 – lat1;
a = pow(sin(dlat/2),2) + cos(lat1) * cos(lat2) * pow(sin(dlon/2),2);
c = 2 * atan2(sqrt(a), sqrt(1-a));

dist = 20925656.2 * c; //radius of the earth (6378140 meters) in feet 20925656.2
return(dist);
}

I decided to make west positive since Deathpod3000 will always be in the western hemisphere so both latitude and longitude will be positive. The formula is the same as the one I used the spreadsheet in the last post, except the atan2 function in the AVR C library has the function parameters in the opposite order as Excel. Actually, Excel seems to have the parameters in the opposite order as the rest of the world lol. (If you’d like a spreadsheet that already has the fomulas for distance and bearing that you can play around with, you can download one at the bottom of my last post.)

You can change the distance to any units you’d like by plugging in the radius of the earth in those units. Use 6,378,140 to get the distance in meters, for example, or 3,959 for miles. I used feet, since an rc car is probably not going to care about anything larger than this.

This function simply takes 2 latitude longitude coordinates as floats and return a float distance in feet. I call this function in my main navigation loop and then make decisions about moving to the next waypoint and what to set the throttle to based on the distance.

Here is the C code to calculate bearing to a latitude longitude waypoint.

//Calculate bearing from lat1/lon1 to lat2/lon2
//Note lat1/lon1/lat2/lon2 must be in radians
//Returns float bearing in degrees
float calcBearing(float lat1, float lon1, float lat2, float lon2)
{
float bearing = 0.0;
//determine angle
bearing = atan2(sin(lon2-lon1)*cos(lat2), (cos(lat1)*sin(lat2))- (sin(lat1)*cos(lat2)*cos(lon2-lon1)));
//convert to degrees
bearing = rtod(bearing);
//use mod to turn -90 = 270
bearing = fmod((bearing + 360.0), 360);
return bearing;
}

Again, the latitudes and longitudes must be in radians. Lat1/lon1 is the current location and lat2/lon2 is the waypoint where you want to go. The fmod() function is used to make sure we end up with 0-359.9 degrees.

You might wonder why the bearing is returned in degrees rather than raidans since everything else seems to be in radians. The i2c compass module I’m using gives heading in degrees so I kept heading and bearing in degrees and latitude longitude coordinates in radians.

To make steering easier, I also created a function to return relative bearing given bearing to waypoint and the current heading. The relative bearing allowed me to easily create a proportional steering function that steers farther to the side the larger the relative bearing. Here is the relative bearing function.

//Calculate relative bearing given bearing and heading
//bearing and heading are in degrees
float CalcRelBearing(float bearing, float heading)
{
float relBearing;
relBearing = bearing – heading;
if(relBearing > 180.0)
{
relBearing = bearing – (heading + 360);
}
if(relBearing < -180.0)
{
relBearing = bearing – (heading – 360);
}
return(relBearing);
}

This function returns +180 – -180 degrees relative bearing with 0 being right on target. Now to steer, I just have to multiple this value by some scaling constant and then add/subtract the result from center servo pulse width to steer the car proportional to the heading error.

//Steer will move steering servo to approriate point based on relative bearing
//OCR1A is set to 1000 full left, 1470 center, 2000 full right to steer
void Steer(float relBearing)
{
int OcrValue = 0;
OcrValue = (relBearing * 6) + 1470;
if(OcrValue > 2000)
{
OcrValue = 2000;
}
if(OcrValue < 1000)
{
OcrValue = 1000;
}
OCR1A = OcrValue;
}

My steering servo was not quite perfect. Normally 1500 is dead center for a servo, but my car steered slightly to the side at 1500. After numerous trials, 1470 seemed to steer the car straight ahead. By changing the scale value of 6 in “(relBearing * 6)” to other values, you can change how fast it steers to correct for heading error. Higher value means more aggressive steering, lower value means less aggressive steering. I did trial runs until I was happy with how fast it was steering to course and steering away from obstacles.

The code also ensures we don’t damage the servo by sending pulses greater than 2 ms (2000 us) or less than 1 ms. If the value after scaling is out of range, the function simply sets the result to the largest or smallest allowable value. Finally, it sets OCR1A, which is the PWM pin I used to control steering to the resulting value, which moves the servo and turns the front wheels. To learn about how I control the steering servo, see this previous post about controlling servos with AVR Atmega microncontrollers.

That’s all you need for basic navigation. If you have questions about anything, please leave a comment or send me an email through the contact page.

Share and Enjoy: