Putting Compass and GPS on same circle


I’m trying to figure out a solution to my problem of how to map a digital compass with GPS on the same circle. When the heading of the GPS (seperating 2 GPS points) and the compass heading are within an acceptable range from each other (~15 degrees offset?), the robot knows it is pointing in a line from GPS A (current location) to GPS B (destination location).

Compass: Overview | Adafruit HMC5883L Breakout - Triple-Axis Magnetometer Compass Sensor | Adafruit Learning System

(I calibrated it for my current location with angle of declination correcting so it is now on true north.)

GPS: Ultimate GPS Shield, http://www.adafruit.com/product/1272

Board: Arduino UNO R3

For the compass, it is within 10 deg North heading of a digital compass used by a smart phone (with airplane mode enabled). It seems OK. As my compass is using true north now, I would think they are on the same circle. Here is my code for the compass to get degrees heading:

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);

void displaySensorDetails(void)
  sensor_t sensor;
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); 

Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); 

Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); 

Serial.println(" uT");  

void compass(void)
   /* Get a new sensor event */ 
  sensors_event_t event; 

  // Display the results (magnetic vector values are in micro-Tesla (uT)) 
  Serial.print("X: "); Serial.print(event.magnetic.x); Serial.print("  ");
  Serial.print("Y: "); Serial.print(event.magnetic.y); Serial.print("  ");
  Serial.print("Z: "); Serial.print(event.magnetic.z); Serial.print("  


  // Hold the module so that Z is pointing 'up' and you can measure the heading with x&y 
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/ (Note - NOT USED)
       // NOTE:  I used this website:  http://www.ngdc.noaa.gov/geomag-

  // Mine is: -13* 2' W, which is ~13 Degrees, or (which we need) 0.22 radians (test)
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.

  float declinationAngle = 0.22 // sample - will vary based on your GPS 


  heading += declinationAngle;
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 
  Serial.print("Heading (degrees): ");   

void setup(void) 
  Serial.println("HMC5883 Magnetometer Test"); Serial.println("");
  /* Initialise the sensor */
    /* There was a problem detecting the HMC5883 ... check your connections */
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
  /* Display some basic information on this sensor */

void loop(void) 


For the GPS, I made the following code for the math to determined the

heading in terms of degres that the robot should drive to (all of this code is later combined to compare compass and GPS headings):

/* angle heading off of North seperating the 2 GPS points, sweeping 

clockwise (but using Quadrants)

   GPS_heading = (180 / M_PI) * (atan((GPS_B_long - GPS_A_long) / (GPS_B_lat - GPS_A_lat))); //M_PI is pi in math library

   This will put a clockwise rotation on the angle between north and a rotating line connecting GPS A and GPS B
   Note: North and South have same equations, but West and East are reversed

//  1st quadrant, angle taken off of vertical (+), NORTH
if ((GPS_B_long > GPS_A_long) && (GPS_B_lat > GPS_A_lat))
     {GPS_heading = ((180 / M_PI) * (atan((GPS_B_long - GPS_A_long) / (GPS_B_lat - GPS_A_lat))));} //M_PI is pi in math library     
        // 4th quadrant, angle taken off of horizontal (+), EAST
       else if  ((GPS_B_long > GPS_A_long) && (GPS_B_lat < GPS_A_lat))
         {GPS_heading = 90 + abs(((180 / M_PI) * (atan((GPS_B_lat - GPS_A_lat)/(GPS_B_long - GPS_A_long)))));}
       // 3rd quadrant, angle taken off of vertical (-), SOUTH   
       else if  ((GPS_B_long < GPS_A_long) && (GPS_B_lat < GPS_A_lat))
         {GPS_heading = 180 + abs(((180 / M_PI) * (atan((GPS_B_long - GPS_A_long) / (GPS_B_lat - GPS_A_lat)))));}
       // 2nd quadrant, angle taken off of horzontal (-), WEST   
       else if  ((GPS_B_long < GPS_A_long) && (GPS_B_lat > GPS_A_lat))
         {GPS_heading = 270 + abs(((180 / M_PI) * (atan((GPS_B_lat - GPS_A_lat)/(GPS_B_long - GPS_A_long)))));}

Problem: When they are put on the same circle, they appear to be 65 degrees off. That is, the compass, when pointing at GPS location B, it reads 65 degrees greater than the GPS heading. (I am able to visually see and point the compass at GPS location B.) For example, if the GPS B heading is at 80 degrees (indicating a NE direction), the compass reads 145 degrees.

I could correct this by subtracting 65 from the compass heading read out, and if it becomes negative, adding 360 to that resulting value, but I’m not sure if that is the correct solution.

So I’m not sure what to do. The compass seems fairly accurate following testing against the digital compass on the phone, and I think the math for the GPS seems ok.

I’m a bit stuck here what is going on?

I rely on this excellent web page for GPS navigation formulas http://movable-type.co.uk/scripts/latlong.html Suggest you check them out, especially the entries regarding bearing.