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.

#!/usr/bin/python
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)
    else:
        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:
        minx=x_out
    
    if y_out < miny:
        miny=y_out

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

    if z_out > maxz:
        maxz=z_out
   

    
    #print x_out, y_out, (x_out * scale), (y_out * scale)
    time.sleep(0.1)

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 web.py 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 https://github.com/bitify/raspi.git 
then
    cd raspi/i2c-sensors

Now edit the file bitify/python/web/server.py and change the values on line 14 to the values you got from the calibration stage.

#!/usr/bin/env python

import web  # web.py
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__":
    app.run()

To start the simple web server run the following command, it has to be run a sudo to access the I2C bus.

    sudo ./run-server.py

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/3d-display.py

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/hmc5883l.py - Simple Python module to access the data from the compass sensor
• i2c-sensors/bitify/python/sensors/mpu6050.py - 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 hmc8553l.py 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.py - 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.

Conclusion

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.