Saturday, 14 December 2013

Pitch, Roll and Yaw using MPU6050 & HMC5883L (with tilt compensation and complementary filter)

Combining the data from an MPU605 and a HMC5883L to give tilt compensated pitch, roll and yaw.

Pitch, roll and yaw (with tilt compensation)

The first thing to do it calibrate the compass similar to this previous post but this time for all three axes.  Run the following Python code, while it's running repeatedly rotate the HMC5883L sensor in three dimension.
Once it's finished it will print out the offset values we need, here are my results.
    minx:  -517
    miny:  -508
    minz:  -600
    maxx:  536
    maxy:  489
    maxz:  321
    x offset:  9
    y offset:  -10
    z offset:  -140

Now we have the calibration data let's plug that in and get a 3d visualisation up and running. I'm assuming that you have installed and running as described in this post.

I have uploaded all the code to github, go ahead and clone this to your Raspberry Pi and your desktop/laptop with the command
    git clone 
    cd raspi/i2c-sensors

Now edit the file bitify/python/web/ and change the values on line 14 to the values you got from the calibration stage.
To start the simple web server run the following command, it has to be run a sudo to access the I2C bus.
    sudo ./
This will start the server which will serve the sensor readings to the 3d program running on a desktop or laptop.  I have only tested this on Linux, it may need adjusting to get it running on Macs and Windows machines.
Make sure you have cloned the gihub repository to you desktop/laptop and then run the following command:
    cd raspi/i2c-sensors/
    python ./bitify/python/graphics/
All being well a window will open showing something similar to the image above.  As you rotate the sensor the visualisation will follow the movements, pitching and rolling will not adversely affect the yaw value (see article linked below for limits to the effectiveness).

Quick code overview

  • i2c-sensors/bitify/python/sensors/ - Simple Python module to access the data from the compass sensor
  • i2c-sensors/bitify/python/sensors/ - Simple Python module to access the data from the gyroscope and accelerometer
These two module are base on the code from previous blogs but has been extended to allow better sensor configuration. The file contains the code to performs tilt compensation on the compass.  A detailed discussion on tilt compensation can be seen here.  I had to amend the code slightly as the article uses different axes for pitch and roll to my set up.

  • i2c-sensors/bitify/python/sensors/ - IMU module which combines the data from both the HMC5883L and MPU6050
This module takes the data from the two sensors and combines them to produce pitch, roll and yaw values. It also holds the complementary filter code but I may move this into the sensor module code.


With three relatively small Python modules we now have a system to determine the orientation of the sensor in all three axes of rotation.   I hope to extend this small library to handle three more sensors, the ADXL345 (accelerometer), the L3G4200D (gyroscope) and the BMP085 (pressure sensor).  My ultimate aim is to build a flight controller for a quadcopter.

Sunday, 24 November 2013

Interfacing a BMP085 Digital Pressure sensor to the Raspberry Pi

I recently bought a sensor with a BMP085 Digital Pressure sensor on it so I thought I'd write a post on how to read the data from the Raspberry Pi in Python over I2C.

Python code

Below is simple test code to initialise the sensor and then continuously loop around reading the temperature and air pressure.
To get a reading out of the sensor you first have to read the factory set calibration block (lines 080-090).  This is different for each device and is used in the lengthy calculations for both temperature and pressure.  The function calculate() is just a direct translation of the code presented in the datasheet, I don't understand what it's doing but it gives us the required values.

Testing the sensor and the code

To test everything was working OK I saved the above code to a file called, ran it and re-directed the output to a file
sudo ./ > pressure-test.dat
I then slowly walked up and down the stairs in my house to get some data.  Then plotted the data with the following gnuplot program

  set terminal wxt persist size 800,800 background '#000000' 
  set style line 99 linecolor rgb "#ffffff" linetype 0 linewidth 2
  set key top right textcolor linestyle 99 
  set grid linestyle 99
  set border linestyle 99

  set yrange  [16.4:17.2]
  set y2range [1003.5:1005]
  set y2tics

  plot filename using 1:2 axes x1y1 title "True temp" w l ,\
       filename using 1:3 axes x1y2 title "True pressure" w l, \
       filename using 1:3 axes x1y2 title "Smoothed" smooth bezier

Here is the command to generate the plot below
gnuplot -e "filename='pressure-test.dat'" gnuplot-pressure.plg
You can see the pressure dropping as I went up the stairs and then back down again.  You can see the temperature went up slightly too which I think was just heat from my hand slowly raising it.

Sensor response as I walked up and down the stairs
To calculate altitude (height above ground) I used the first (p0) and the lowest (p) readings from the output and plugged them into the following formula, again this is taken from the datasheet.

This gave me a height of 2.86m, I was surprised to get a significant reading by just walking up and down the stairs so when I finally add it to a quad-copter I should get good results.

Sunday, 17 November 2013

Connecting and calibrating a HMC5883L Compass on the Raspberry Pi

Here is how to connect a HMC5883L Compass to the Raspberry Pi, calibrate it and read the data. Connecting the compass is simple enough, follow the steps here which show how to connect a similar I2C device.

Simple Python Code

Some simple code to read the data, calculate a bearing and print it out
When you run the script you'll get something like
    Bearing:  70.0168934781

Calibrating the compass

Well that was easy wasn't it. Well not quite, if you start to rotate your device you might notice the values don't seem right. Start by rotating it until you get close to 0 degrees, then rotate it physically through four 90 degree steps, taking a reading at each step. Continue until you have gone through 360 degrees. Here is my output
    Bearing:  0.292322869229
    Bearing:  89.4543424066
    Bearing:  175.645778937
    Bearing:  266.66908262
    Bearing:  0.298412819995
So what's happening?  We can see the readings are off, but not by much.  Make a change to the code by replacing lines 35-43 with
re-run the program and direct the output to a file. While the program is running repeatedly rotate your compass backwards and forwards through 360 degrees, make sure you keep it flat otherwise you'll get some odd results.
    sudo ./ > compass-plot.dat
Now lets plots the data and look at what's going on, here is a gnuplot program and the command to run it
set terminal wxt persist size 800,800 background '#000000' 

    set style line 99 linecolor rgb "#ffffff" linetype 0 linewidth 2
    set key top right textcolor linestyle 99 
    set grid linestyle 99
    set border linestyle 99

    plot filename using 1:2 title "Raw compass values" linecolor rgb "green" 
Save this to gnuplot-compass.plg and then run the following command
    gnuplot -e "filename='compass-plot.dat'" gnuplot-compass.plg
You'll then be presented with a graph similar to this one

Scatter digram of the raw compass data
We can see that the circle isn't quite centered around the origin, although in this case it's not off by much. My previous compass was off by a much larger value. This plot shows the offset in Y to be about 150.

Scatter diagram of my old compass which had a much bigger offset value

Now we can use this data to calculate the standard offset that we apply to the compass readings to correct things. Replace the for loop in the Python program with

run the program again rotating the compass through 360 degrees. Once the program finishes it will print out the offsets you need to apply to your calculations, these are the values I got
    minx:  -216
    miny:  -193
    maxx:  197
    maxy:  213
    x offset:  -10
    y offset:  10
We are almost done, back to the first program and include the offset in the calculations, changes lines 35-37 to Then re-run the test by taking a reading every 90 degrees of rotation, here are my newly adjusted values
    Bearing:  0.278132667296
    Bearing:  90.0
    Bearing:  180.290839022
    Bearing:  272.501622814
    Bearing:  359.725859606
As you can see these values are much better, there will always be a small variance as the data is a bit noisy.


As you can see it's relatively easy to attached a compass module to your Raspberry Pi, calibrate it and start to get meaningful readings.  Oh and don't have any large metal objects or large bits of electrical equipment close to the compass when testing.

Saturday, 16 November 2013

Using a complementary filter to combine Accelerometer and Gyroscopic data

This post shows how to combine data from the accelerometer and gyroscope using a complementary filter to produce a better readings from the MPU-6050.

Complementary filter
The image above shows data for a negative rotation around the Y axis followed by a positive rotation around the X axis.  It includes the base accelerometer, gyroscope and the filtered data.  Lets look at things in a bit more detail.

The following graph show a simple rotation in X of roughly 90-100 degrees (I didn't measure it accurately).  The red line shows the accelerometer data and as we can see from the spikes it's a noisy data set.  The green line show the rotation angle calculated from summing the individual angles read from the gyroscope.  While this data is less noisy it is prone to drift over time, the gyroscope doesn't return back to zero when not moving.

The blue line shows the complementary filter at work.  It combines the two data sets by merging fast rotations from the gyroscope with the slower trends from the accelerometer and we get the best of both worlds. For a full explanation of the theory behind this type of filter I recommend reading this excellent paper.  If you just want some simple code then read on.

The interesting parts are lines 76 to 91. First we read all the scaled data from the device. Adjust the gyroscope data by the offset. The offset is the value of the gyroscope reading when it's not moving and is taken from the very first reading. This is fine for simple testing but ideally a true offset value should be determined by calibrating the sensor.
Now calculate the gyroscope delta, this is how much the sensor has rotated since the last sample was taken and then add it to a running total This gives us a rotation angle just from reading from the gyroscope (the green line in the graph above)
Next read the rotation values from the accelerometer just like we did in the previous post Now the complementary filter is used to combine the data.
We take the previous readings (last_x, last_y) and add in the gyroscope data then scale this by K, then add in the accelerometer data scaled by K1 and this value is our new angle. The coefficients K and K1 should add up to 1, in this case they are 0.98 and 0.02 respectively. You can change the values of K and K1 to suit your application as described in the previously linked article. The time intervals for the loop needs to be reasonably accurate for this to work well and the sample rate should be 100Hz or higher.

If you run the code and direct the output to a file
    sudo ./ > plot.dat
you can then generate gnuplot diagrams similar to those above, save the following to a file gnuplot-command.plg
    set terminal wxt persist size 800,600 background '#000000' # enhanced font 'Consolas,10' 

    set style line 99 linecolor rgb "#ffffff" linetype 0 linewidth 2
    set key top right textcolor linestyle 99 
    set grid linestyle 99
    set border linestyle 99

    set xlabel "time (s)" textcolor linestyle 99
    set ylabel "degrees" textcolor linestyle 99

    set yrange [-180:180]

    plot filename using 1:2 title "Accelerometer X" with line linewidth 2 , \
      filename using 1:3 title "Gyroscope X" with line linewidth 2 , \
      filename using 1:4 title "Filter X" with line linewidth 2
    plot filename using 1:5 title "Accelerometer Y" with line linewidth 2 , \
      filename using 1:6 title "Gyroscope Y" with line linewidth 2 , \
      filename using 1:7 title "Filter Y" with line linewidth 2 
then to generate a graph
    gnuplot -e "filename='plot.dat'" gnuplot-command.plg

In the next post I show how to hook up a HMC5883L Compass module and incorporate it into the code so we can get a true bearing.  Well that is when I get a new one as I seem to have fried mine.

3D OpenGL visualisation of the data from an MPU-6050 connected to a Raspberry Pi

In this post I'll show how to serve the data over http and display a 3D representation in OpenGL extending on a previous blog post detailing how to read data from the MPU-6050 sensor and convert it into a something useful.

Using a simple web server to serve up the data

Let's start by setting up a simple server based on, which is installed via apt-get
sudo apt-get install python-webpy
Now create a directory to put the code in and create a simple test program
mkdir webpy
cd webpy
Use the following as a test
Save the and then set it as executable with
chmod +x 
and then run the code
 you will see something like this showing the server is waiting for a request (pressing Ctrl+C will stop the server)
Now point your browser at http://ip-address-of-your-pi:8080 and it will show a web page with the content of Hello, world!.  We can make use of this to read data from a remote machine, in my case my Linux desktop.

Adding the sensor code to the server

Replace the contents of with The server has to be run as sudo so you have permissions to read from the I2C
sudo ./
Connecting via your browser will now return the X & Y rotation values e.g.
-28.7291281627 -39.4833542336

3D visualisation

I'm using a Linux desktop and that is all I have tested this simple code on, I've no idea if it works on Windows or Macs and it certainly won't run on the Pi itself.  I'm no OpenGL guru so this code is just hacked together to get something visible. 

Setting up OpenGL and pygame
sudo apt-get install python-opengl
sudo apt-get install python-pygame
Now save the following to a file (in my case and run it
Remember to change the URL line 038 to your specific value (the address you used earlier to test the server). When you run it a window will open showing the orientation of the sensor, rotating the sensor will update the display.

You'll notice that when the sensor isn't being physically moved the noisy data is causing it to wobble.  The next blog post shows how to reduce this.

Thursday, 7 November 2013

Reading data from the MPU-6050 on the Raspberry Pi

In a previous post I showed how to connect an Accelerometer & Gyro sensor to the Raspberry Pi, in this post I'll show some simple Python code to read the data it offers.

To be able to read from the I2C using Python bus we need to install the smbus module
sudo apt-get install python-smbus 
Now to some code, this is just simple test code to make sure the sensor is working
When you run the code you will see output similar to this
gyro data
gyro_xout:  -92  scaled:  -1
gyro_yout:  294  scaled:  2
gyro_zout:  -104 scaled:  -1

accelerometer data
accel_xout:  -3772  scaled:  -0.230224609375
accel_yout:  -52    scaled:  -0.003173828125
accel_zout:  15408  scaled:  0.9404296875
x rotation:  -13.7558411667
y rotation:  -0.187818934829

Accelerometer data

Let's have a look at the code in more detail.
These three lines read the raw X,Y & Z accelerometer values, the parameter in each call is the register within the sensor that holds the data.  The sensor has a number of registers which have different functionality as documented in this datasheet.  The registers we are interested in for the acceleromter data are 0x3b, 0x3d, 0x3f and these hold the raw data in 16 bit two's complement format.

The following code reads a word (16 bits) from a given register and converts it from two's complement
Once we have the raw data we need to scale it and then convert it into something useful like a rotation angle. Again from the data sheet we can see the default scaling we need to apply to the raw accelerometer values is 16384, so we divide the raw data by this value.
Now we have the values that gravity is exerting on the sensor in each of the three dimensions, from this we can calculate the rotations in the X & Y axes.
Here is an excellent article showing the details behind the maths for this.  What this gives us is the rotation angle in degrees for both the X & Y axes and is shown in the output.
x rotation: -13.755841166
y rotation: -0.187818934829
So in this instance the sensor is rotated by -13.7o around X and -0.1o around Y.

Gyroscope data

In a similar manner we can read the data from the Gyroscope part of the sensor. This is done in the following code
So we read the values from the registers 0x43, 0x45 & 0x47, again we can see from the datasheet that these hold the raw gyro data. To scale these we divide by 131 to give the degrees per second rotation value.
gyro_xout:  -92  scaled:  -1
gyro_yout:  294  scaled:  2
gyro_zout:  -104 scaled:  -1
The output in my case show the gyro wasn't moving when I took reading.

Final thoughts

The code I present here is very basic and should be extended to handle errors and allow the sensor to be configured with different sensitivity levels. I've done this in my application and embedded it into a web server. This allows me to make a simple http request to the Raspberry Pi and get a reading from the sensor.

To help me test and visualise the data better I've written some simple OpenGL code to graphically represent the sensor's orientation in 3D space.

This OpenGL code runs on my Linux desktop machine and queries the Pi periodically to get the data and renders the above image. See this post for details how

In the next article I'll show how to combine the accelerometer and gyroscope data together to get a more accurate reading and help reduce noise.

Wednesday, 6 November 2013

Interfacing Raspberry Pi and MPU-6050

I wanted to interface my Pi to a Six-Axis Gyro + Accelerometer sensor and the one I settled on was based on a MPU-6050 chip.  I went for this board mainly because I could get it cheap on eBay and wasn't worried about the cost if I broke it.

Found on eBay for a few quid

Set up (for Rasbian)

It's an I2C board so first you need to install the relevant Linux drivers, here's how.  Open the file for editing (needs sudo)
sudo vi /etc/modules
add the following lines to the bottom of the file, save it and reboot the Pi
Now check the blacklists file
sudo vi /etc/modprobe.d/raspi-blacklist.conf

and make sure that the following lines start with a # (a comment) if they are present, if not don't worry
#blacklist spi-bcm2708
#blacklist i2c-bcm2708
Pin connections

Connecting the sensor 

To connect the sensor you need to use the GPIO pins on the Pi, the important pins are
  • Pin 1 - 3.3V connect to VCC
  • Pin 3 - SDA connect to SDA
  • Pin 5 - SCL connect to SCL
  • Pin 6 - Ground connect to GND
these need to be connect as shown in the image.

Once you have the board connected you can test to see if the Pi has detected it.  This is done with the following command to install the i2c tools
sudo apt-get install i2c-tools
and then either
sudo i2cdetect -y 0 (for a Revision 1 board like mine)
sudo i2cdetect -y 1 (for a Revision 2 board)
then you should see output showing any I2C devices that are attached and their addresses

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- --
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- --
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- --
70: -- -- -- -- -- -- -- --

This shows that the Pi has detected the sensor with an address of 0x68 (hexadecimal), this address is needed to interact with it.  Enter the following command and you should get an output of 0x68 on screen if everything is working properly.
sudo i2cget -y 0 0x68 0x75
This command talks to the device whose address is 0x68 (the sensor) and retrieves the value in the register 0x75 which has a default value of 0x68 the same value as the address.

Next time I'll show how to use Python to read data from the sensor.