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.
import smbus
import time
import math

bus = smbus.SMBus(0)
address = 0x1e

def read_byte(adr):
    return bus.read_byte_data(address, adr)

def read_word(adr):
    high = bus.read_byte_data(address, adr)
    low = bus.read_byte_data(address, adr+1)
    val = (high << 8) + low
    return val

def read_word_2c(adr):
    val = read_word(adr)
    if (val >= 0x8000):
        return -((65535 - val) + 1)
        return val

def write_byte(adr, value):
    bus.write_byte_data(address, adr, value)

write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
write_byte(2, 0b00000000) # Continuous sampling

scale = 0.92

minx = 0
maxx = 0
miny = 0
maxy = 0
minz = 0
maxz = 0

for i in range(0,100):
    x_out = read_word_2c(3)
    y_out = read_word_2c(7)
    z_out = read_word_2c(5)
    if x_out < minx:
    if y_out < miny:

    if z_out < minz:
    if x_out > maxx:
    if y_out > maxy:

    if z_out > maxz:

    #print x_out, y_out, (x_out * scale), (y_out * scale)

print "minx: ", minx
print "miny: ", miny
print "minz: ", minz
print "maxx: ", maxx
print "maxy: ", maxy
print "maxz: ", maxz

print "x offset: ", (maxx + minx) / 2
print "y offset: ", (maxy + miny) / 2
print "z offset: ", (maxz + minz) / 2
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.
#!/usr/bin/env python

import web  #
import smbus

import bitify.python.sensors.imu as imu

urls = (
    '/', 'index'

bus = smbus.SMBus(0)
imu_controller = imu.IMU(bus, 0x68, 0x1e, "IMU")
imu_controller.set_compass_offsets(9, -10, -140)

app = web.application(urls, globals())

class index:
    def GET(self):
        (pitch, roll, yaw) = imu_controller.read_pitch_roll_yaw()
        result = "%.2f %.2f %.2f" % (pitch, roll, yaw)
        return result

if __name__ == "__main__":
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.

Temperature logging with a DS18B20 and a Raspberry Pi

I wanted to do some temperature logging so I hooked up a DS18B20 temperature sensor to a Raspberry Pi. About the DS18B20 Dallas DS18B...