LSM303 Compass – Tilt Compensation

The LSM303 compass design makes use of our BM004 module and an Arduino Uno.  The code that converts the magnetic field reading from the IC to a heading is very simple.  However, adding tilt compensation is a little more difficult.

First, for a more detailed discussion of using the LSM303DLHC as a compass see ST Micro’s AN3192. A copy is maintained at the BM004 application note page at www.solutions-cubed.com.

We’ve also provided example code for the Arduino in C.  Using that code as a reference is the most efficient way to see how to implement tilt compensated heading calculations.   It should be transferable to most other C based controllers as long as you have access to trigonometry libraries.

Heading = 180°*atan(Myh/Mxh)/PI where Myh = My and Mxh = Mx => the magnetometer register values.

However this equation is only valid with the tilt of the IC or module is equal to 0°. For tilt compensation you need the pitch and roll and other more detailed calculations.

Pitch and Roll:

You need the normalized acceleration values for the x and y axes, Ax and Ay, to calculate pitch and roll.

Axn = Ax/sqrt(Ax2 + Ay2 + Az2)

Ayn = Ay/sqrt(Ax2 + Ay2 + Az2)

Pitch = arcsin(-Axn)

Roll = arcsin(Ayn/cos(Pitch))

To provide accurate heading information you’ll need to calibrate the magnetometer values for your specific sensor. One simple method to do this is to capture the min and max values for each axes Mx, My, and Mz. Rotate the sensor around all axes and write down the max and min values for each axis. The output of our Arduno code that tracks calibration values is shown below.

To offset and scale your magnetic field values use the equations below.

Mxc = (Mx-Mminx) / (Mmaxx-Mminx) *2 -1;

Myc = (My-Mminy) / (Mmaxy-Mminy) *2- 1;

Mzc = (Mz-Mminz) / (Mmaxz-Mminz) * 2-1;

Then calculate Mxh and Myh with these values.

Mxh = Mxc*cos(Pitch)+Mzc*sin(Pitch);

Myh = Mxc *sin(Roll)*sin(Pitch)+Myc*cos(Roll)-Mzc*sin(Roll)*cos(Pitch);

And finally, you can calculate your heading using the new values for Myh and Mxh.

Here is the code I used in an Arduino.

```/*
BM004_Arduino_compass_tilt_compensated:  This program reads the magnetometer and
accelermoter registers from ST Micro's LSM303DLHC.  The register values are used to generate
a tilt compensated heading value.  NOTE:  placing the compass near metallic objects can impact

Schematics associated with the BM004 moudle may be used for hardware wiring information.

*/

#include <Wire.h>
#include <Math.h>

float Pitch;
float Roll;

float Accx;
float Accy;
float Accz;

float Magx;
float Magy;
float Magz;
float Mag_minx;
float Mag_miny;
float Mag_minz;
float Mag_maxx;
float Mag_maxy;
float Mag_maxz;

/*
Send register address and the byte value you want to write the accelerometer and
loads the destination register with the value you send
*/
{
Wire.beginTransmission(0x19);   // Use accelerometer address for regs >=0x20
Wire.write(data);
Wire.endTransmission();
}

/*
Send register address to this function and it returns byte value
for the accelerometer register's contents
*/
{
byte data;
Wire.beginTransmission(0x19);   // Use accelerometer address for regs >=0x20
Wire.endTransmission();

delayMicroseconds(100);

Wire.requestFrom(0x19,1);   // Use accelerometer address for regs >=0x20
Wire.endTransmission();

delayMicroseconds(100);

return data;
}

/*
Send register address and the byte value you want to write the magnetometer and
loads the destination register with the value you send
*/
{
Wire.beginTransmission(0x1E);   // Else use magnetometer address
Wire.write(data);
Wire.endTransmission();

delayMicroseconds(100);
}

/*
Send register address to this function and it returns byte value
for the magnetometer register's contents
*/
{
byte data;
Wire.beginTransmission(0x1E);   // Else use magnetometer address
Wire.endTransmission();

delayMicroseconds(100);

Wire.requestFrom(0x1E,1);   // Else use magnetometer address
Wire.endTransmission();

delayMicroseconds(100);

return data;
}

void init_Compass(void)
{
WriteAccRegister(0x67,0x20);  // Enable accelerometer, 200Hz data output

WriteMagRegister(0x9c,0x00);  // Enable temperature sensor, 220Hz data output
WriteMagRegister(0x20,0x01);  // set gain to +/-1.3Gauss
WriteMagRegister(0x00,0x02);  // Enable magnetometer constant conversions
}

/*
Readsthe X,Y,Z axis values from the accelerometer and sends the values to the
serial monitor.
*/
void get_Accelerometer(void)
{

// accelerometer values

// need to convert the register contents into a righ-justified 16 bit value
Accx = (xh<<8|xl);
Accy = (yh<<8|yl);
Accz = (zh<<8|zl);

}

/*
Reads the X,Y,Z axis values from the magnetometer sends the values to the
serial monitor.
*/
void get_Magnetometer(void)
{
// magnetometer values

// convert registers to ints
Magx = (xh<<8|xl);
Magy = (yh<<8|yl);
Magz = (zh<<8|zl);
}

/*
Converts values to a tilt compensated heading in degrees (0 to 360)
*/
{
// You can use BM004_Arduino_calibrate to measure max/min magnetometer values and plug them in here.  The values
// below are for a specific sensor and will not match yours
Mag_minx = -572;
Mag_miny = -656;
Mag_minz = -486;
Mag_maxx = 429;
Mag_maxy = 395;
Mag_maxz = 535;

// use calibration values to shift and scale magnetometer measurements
Magx = (Magx-Mag_minx)/(Mag_maxx-Mag_minx)*2-1;
Magy = (Magy-Mag_miny)/(Mag_maxy-Mag_miny)*2-1;
Magz = (Magz-Mag_minz)/(Mag_maxz-Mag_minz)*2-1;

// Normalize acceleration measurements so they range from 0 to 1
float accxnorm = Accx/sqrt(Accx*Accx+Accy*Accy+Accz*Accz);
float accynorm = Accy/sqrt(Accx*Accx+Accy*Accy+Accz*Accz);

// calculate pitch and roll
Pitch = asin(-accxnorm);
Roll = asin(accynorm/cos(Pitch));

// tilt compensated magnetic sensor measurements
float magxcomp = Magx*cos(Pitch)+Magz*sin(Pitch);
float magycomp = Magx*sin(Roll)*sin(Pitch)+Magy*cos(Roll)-Magz*sin(Roll)*cos(Pitch);

// arctangent of y/x converted to degrees

}

void setup() {
Wire.begin();
Serial.begin(9600);  // start serial for output
init_Compass();
}

void loop() {
get_Accelerometer();
get_Magnetometer();
delay(100);
}```

1. Steve says:

If i have a dual Axis device output in acceleration “g”, SIn of angle being equivalent to 0 to 1g.
can i use the following equations?

Roll Output = ARCSIN ( SIN(ROLL) x COS(PITCH) )
Pitch Output = ARCSIN ( SIN(PITCH) x COS(ROLL) )

• Hey Steve,

Check the ST Micro application note. Page 21 shows the derivation of the pitch and roll values. They indicate that the Z axis is not needed, but you should compare their equations to yours to verify.

ST Micro AN3192

Lon

2. Dan says:

For the pitch and roll calculations where the acceleration values are used, if the compass is in motion, then are these equations invalid because of the extra accelerations? I’m assuming that the motion is not smooth but is a slow moving vehicle, less than 1 mph across a bumpy ground. I’m also wondering about compensating for operating motors. As I mentioned, my idea was to use this compass on a slow moving vehicle. Is the interference from motors linear, or at least predictable?

• Hi Dan,

I wouldn’t say that the calculations are invalid with acceleration changes. However, in use you might want to average, or smooth, the output to remove spurious results. Of greater impact would be changes in the magnetometer min and max values as the device moves. I’ve seen examples where the magnetometer readings are calibrated “on-the-fly”. I think that is a better method than calculating the magnetometer min/max values and then using them as fixed calibration settings.

If you take a look at a more recent blog post related to hard iron calibration there are some Freescale application notes referenced. I found those to be very helpful, as they lay out both the math and the process for magnetometer calibration. These are part of a series of application notes related to a compass design for a smart phone.

Interference from electronic components will most likely impact the magnetometer readings. These are typically broken down into hard and soft iron interference. My understanding is that hard iron effects, caused by things like permanent magnets(motors), are somewhat fixed and associated with the sensor’s placement and surrounding components. A magnetometer rotated in three dimensions will trace out a sphere. Hard iron interference causes the sphere to be offset from the origin of an X,Y,Z coordinate system. The code I used for this Arduino basically attempts to relocate the X and Y coordinates back to the origin, since a compass lays flat on a plane.

Soft iron interference is much more difficult to compensate for, as it stretches the sphere into an ellipsoid. The ellipsoid then has to be re-spherized (is that a word?).

Some of the newer sensors come with hard iron calibration values programmed into them. However, these factory calibration settings can only compensate for errors associated with the sensor, and not surrounding permanent magnets like motors and speakers.

Hope this sheds some light on the topic.

Lon

3. nick says:

Your heading ,that is based on your screenshot example is approx 63,5. How can i use it to find the yaw angle in combination with the yaw from the gyro? How can i fuse them? I want the yaw angle for my copter and also i want when the copter starts the yaw to be equal to 0, not 63. Can you suggest me something? I hope to understand my question because i am a noobie at these things:P .

• Hi Nick,

I think the yaw angle is relative to magnetic north. If you’re just looking for an offset then determine the yaw angle at start use that value to correct the yaw angle. For example, if the yaw at start is 63, then your yaw calculations should be yaw = yaw_raw – 63.

If you need greater understanding, Google Freescale (or NXP) AN4248 “Implementing a Tilt-Compensated eCompass using Accelerometer and Magnetometer Sensors”. Page 8, equation 22, is a derived equation for determining yaw angle. The application note refers to a phone, but could just as easily be something else.

They have an additional application note associated with errors caused by things like hard and soft magnetic interference and sensor noise (AN4249).

This level of design is not for the faint of heart. Good luck.

Lon

• nick says:

Ok. Thank you for the answer 🙂

4. Jack says:

Hi. How can i use the heading in your code to find the final yaw angle? Will it
consist of magnetometer heading and gyro yaw ; and if yes how?