Tilt Compensated Compass

Create a Digital Compass with the Raspberry Pi – Part 2 – “Tilt Compensation”

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;

pi@raspberrypi ~ $ git clone https://github.com/mwilliams03/BerryIMU.git

Calculating Compass Heading

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

The below image is from a page from application note  AN3192 from ST, the manufacture of the LSM303D 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.

Compass 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;

Tilt Compensated compass

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

BerryIMU Raspberry Pi Gyroscope Accelerometer

Reading Values from the Accelerometer

Most accelerometers are on the same chip as a magnetometer, which is in this instance a LSM303D 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 LSM303D, the i2c address is the same for both accelerometer and magnetometer.

For the BerryIMU, the default address is 0x1D.

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 | LSM303D_OUT_X_L_A, sizeof(block), block);

        *a = (int16_t)(block[0] | block[1] << 8) >> 4;
        *(a+1) = (int16_t)(block[2] | block[3] << 8) >> 4;
        *(a+2) = (int16_t)(block[4] | block[5] << 8) >> 4;
}

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 LSM303D_OUT_X_L_A (0x08). This is shown on page 40 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( LSM303D_CTRL1, 0x5F);   // Acc data rate = 50Hz, BDU enable
    writeAccReg( LSM303D_CTRL2, 0x18);   // Acc full-scale = 8g

 

LSM303D_CTRL1, a value of 0x5F, which is 1011111 in binary.  This tells the accelerometer to enable all axis, set it to continuous update mode and a data rate of 50Hz – Page 34 & 35 of the Datasheet. LSM303D_CTRL2, a value of 0x18,  will set the accelerometer to +/-8gauss 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
RAW heading in Degrees 115.494 ##### Tilt Comp heading 149.246
RAW heading in Degrees 113.482 ##### Tilt Comp heading 149.254
RAW heading in Degrees 112.162 ##### Tilt Comp heading 152.356
RAW heading in Degrees 112.401 ##### Tilt Comp heading 159.583
RAW heading in Degrees 111.491 ##### Tilt Comp heading 155.175
RAW heading in Degrees 112.166 ##### Tilt Comp heading 148.276
RAW heading in Degrees 113.433 ##### Tilt Comp heading 163.542
RAW heading in Degrees 115.034 ##### Tilt Comp heading 176.560
RAW heading in Degrees 116.565 ##### Tilt Comp heading 187.912
RAW heading in Degrees 118.491 ##### Tilt Comp heading 177.744
RAW heading in Degrees 121.752 ##### Tilt Comp heading 127.015
RAW heading in Degrees 127.208 ##### Tilt Comp heading 144.155
RAW heading in Degrees 135.176 ##### Tilt Comp heading 166.488
RAW heading in Degrees 146.964 ##### Tilt Comp heading 201.184
RAW heading in Degrees 162.320 ##### Tilt Comp heading 158.127
RAW heading in Degrees 170.923 ##### Tilt Comp heading 146.053
RAW heading in Degrees 179.056 ##### Tilt Comp heading 157.336
RAW heading in Degrees 181.557 ##### Tilt Comp heading 162.723

How to Create an Inclinometer using a Raspberry Pi and an IMU

This guide covers how to use an Inertial Measurement Unit (IMU) with a Raspberry Pi to create an inclinometer, just like the type you will find in a 4WD.

A prerequisite for this guide is to have a gyro and accelerometer from an IMU already up and running on your Raspberry Pi. A guide to interfacing an IMU with a Raspberry Pi can be found here.

We will be covering some basic SDL which will be used to produce our graphics.

 

The IMU used in this guide is the BerryIMU.  However, other IMUs or accelerometers and gyroscopes can be used.. Eg  Pololu MinIMU, Adafruit IMU and Sparkfun IMUs

Continue reading

Accelerometer Gyroscope angle Raspberry

Guide to interfacing a Gyro and Accelerometer with a Raspberry Pi

This guide covers how to use an Inertial Measurement Unit (IMU) with a Raspberry Pi . This is an updated guide and improves on the old one found here.

In this guide I will explain how to get readings from the IMU and convert these raw readings into usable angles. I will also show how to read some of the information in the datasheets for these devices.

This guide focuses on the BerryIMU. However, the theory and principals below can be applied to any digital IMU, just some minor modifications need to be made. Eg  Pololu MinIMU, Adafruit IMU and Sparkfun IMUs

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

pi@raspberrypi ~ $ git clone https://github.com/mwilliams03/BerryIMU.git

 
BerryIMU Raspberry Pi Gyroscope Accelerometer

A note about Gyros and Accelerometers

When using the IMU to calculate angles, readings from both the gyro and accelerometer are needed which are then combined. This is because using either on their own will result in inaccurate readings. And a special note about yaw.

Here is why;
Gyros - A gyro measures the rate of rotation, which has to be tracked over time to calculate the current angle. This tracking causes the gyro to drift. However, gyros are good at measuring quick sharp movements.

Accelerometers - Accelerometers are used to sense both static (e.g. gravity) and dynamic (e.g. sudden starts/stops) acceleration. They don’t need to be tracked like a gyro and can measure the current angle at any given time. Accelerometers however are very noisy and are only useful for tracking angles over a long period of time.

Accelerometers cannot measure yaw.   To explain it simply, yaw is when the accelerometer is on a flat level surface and it is rotated clockwise or anticlockwise.  As the Z-Axis readings will not change, we cannot measure yaw.   A gyro and a magnetometer can help you measure yaw. This will be covered in a future guide.

Here is an excellent tutorial about accelerometers and gyros.

Setting up the IMU and I2C

The IMU used for this guid  a BerryIMU which uses a L3GD20H 3-axis gyroscope and a LSM303DLHC 3-axis accelerometer and 3-axis magnetometer.
The datasheets are needed if you want to use these devices;
L3GD20H Datasheet
LSM303DLHC Datasheet

This IMU communicates via the I2C interface.

The image below shows how to connect the BerryIMU to a Raspberry Pi

BerryIMU Raspberry Pi
Continue reading

Raspberry Pi Digital compass

Create a Digital Compass with the Raspberry Pi – Part 1 – “The Basics”

This will be a multipart series on how to use a digital compass(magnetometer) with your Raspberry Pi.

The magnetometer used in these tutorials is a LSM303D which is on a BerryIMU. We will also point out where some of the information can be found in the Datasheet for the LSM303D. This will help you understand how the LSM303D works.

The math and logic in this series can also be used with other magnetometers or IMUs.

We will also go over how to do some basic communication on the i2c bus. As well as using SDL to display the compass heading as traditional compass as shown in the video above.

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

pi@raspberrypi ~ $ git clone https://github.com/mwilliams03/BerryIMU.git

Overview of a Compass

Raspberry Pi Compass
A traditional Magnetic compass (as opposed to a gyroscopic compass) consists of a small, lightweight magnet balanced on a nearly frictionless pivot point. The magnet is generally called a needle. The Earth’s Magnetic field will cause the needle to point to the North Pole.

To be more accurate, the needle points to the Magnetic North. The angle difference between true North and the Magnetic North is called declination. Declination is different in different locations. This angle varies depending on position on the Earth’s surface, and changes over time.

The strength of the earth’s magnetic field is about 0.5 to 0.6 gauss .

Continue reading

matchbox-desktop raspberry pi

Matchbox-Desktop on the Raspberry Pi

Matchbox-desktop is a lightweight windows manager for embedded systems. It works great on a Raspberry Pi with one of the smaller TFTs attached. E.g. PiScreen, PiTFT , etc…

Matchbox-desktop is “finger friendly” and makes it easy to navigate while just using your fingers on the touchscreen.

Mathcbox-Desktop needs to be installed from source and via apt-get to get it going correctly, in this order;

pi@raspberrypi ~ $ sudo apt-get update
pi@raspberrypi ~ $ sudo apt-get install autoconf autogen intltool libtool libx11-dev libxext-dev libxft-dev libpng-dev libgconf2-dev libgtk2.0-dev libstartup-notification0-dev libdbus-glib-1-dev -y
pi@raspberrypi ~ $ git clone http://git.yoctoproject.org/cgit/cgit.cgi/libmatchbox/
pi@raspberrypi ~ $ git clone http://git.yoctoproject.org/cgit/cgit.cgi/matchbox-common/
pi@raspberrypi ~ $ git clone git://git.yoctoproject.org/matchbox-desktop
pi@raspberrypi ~ $ cd libmatchbox
pi@raspberrypi ~ $ ./autogen.sh
pi@raspberrypi ~ $ make
pi@raspberrypi ~ $ sudo make install
pi@raspberrypi ~ $ cd ../matchbox-common
pi@raspberrypi ~ $ ./autogen.sh
pi@raspberrypi ~ $ make
pi@raspberrypi ~ $ sudo make install
pi@raspberrypi ~ $ cd ../matchbox-desktop
pi@raspberrypi ~ $ ./autogen.sh
pi@raspberrypi ~ $ make
pi@raspberrypi ~ $ sudo make install
pi@raspberrypi ~ $ sudo apt-get install matchbox-desktop

To start matchbox-desktop

pi@raspberrypi ~ $ FRAMEBUFFER=/dev/fb1 xinit /usr/local/bin/matchbox-desktop

BerryIMU for the Raspberry Pi

New Kickstarter Launched – BerryIMU

An accelerometer, gyroscope and magnetometer which is specifically designed for the Raspberry Pi.

 

What is it?

BerryIMU is an inertial measurement unit, or IMU, that measures and reports on velocity, orientation and gravitational forces, using a combination of an accelerometer, gyroscope and a magnetometer.

IMUs can be found in devices like, quad copters, smart phones, segways, Wii Remote etc.. and are used to sense movement and orientation.

BerryIMU for the raspberry pi

Adding BerryIMU to your Pi will open up a whole new world of possibilities.

BerryIMU is specifically designed for the Raspberry Pi and is designed to take some of the complexity out of IMUs and to try and make them more enjoyable to use. BerryIMU includes the latest and greatest sensor ICs.

Head on over to Kickstarter.com to check it out;
https://www.kickstarter.com/projects/2135028730/berryimu-orientation-sensor-for-the-raspberry-pi

Blip, blop, bloop…