How to Calculate Compass Heading from GPS Points

How to Calculate Compass Heading from GPS Points

So you’re building a robot with an Arduino, GPS, a compass and you want it to know which direction to turn so it goes to a waypoint?  Me too!   Here is my demo sketch on how to calculate the waypoint heading.   It amounts to using the atan2 function to calculate the angle between two points, converting the angle to degrees, then normalizing it to a compass reading.

The below values are hardcoded. You can run the sketch yourself to test the calculation.   I’ve spot tested points between all the 90 degree angles but did not include them for brevity.  If you want to modify with your own values replace the variables currentLatitude and currentLongitude with your own coordinates in Decimal Degrees format. Simply go to Google Maps, right click a place on the map, select “What’s here?”. At the bottom click the link to the coordinates. From there you can copy the values out of the text box and paste it directly in to the sketch. Do the same for your destination and paste it in the call to getWayPointHeading().


/*
 * This sketch demonstrates how to calculate a compass heading from two GPS points.
 * Prints out the 0-360 compass heading from the current location to the target waypoint
 */

// Sample GPS coordinates representing the current location. 
float currentLatitude  =   38.590214;
float currentLongitude =  -121.767142;


void setup() {
  Serial.begin(9600);
}

void loop() {

  Serial.print("North: ");
  Serial.println(getWaypointHeading(38.601164, -121.767199));  // a point due north

  Serial.print("East: ");
  Serial.println(getWaypointHeading(38.590264, -121.761617));  // a point due east

  Serial.print("South: ");
  Serial.println(getWaypointHeading(38.586864, -121.767143));  // a point due south

  Serial.print("West: ");
  Serial.println(getWaypointHeading(38.590215, -121.803073));  // a point due west
 
  while(1);

}

/*
 * This function takes a lat/log and returns the 0-360 compass heading from the current 
 * globally defined lat/log
 */
int getWaypointHeading(float targetLatitude, float targetLongitude) {

  // radians
  float heading = atan2(targetLatitude - currentLatitude, targetLongitude - currentLongitude);

  // convert to degrees 0 to 180/-180
  heading *= (180 / 3.14159);

  // convert to 0-360 compass degrees, North = 0
  heading = (450 - (int)heading) % 360;

  return heading;

}

Sample Output


North: 0
East: 90
South: 180
West: 270

One thought on “How to Calculate Compass Heading from GPS Points

Leave a Reply

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.