# 3 Axis Magnetometer Tilt Compensation

This page shows you 3 axis Magnetometer tilt compensation using an accelerometer and outputs the results to a small OLED display (SSD1360) where it re-creates a traditional compass rose. It also shows the difference between tilt compensated output and original bearing. At the bottom of the display roll and pitch angles are displayed.

You can learn about the HMC5883L magnetometer here.

Here you can find out the real way to make a magnetometer produce a bearing that does not change with magnetometer tilt from horizontal. I spent quite a bit of time figuring this out because there is a subtle pitfall, that I'll detail below. You can find all the equations on the web but you might have trouble applying them if you have already built a compass using an HMC5883L (or QST5883L).

## Why do you need an accelerometer?

Depending on where you are on the Earths Surface the magnetic vector changes direction. Specifically it will be horizontal at the equator and 90º down at the North pole and 90º up at the South pole. The diagram below illustrates what is going on: It also varies all over the place which is why the good folks at NCEI have mapped out the Earth's entire magnetic field.

## 3 Axis Magnetometer Tilt Compensation Bearing Error

If there is a significant vertical component the the magnetic vector then tilting the magnetometer will introduce part of that vertical component Zh into the sensors for Xh or Yh. It means that the more you tilt the device the more Xh or Yh you get (or less depending on your location!).

Since Yh and Xh are used to calculate a bearing then you have a bearing error - which is usually significant but that depends on where you are located and how strong the Zh component is at your location.

If you are located at the equator then the magnetic field is horizontal and there is little vertical component in the field. That means the Yh and Zh components are large compared to the Zh component i.e. the Zh component has little effect so tilting has little effect on the bearing.

## Using an accelerometer

An accelerometer measures the Earths gravitational field in 3 degrees of freedom (x, y and z) and the gravity vector always points to the centre of mass of the planet. With these measurements you can calculate pitch and roll angles that determine how far off of the horizontal the x and y planes are located.

You can then use these results to figure out how much Zh component is applied (depending on how much tilt is present) and remove it.

# Coordinate System

The first thing to get right is your coordinate system. In the compass project, that I wrote from scratch, I decided that the "normal" x - y graph was a good idea as it allows you to see mathematical operations in a "normal" way.

The problem with that approach is that you have to fiddle about with the bearing angle since with x to the right and y upwards, an atan function returns zero degrees when the magnetic vector is pointing right. What you really want is zero pointing up the y-axis. In that project transformations were made to make this happen.

The problem with tilt compensation is that the equations that are used work on an entirely different coordinate system - that coordinate system is used by aircraft navigation systems and uses the x coordinate pointing forwards, y pointing right and z pointing down. So if you want the tilt compensation to work you have to use the same coordinate system (either that or re-work out all the rotation matrix maths!).

## Bearing, Roll and Pitch Definitions

The following diagram shows the x, y, z axes, roll and pitch directions: Image source: wikimedia-commons : Update by Jrvz. licensed under CC BY SA 3.0 added x,y,z text

Imagine your self sitting in the cockpit, then:

Yaw: Rotate clockwise about z for positive Yaw; The bearing angle increases.

Roll: The right wing goes down for positive Roll.

Pitch: The nose goes up for positive Pitch.

# Tilt Compensation Algorithm

Once you have sorted out the x, y, and z axes that you want to use then we need to work out the equivalent Yh and Xh - magnetic x and y components. Remember to modify your magnetometer output to match the desired axis directions (as shown above) - this depends on the way you mount your magnetometer.

The best reference I have found for magnetometer tilt compensation algorithm is this one:

This is by Michael J. Caruso Honeywell, SSEC

The document goes into detailed analysis of sources of error and practical ADC resolution that you would find useful in a commercial design.

It does not go into the matrix maths that you need to derive the equations (you can find these in other pdf documents on the web) but if you follow the x, y, z setup then the given equations will just work: Source: http://www.brokking.net/YMFC-32/YMFC-32_document_1.pdf

Where Î¦ is the pitch angle, and Î is the roll angle. Both in radians.

The idea behind the equations is to perform rotations on the magnetic vector to find the magnetic components that are in the horizontal plane.

## 3 Axis Magnetometer Tilt Compensation the Gotcha

As with all gotchas it is a subtle one - also in hindsight all gotchas seem obvious when at the time they are not. When working on the compass code it was only necessary to work out the x and y components of the magnetometer outputs to get a bearing. That means you only needed to work out the hard iron offsets for x and y axes. The following code was used to do that (within the calibrate function):

if ( magnetometerReady() ) getMagnetometer(&x, &y, &z);
if (x>maxx) maxx = x;
if (x<minx) minx = x;
if (y>maxy) maxy = y;
if (y<miny) miny = y;

This code repeated for 10 seconds while you rotate the HMC5883L on a flat surface. Once the max and min are obtained the the calc_offsets function returns the offx and offy values that are subtracted from the magnetometer output to give values centered on (0,0). This works fine and returns a bearing but not a tilt compensated one.

Then the code uses atan2 to get the bearing:

getMagnetometer(&x, &y, &z);

int atan2val = 180/M_PI * atan2((float)(x-offx),(float)(y-offy));
bearing = (-atan2val + 360 ) % 360;

Note: x-offx and y-offy values inside the atan2 function are swapped compared to the tilt compensated (below) code due to the different x-y axis orientation.

The problem when you go from the non-tilt-compensated code to the tilt-compensated code is that the z component is required (see the above equations).

However because I used the old code I did not calculate the z offset. The bearing code only uses the x,y magnetic field output so it seems you don't need to worry too much about the z component - Wrong!

The following code is required to get the offset:

if ( magnetometerReady() ) getMagnetometer(&x, &y, &z);
if (x>maxx) maxx = x;
if (x<minx) minx = x;
if (y>maxy) maxy = y;
if (y<miny) miny = y;
if (z>maxz) maxz = z;
if (z<minz) minz = z;

However the routine is split into two parts - one to get the z values and one to get the x-y values. This is done because when you rotate the device you don't want the x-y values to record the z component of the magnetic vector - you only want to record these values when the device is flat.

You could change this to observe both the accelerometer outputs and the magnetometer outputs to only measure the z value when max or min z is ouput by the accelerometer.

After that the following code returns a tilt compensated bearing:

mx = (float)x;
my = (float)y;
mz = (float)z;

// Compensated
int cx = (int)( mx*cos(p) + my*sin(r)*sin(p) - mz*cos(r)*sin(p) );
int cy = (int)( my*cos(r) + mz*sin(r) );

float atan2val = 180/M_PI * atan2((float)(cy),(float)(cx));

Notice how easy it is to forget the z component because the final calculation uses only cx and cy!

## Issue with 3 Axis Magnetometer Tilt Compensation

The key issue with tilt compensation is:

You have to turn the magnetometer upside down.

You have to do this to get the max and min z field values in order to get the z offset value. Once you do this the tilt compensation formula works fine.

If you think about this for a minute - this is fine for calibrating a phone where it is easy to turn it upside down but it is not even possible to do it for a fixed installation in a vehicle.

It really means that installation in a vehicle relies on the fact that it remains level most of the time - the only calibration needed is to rotate the vehicle in the horizontal plane (around z).

In fact the old gimbal based systems always keep the compass level so are automatically tilt compensated (after settling)!

# Hardware

Use the following components for 3 axis magnetometer tilt compensation:

• Arduino Nano (or Uno),
• SSD1306 OLED display,
• HMC5883L (or QST5883L) breakout board,
• Push to make button (1 off).

# Hardware Connections

Use the following connections for 3 axis magnetometer tilt compensation:

These are the same as used in the 2nd example in this page (the compass code without tilt compensation but with an OLED SSD1306 Display):

Arduino Uno/
Nano pin
Label
Destination
A5
SCL
5883L SCL
A4
SDA
5883L SDA
5V
VCC
5883L VCC
GND
GND
5883L GND
D2 D2 To Button
GND GND To Button
D11
MOSI
OLED_MOSI
D13
CLK
OLED_CLK
D9
DC
OLED_DC
D8
CS
OLED_CS
D10
RST
OLED_RESET
The push button is a normally open one.

# Tilt Compensation Example Code

This 3 axis magnetometer tilt compensation example uses the same hardware as the magnetometer compass project.

The C code below shows you how to do HMC5883L Arduino tilt compensation but uses the alternative chip QST5883L (HMC5883L chip versions are getting hard to  obtain as Honeywell stopped production, but licensed the design to the QST Corporation). Essentially the output from these chips is the same but they use slightly re-arranged internal hardware e.g. different size ADC and registers.

The QST5883L chip is used here as the magnetometer but if you have an HMC5883L chip then you need to use a library (Adafruit HMC5883L Unified), since the HMC5883L is more difficult to use (the library makes it easy). Use the library to retrieve values and supply them from the getMagnetometer() function.

This example rotates the display 180 degrees compared to the original compass code as it makes the x coordinate easier to use as the straight ahead axis as per aircraft bearing, roll and pitch definitions.

The description used in the original compass applies to this code - the only difference being the 3 axis magnetometer tilt compensation function that calculates and returns the compensated bearing:

float getMagnetometerTiltCompensatedBearing(int x, int y, int z)

## Arduino Tilt Compensation Sketch

The following 3 axis magnetometer tilt compensation sketch works on and Arduino Nano or Uno but the algorithm can be applied to any microcotnroller with the relevant chips connected.

``````// QST5883L compass and SSD1306 OLED display
// 3 Axis Magnetometer Tilt compensation using ADXl345
//
// Coords x ahead, y right - follows aircraft convention.
//
// Copyright John Main - Free for non commercial use.
//
#include "Wire.h" ;# For 5883
#include <SPI.h>  ;# For SSD1306 board.
#include <EEPROM.h>
#include "I2Cdev.h"
#include "math.h"

#define OLED_MOSI  11
#define OLED_CLK   13       // LED_BUILTIN
#define OLED_DC     9
#define OLED_CS     8
#define OLED_RESET 10

#define BUTTON_CAL  2
#define BUTTON_TEST 5
#define LED  4 // LED_BUILTIN pin 13 is also SPICLK SCK

#define EEADDR 98 //66 // Start location to write EEPROM data.
#define CALTIME 10000  // In ms.
#define SMOOTH_ACCELL 20

#define SCREEN_HEIGHT 128 // The screen is rotated in sw.
#define SCREEN_WIDTH   64 // This is default height.

static byte stat,ovfl,skipped;
static int minx,maxx,miny,maxy,minz,maxz;
static int offx=0,offy=0,offz=0;

static byte showMT=0;

const int radius = SCREEN_WIDTH / 2;
const int midx   = radius;  // Middle of compass rose.
const int midy   = radius;  // Middle of compass rose.

Adafruit_SSD1306 display(OLED_DC, OLED_RESET, OLED_CS); // hw spi

/////////////////////////////////////////////////////////////
// Write the byte d
Wire.write(d);
Wire.endTransmission();
}

/////////////////////////////////////////////////////////////
void calc_offsets(void)  {
offx = (maxx+minx)/2;   // maxx/2 - minx/2 + 2minx/2
offy = (maxy+miny)/2;
offz = (maxz+minz)/2;
}

/////////////////////////////////////////////////////////////
Wire.beginTransmission(0x0d); // Read from status reg
Wire.write(0x06);
int num = Wire.requestFrom((byte)0x0d, (byte)1);
Wire.endTransmission();

ovfl    = stat & 0x02;
skipped = stat & 0x04;

return (stat && 0x01);  // 0x01 is the DRDY Data Ready flag
}

/////////////////////////////////////////////////////////////
// If data is not ready x,y,z are not changed.
// raw data is aligned to HMC5883 coords
byte getMagnetometerRaw(int16_t *x, int16_t *y, int16_t *z) {

if ( !magnetometerReady() ) return 0;

Wire.beginTransmission(0x0d);
int err = Wire.endTransmission();

if (!err) {
Wire.requestFrom((byte)0x0d, (byte)6); //Blocking?
while(Wire.available()<6); //Wait if above blocking then this not needed.
}
return 1;
}

/////////////////////////////////////////////////////////////
// Orient to board coordinates
void getMagnetometer(int16_t *x, int16_t *y, int16_t *z) {

if ( !getMagnetometerRaw(x, y, z) ) return;

// modify for accel board orientation (board x = up, y to left).
*y = -(*y);
}

/////////////////////////////////////////////////////////////
float getMagnetometerBearing(int16_t x, int16_t y, int16_t z) {

float atan2val = 180/M_PI * atan2((float)y,(float)x); // NEW coords.
int b = (int)(-atan2val + 360 ) % 360;
return b;
}

/////////////////////////////////////////////////////////////
void readMagnetometer(int16_t *x, int16_t *y,int16_t *z) {
getMagnetometer(x, y, z);  // Note: Addresses of pointers passed.
}

///////////////////////////////////////////////////////////////
float getMagnetometerTiltCompensatedBearing(int x, int y, int z) {
float r, p, mx, my, mz;

getRollPitch(&r, &p);

int rdeg = r;
int pdeg = p;
r = (M_PI/180)*r;
p = (M_PI/180)*p;

mx = (float)x;
my = (float)y;
mz = (float)z;

int cx = (int)( mx*cos(p) + my*sin(r)*sin(p) - mz*cos(r)*sin(p) );
int cy = (int)( my*cos(r) + mz*sin(r) );
// Also diffrerent here:
// https://web.archive.org/web/20130624074336/http://www.loveelectronics.co.uk:80/Tutorials/13/tilt-compensated-compass-arduino-tutorial
// Same as 1st!!!

int tx=cx,ty=cy,tz=z; // Done offx,y above
float atan2val = 180/M_PI * atan2((float)(ty),(float)(tx));

int tb = (int)(-atan2val + 360 ) % 360;

return tb;
}

///////////////////////////////////////////////////////////////
// Returns angle calculated from z, y, z accelerometers.
//
void getRollPitchRawFloat(float *roll, float *pitch) {
float r,x,y,z;
int16_t ax, ay, az;

// Datasheet: OPERATION AT VOLTAGES OTHER THAN 2.5 V
// 3v3 X,Y 25mg too high, z 20mg too low
// 3V3 lsb value 265/g c  (g/265)=0.03698
// 2V5 lsb value 256/g   (g/256)=0.03828 z axis unaffected by voltage supply.

accel.getAcceleration(&ax, &ay, &az);

r = sqrt(x*x+y*y+z*z);

// modify for accel board orientation (board x = up, y to left).
y = -y;
*pitch  = 180/M_PI * asin(x/r);
*roll   = 180/M_PI * -asin(y/r);
}

///////////////////////////////////////////////////////////////
void getRollPitch(float *roll, float *pitch) {
static float avg_r[SMOOTH_ACCELL], avg_p[SMOOTH_ACCELL];
static byte idx=0;
float r,p;

getRollPitchRawFloat(roll, pitch);

avg_r[idx] = *roll;                 // Smooth.
avg_p[idx] = *pitch;
idx++;
if (idx>=SMOOTH_ACCELL) idx = 0;

r = p = 0;
for (int i=0; i<SMOOTH_ACCELL; i++) {
r += avg_r[i];
p += avg_p[i];
}
r /= SMOOTH_ACCELL;
p /= SMOOTH_ACCELL;

*roll  = r;
*pitch = p;
}

/////////////////////////////////////////////////////////////
void setup() {

pinMode(LED,OUTPUT);

pinMode(BUTTON_CAL,INPUT_PULLUP);
pinMode(BUTTON_TEST,INPUT_PULLUP);

Wire.begin(); // Start I2C
Wire.setClock(100000); // Test at high speed

display.begin(SSD1306_SWITCHCAPVCC);
display.clearDisplay();
display.setTextSize(1);
//   display.setTextColor(WHITE);       // Does NOT erase background.
display.setTextColor(WHITE, BLACK);  // Does erase background.

// Datasheet suggests this for chip startup HMC5883L.
// Set reset register.Datasheet suggests this as 0x01.
// Control reg : Mode:continuous, ODR:10Hz, RNG:2G, OSR:512 (over sample)
// In lab gauss > 2G so need higher range.

calc_offsets();

display.setRotation(3);

Serial.begin(115200);
Serial.println("Compass hmc5883L");

accel.initialize();
Serial.println(F("Testing device connections..."));
Serial.println(accel.testConnection() ? F("successful") : F("failed") );

accel.setRate(ADXL345_RATE_100);  // This is default but shows the value.

accel.setFullResolution(1); // 0 => 10 bit mode.
accel.setLowPowerEnabled(0);
accel.setRange(0); // 0 => 2g, 3 => 16g
}

/////////////////////////////////////////////////////////////
// Normalized line drawing (0,0) = bot left
void drawLine (int x1, int y1, int x2, int y2) {
display.drawLine(x1, SCREEN_HEIGHT-y1, x2, SCREEN_HEIGHT-y2, 1);
}

/////////////////////////////////////////////////////////////
// Normalized line drawing (0,0) = bot left
void drawCircle (int x, int y, int radius) {
}

/////////////////////////////////////////////////////////////
void drawBearing(float bearing, int midx, int midy, int radius) {
bearing += 90; // Rotate arctan2 to vertical
// Screen +y is down but drawLine adjusts it up.
}

/////////////////////////////////////////////////////////////
// Can choose whether to write to EEPROM for testing.
void calibrate(boolean eeprom_write, byte xy_or_z) {
unsigned long calTimeWas = millis();

int x,y,z;
float deg=0,deg2=0;

maxx = minx = x; // Set initial values to current magnetometer readings.
maxy = miny = y;
maxz = minz = z;

delay(300); // Allow button release.

while(1) {  // Calibration loop.

delay(300); // Allow button release.
return; // Abort
}

if ( magnetometerReady() ) getMagnetometer(&x, &y, &z);
if (x>maxx) maxx = x;
if (x<minx) minx = x;
if (y>maxy) maxy = y;
if (y<miny) miny = y;
if (z>maxz) maxz = z;
if (z<minz) minz = z;

display.clearDisplay();

display.setTextSize(2);
display.setCursor(0,0);

if(eeprom_write) display.print("CALIB");
else             display.print("TEST");

display.setCursor(0,16);
if (!xy_or_z) display.print("XY"); else
display.print("Z");

int secmillis  = millis()-calTimeWas;
if (secmillis>CALTIME) break;                    // Exit after time up.

int secs = (int)((CALTIME-secmillis+1000)/1000);
display.setCursor(0,32);  display.print("--> "); display.print((int)((CALTIME-secmillis)/1000));

deg = (360.0/CALTIME)*secmillis; // Rotate a line for countdown duration.

deg2 += deg; // Rotate a line for countdown duration. Fun.
deg = fmod(deg,360);

for(int i=0;i<360;i+=45)   // 45 Degree spokes (rotating)
drawBearing(i + (45/secs)*10, midx, midy, radius-7);

display.display();  // Update display.
delay(10);
} // while cal

if (xy_or_z==0) {
offx = ((maxx-minx)/2)+minx;
offy = ((maxy-miny)/2)+miny;
} else
offz = ((maxz-minz)/2)+minz;

if(eeprom_write) {

if (xy_or_z==0) {
} else {
}
}

unsigned long dispExitTimeWas = millis();

while(1) {

display.clearDisplay();

display.setTextSize(2);
display.setCursor(0,0);

// Make sure this does not repeat endlessly!
if(eeprom_write) display.print("EEPROM Written");
else             display.print("TEST DMY Write");
if (millis()- dispExitTimeWas>2000) break;

display.display();  // Update display.
delay(10);
}
calc_offsets();
}

/////////////////////////////////////////////////////////////
void draw(int Mx, int My, int bearing, int tbearing) {
int lineH, lineNum;

// SSD1306 init.
display.clearDisplay();

// display fancy stuff
drawCircle(midx, midy, radius-1);    // -1 as circle is 1 bigger

for(int i=0;i<360;i+=45)   // 45 Degree spokes

drawBearing(0, midx, midy, radius);       // North

display.setTextSize(2);
lineH = 16;

lineNum=0;
// Bearing
display.setCursor(15,lineH*lineNum);  display.print(bearing);
display.print((char)247);
lineNum++;  // Next line

// Tilt Bearing
display.setCursor(15,lineH*lineNum);  display.print(tbearing);
display.print((char)247);
lineNum++;  // Next line

if(Mx>maxy || Mx<minx || My>maxy || My<miny) {
display.setCursor(0,lineH*lineNum++);  display.print("*CAL*");
}

display.setTextSize(1);
lineH = 8;
lineNum=0;

// Roll Pitch
float r,p;
getRollPitch(&r,&p);

display.setCursor(0,120);  display.print(r,1);
display.print((char)247);

display.setCursor(40,120);  display.print(p,1);
display.print((char)247);

display.display();  // Update display.
}

/////////////////////////////////////////////////////////////
void loop(void) {
static unsigned long BLTimeWas=millis();
int x,y,z; // Raw compass output values.
int bearing,i;

if (digitalRead(BUTTON_CAL)==0)  { calibrate(1,0); calibrate(1,1); }
if (digitalRead(BUTTON_TEST)==0) { calibrate(0,0); calibrate(0,1); }

getMagnetometer(&x,&y,&z);

bearing = (int)getMagnetometerBearing(x-offx, y-offy, z-offz);

int tbearing = getMagnetometerTiltCompensatedBearing(x-offx, y-offy, z-offz);

draw(x,y,bearing,tbearing);

if (millis()-BLTimeWas>400) { // LED toggle
BLTimeWas = millis();
static byte togLED=0;
togLED = !togLED;
if(togLED) digitalWrite(LED,HIGH); else digitalWrite(LED,LOW);
}
}

// End of QST5883L OLED compass - // 3 axis magnetometer tilt compensation example.

``````

[ File: qmc5883l-tilt-comp.ino ]

## 3 Axis Magnetometer Tilt Compensation Code Operation

On the screen both bearings are displayed as lines pointing North. The longer line is the original bearing while the shorter one is the compensated one.

At the top of the display the original bearing is shown while below that the compensated bearing is displayed (in digits).

At the bottom of the display on the left is the roll angle while on the right is the pitch angle.

The image below shows the compass tilted with a roll at an angle of 17.5° while the pitch is 0.2°. The resulting bearing is 320° with a tilt corrected bearing of 347°. You can see the original line bearing drawn longer while the short line is the tilt compensated one. These lines point to magnetic North.

Top: Non compensated 3 axis bearing.
Next: 3 axis magnetometer tilt compensation bearing. Left: roll in degrees, Right: Pitch in degrees.

End of 3 axis magnetometer tilt compensation example.