This part 2 of a multi part series on how to use a digital compass(magnetometer) with your Raspberry Pi.

Part 1 can be found here and is a prerequisite to part 2.

Code for this guide

Git repository here
The code can be pulled down to your Raspberry Pi with;

The code for this guide can be found under the compass_tutorial02_tilt_compensation directory.

Calculating Compass Heading

Part 1 covered how to get the heading from the magnetometer, however this is only reliable when the magnetometer is on a flat surface.  If the magnetometer is tilted, the heading will skew and not be correct.

The chart below shows a compass being held at 200 degrees and being tilted in various directions. The blue line is the raw heading, the orange line is the heading after applying tilt compensation. As you can see,  without tilt compensation the heading will change if the compass is tilted.

The below image is from a page from application note  AN3192 from ST, the manufacture of the LSM9DS0 which is the magnetometer and accelerometer we are using in this guide. This application note goes into detail on how to calculate a tilt compensated heading.

In summary, it states that if the magnetometer is at a level position, then the only calculation that is needed to be performed is;

heading = 180*atan2(magRawY,magRawX)/M_PI;

If the magnetometer is tilted, we need to factor in the tilt values to get an accurate heading. This is what the application note says about this;

In summary; We will use an accelerometer to calculate pitch & roll which will then be included in our formula to calculate the heading with tilt compensation. Here is the formula;

float magXcomp = mag_raw[0]*cos(asin(accXnorm))+mag_raw[2]*sin(pitch);
float magYcomp = mag_raw[o]*sin(asin(accYnorm/cos(pitch)))*sin(asin(accXnorm))+mag_raw[1]*cos(asin(accYnorm/cos(pitch)))-mag_raw[2]*sin(asin(accYnorm/cos(pitch)))*cos(asin(accXnorm));
heading = 180*atan2(magYcomp,magXcomp)/M_PI;

The above formula looks very complex, you don't really need to understand it and we do simplify it further on in this post.
One point to note ;The arcsin function has good linearity between about -45º to +45º, so the accuracy of the
pitch and roll calculation degrades when tilt angles exceed this range

Reading Values from the Accelerometer

Most accelerometers are on the same chip as a magnetometer, which is in this instance a LSM9DS0 on a BerryIMU.  In some instances the i2c address of the accelerometer will be different than the magnetometer (E.g on the LSM303DLHC).  On the LSM9DS0 , the i2c address is the same for both accelerometer and magnetometer.

For the BerryIMU, the default address is 0x1E.

We will create a new function to write to the accelerometer;

void writeMagReg(uint8_t reg, uint8_t value)
{
  selectDevice(file,MAG_ADDRESS);
  int result = i2c_smbus_write_byte_data(file, reg, value);
    if (result == -1)
    {
        printf ("Failed to write byte to I2C Mag.");
        exit(1);
    }
}

And we will also create a new function to read from the accelerometer. Reading values from the accelerator is similar to reading values from the magnetometer, we will read a block of 6 bytes.

void readACC(int  *a)
{
        uint8_t block[6];
        selectDevice(file,ACC_ADDRESS);
        readBlock(0x80 | OUT_X_L_A, sizeof(block), block);
      *a = (int16_t)(block[0] | block[1] << 8);
      *(a+1) = (int16_t)(block[2] | block[3] << 8);
      *(a+2) = (int16_t)(block[4] | block[5] << 8);
}

Here is what is happening in the readACC() function;
1) An array of 6 bytes is first created to store the values.
2) Using the readBlock() function (created in part 1), we read 6 bytes starting at OUT_X_L_A (0x28). This is shown on page 61 of the datasheet.
3) The values are expressed in 2’s complement (MSB for the sign and then 15 bits for the value) so we need to combine;
block[0] & block[1] for X axis
block[2] & block[3] for Y axis
block[4] & block[5] for Z axis

Working with the code we created in part 1, we now need to add code to enable the accelerometer;

 // Enable accelerometer.
   writeAccReg(CTRL_REG1_XM, 0b01100111);
   writeAccReg(CTRL_REG2_XM, 0b00100000);

CTRL_REG1_XM tells the accelerometer to enable all axis, set it to continuous update mode and a data rate of 100Hz - Page 55 of the Datasheet.
CTRL_REG2_XM,  will set the accelerometer to +/-16 gauss full scale.

The Math

First, we need to normalize the raw accelerometer data;

accXnorm = accRaw[0]/sqrt(accRaw[0]* accRaw[0]+ accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);
accYnorm =accRaw[1]/sqrt(accRaw[0] *accRaw[0] + accRaw[1] * accRaw[1] + accRaw[2] * accRaw[2]);

As the pitch and roll is calculated a number of times in the tilt compensation formula, we will split these off and perform both calculations once beforehand;

	pitch = asin(accXnorm);
	roll = -asin(accYnorm/cos(pitch));

We now calculate the new tilt compensated magnetometer readings;

magXcomp = *mag_raw*cos(pitch)+*(mag_raw+2)*sin(pitch);
magYcomp = *mag_raw*sin(roll)*sin(pitch)+*(mag_raw+1)*cos(roll)-*(mag_raw+2)*sin(roll)*cos(pitch);

We then use these new compensated values in our heading formula;

	heading = 180*atan2(magYcomp,magXcomp)/M_PI;

We will also convert the values so that they are in the range of 0 to 360;

if(heading < 0)
    heading += 360;

Display the New Heading

After the last calculation, we can add another print statement to print out the new compensated heading.

                printf("Compensated  Heading %7.3f  \n", heading);

When you now run the program, you will notice that the heading stays the same value if the magnetometer is tilted to within 40 degrees.

RAW heading in Degrees 172.000 ##### Tilt Comp heading 170.043
RAW heading in Degrees 170.044 ##### Tilt Comp heading 168.015
RAW heading in Degrees 167.341 ##### Tilt Comp heading 164.817
RAW heading in Degrees 163.133 ##### Tilt Comp heading 172.429
RAW heading in Degrees 155.924 ##### Tilt Comp heading 162.065
RAW heading in Degrees 149.081 ##### Tilt Comp heading 153.045
RAW heading in Degrees 144.548 ##### Tilt Comp heading 152.323
RAW heading in Degrees 139.844 ##### Tilt Comp heading 157.862
RAW heading in Degrees 135.603 ##### Tilt Comp heading 171.924
RAW heading in Degrees 130.305 ##### Tilt Comp heading 172.325
RAW heading in Degrees 122.636 ##### Tilt Comp heading 146.452
RAW heading in Degrees 118.951 ##### Tilt Comp heading 143.692

Compiling

Upside down IMU

If your IMU is upside down, you will need to invert X & Y accelerator values for the tilt compensation to work.
Just add the two lines below right after the raw values have been read from the accelerometer.

              accRaw[0] = -accRaw[0];
              accRaw[1] = -accRaw[1];

Guides and Tutorials

• Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi
• Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi - Kalman Filter
• Create a Digital Compass with the Raspberry Pi – Part 1 – “The Basics”
• Create a Digital Compass with the Raspberry Pi – Part 3 – “Calibration”
• Create a Digital Compass with the Raspberry Pi – Part 4- “Smartphone Replica”
• Converting values from an Accelerometer to Gs – “ Proper Acceleration”
• Using the BerryIMUv3 on a Raspberry Pi Pico
• Double tap detection with BerryIMUv3
• How to Create an Inclinometer using a Raspberry Pi and an IMU
• Raspberry Pi Digital Spirit Level