LSM303 Compass – Tilt Compensation

image

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.

Calculating Heading, Pitch, and Roll:
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.

Basic Heading:

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))

Tilt Compensated Heading:

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.

image

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.

Heading = 180°*atan(Myh/Mxh)/PI

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
readings.

Schematics associated with the BM004 moudle may be used for hardware wiring information.
see www.solutions-cubed.com for additional information.

*/

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

    float Heading;
    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
*/
void WriteAccRegister(byte data, byte regaddress)
{
    Wire.beginTransmission(0x19);   // Use accelerometer address for regs >=0x20
    Wire.write(regaddress);
    Wire.write(data);  
    Wire.endTransmission();     
}

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

    delayMicroseconds(100);

    Wire.requestFrom(0x19,1);   // Use accelerometer address for regs >=0x20
    data = Wire.read();
    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
*/
void WriteMagRegister(byte data, byte regaddress)
{
    Wire.beginTransmission(0x1E);   // Else use magnetometer address
    Wire.write(regaddress);
    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 ReadMagRegister(byte regaddress)
{
    byte data;
    Wire.beginTransmission(0x1E);   // Else use magnetometer address  
    Wire.write(regaddress);
    Wire.endTransmission();

    delayMicroseconds(100);

    Wire.requestFrom(0x1E,1);   // Else use magnetometer address
    data = Wire.read();
    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
  byte xh = ReadAccRegister(0x29);
  byte xl = ReadAccRegister(0x28);
  byte yh = ReadAccRegister(0x2B);
  byte yl = ReadAccRegister(0x2A);
  byte zh = ReadAccRegister(0x2D);
  byte zl = ReadAccRegister(0x2C);

  // 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
  byte xh = ReadMagRegister(0x03);
  byte xl = ReadMagRegister(0x04);
  byte yh = ReadMagRegister(0x07);
  byte yl = ReadMagRegister(0x08);
  byte zh = ReadMagRegister(0x05);
  byte zl = ReadMagRegister(0x06);

  // 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)
*/
void get_TiltHeading(void)
{
  // 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
  Heading = 180*atan2(magycomp,magxcomp)/PI;

  if (Heading < 0)
      Heading +=360;

    Serial.print("Heading=");
    Serial.println(Heading);    
  }  

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

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

Comments

  1. 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. 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. 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

  4. 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?
    Thnaks in advance.

    • Hi Jack,

      Please see the other comments and the links I’ve provided related to the NXP application notes. They describe in detail one method for determining yaw.

      Lon

Trackbacks

  1. […] to calibrate my sensors, I can move on to calculating the vectors for my flow meters. Deriving a yaw angle from the magnetometer means that I no longer need to worry about physical orientation of the […]

Speak Your Mind

*