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.

#!/usr/bin/python
import smbus
import time

bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) if you have a revision 2 board 
address = 0x77

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 -((0xffff - val) + 1)
    else:
        return val

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

def twos_compliment(val):
    if (val >= 0x8000):
        return -((0xffff - val) + 1)
    else:
        return val

def get_word(array, index, twos):
    val = (array[index] << 8) + array[index+1]
    if twos:
        return twos_compliment(val)
    else:
        return val

def calculate():
    # This code is a direct translation from the datasheet
    # and should be optimised for real world use
    
    #Calculate temperature
    x1 = ((temp_raw - ac6) * ac5) / 32768
    x2 = (mc * 2048) / (x1 + md)
    b5 = x1 + x2
    t = (b5 + 8) / 16
    
    # Now calculate the pressure
    b6 = b5 - 4000 
    x1 = (b2 * (b6 * b6 >> 12)) >> 11
    x2 = ac2 * b6 >> 11
    x3 = x1 + x2
    b3 = (((ac1 * 4 + x3) << oss) + 2) >> 2 
    
    x1 = (ac3 * b6) >> 13 
    x2 = (b1 * (b6 * b6 >> 12)) >> 16 
    x3 = ((x1 + x2) + 2) >> 2 
    b4 = ac4 * (x3 + 32768) >> 15 
    b7 = (pressure_raw - b3) * (50000 >> oss)
    if (b7 < 0x80000000):
        p = (b7 * 2) /b4
    else:
        p = (b7 / b4) *2
    x1 = (p >> 8) * (p >> 8)
    x1 = (x1 * 3038) >> 16
    x2 = (-7357 * p) >> 16
    p = p + ((x1 + x2 + 3791) >> 4)
    return(t,p)

calibration = bus.read_i2c_block_data(address, 0xAA, 22)

oss = 3              # Ultra high resolution
temp_wait_period = 0.004
pressure_wait_period = 0.0255 # Conversion time

# The sensor has a block of factory set calibration values we need to read
# these are then used in a length calculation to get the temperature and pressure
ac1 = get_word(calibration, 0, True)
ac2 = get_word(calibration, 2, True)
ac3 = get_word(calibration, 4, True)
ac4 = get_word(calibration, 6, False)
ac5 = get_word(calibration, 8, False)
ac6 = get_word(calibration, 10, False)
b1 =  get_word(calibration, 12, True)
b2 =  get_word(calibration, 14, True)
mb =  get_word(calibration, 16, True)
mc =  get_word(calibration, 18, True)
md =  get_word(calibration, 20, True)


while True:
    # Read raw temperature
    write_byte(0xF4, 0x2E)          # Tell the sensor to take a temperature reading
    time.sleep(temp_wait_period)    # Wait for the conversion to take place
    temp_raw = read_word_2c(0xF6)

    write_byte(0xF4, 0x34 + (oss << 6)) # Tell the sensor to take a pressure reading
    time.sleep(pressure_wait_period)    # Wait for the conversion to take place
    pressure_raw = ((read_byte(0xF6) << 16) \
                     + (read_byte(0xF7) << 8) \
                     + (read_byte(0xF8)) ) >> (8-oss)


    temperature, pressure = calculate()
    print time.time(), temperature / 10., pressure / 100.
    time.sleep(1)

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 read-pressure.py, ran it and re-directed the output to a file

sudo ./read-pressure.py > 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 (p₀) 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.

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

#!/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

x_out = read_word_2c(3) * scale
y_out = read_word_2c(7) * scale
z_out = read_word_2c(5) * scale

bearing  = math.atan2(y_out, x_out) 
if (bearing < 0):
    bearing += 2 * math.pi

print "Bearing: ", math.degrees(bearing)

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-test.py > 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

minx = 0
maxx = 0
miny = 0
maxy = 0

for i in range(0,500):
    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 x_out > maxx:
        maxx=x_out
    
    if y_out > maxy:
        maxy=y_out
    
    #print x_out, y_out, (x_out * scale), (y_out * scale)
    time.sleep(0.1)

print "minx: ", minx
print "miny: ", miny
print "maxx: ", maxx
print "maxy: ", maxy
print "x offset: ", (maxx + minx) / 2
print "y offset: ", (maxy + miny) / 2

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

x_offset = -10
y_offset = 10
x_out = (read_word_2c(3) - x_offset) * scale
y_out = (read_word_2c(7) - y_offset) * scale
z_out = (read_word_2c(5)) * scale

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.

Conclusion

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.

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 The Balance Filter by Shane Colton.  If you just want some simple code then read on.

The interesting parts are lines 76 to 91.

#!/usr/bin/python

import smbus
import math
import time

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

gyro_scale = 131.0
accel_scale = 16384.0

address = 0x68  # This is the address value read via the i2cdetect command

def read_all():
    raw_gyro_data = bus.read_i2c_block_data(address, 0x43, 6)
    raw_accel_data = bus.read_i2c_block_data(address, 0x3b, 6)

    gyro_scaled_x = twos_compliment((raw_gyro_data[0] << 8) + raw_gyro_data[1]) / gyro_scale
    gyro_scaled_y = twos_compliment((raw_gyro_data[2] << 8) + raw_gyro_data[3]) / gyro_scale
    gyro_scaled_z = twos_compliment((raw_gyro_data[4] << 8) + raw_gyro_data[5]) / gyro_scale

    accel_scaled_x = twos_compliment((raw_accel_data[0] << 8) + raw_accel_data[1]) / accel_scale
    accel_scaled_y = twos_compliment((raw_accel_data[2] << 8) + raw_accel_data[3]) / accel_scale
    accel_scaled_z = twos_compliment((raw_accel_data[4] << 8) + raw_accel_data[5]) / accel_scale

    return (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z)
    
def twos_compliment(val):
    if (val >= 0x8000):
        return -((65535 - val) + 1)
    else:
        return val

def dist(a, b):
    return math.sqrt((a * a) + (b * b))


def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

bus = smbus.SMBus(0)  # or bus = smbus.SMBus(1) for Revision 2 boards

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

now = time.time()

K = 0.98
K1 = 1 - K

time_diff = 0.01

(gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()

last_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
last_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

gyro_offset_x = gyro_scaled_x 
gyro_offset_y = gyro_scaled_y

gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y

print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (last_x), gyro_total_x, (last_x), (last_y), gyro_total_y, (last_y))

for i in range(0, int(3.0 / time_diff)):
    time.sleep(time_diff - 0.005) 
    
    (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()
    
    gyro_scaled_x -= gyro_offset_x
    gyro_scaled_y -= gyro_offset_y
    
    gyro_x_delta = (gyro_scaled_x * time_diff)
    gyro_y_delta = (gyro_scaled_y * time_diff)

    gyro_total_x += gyro_x_delta
    gyro_total_y += gyro_y_delta

    rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
    rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

    last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
    last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
    
    print "{0:.4f} {1:.2f} {2:.2f} {3:.2f} {4:.2f} {5:.2f} {6:.2f}".format( time.time() - now, (rotation_x), (gyro_total_x), (last_x), (rotation_y), (gyro_total_y), (last_y))

First we read all the scaled data from the device.

    (gyro_scaled_x, gyro_scaled_y, gyro_scaled_z, accel_scaled_x, accel_scaled_y, accel_scaled_z) = read_all()

Adjust the gyroscope data by the offset.

    gyro_scaled_x -= gyro_offset_x
    gyro_scaled_y -= gyro_offset_y

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

    gyro_x_delta = (gyro_scaled_x * time_diff)
    gyro_y_delta = (gyro_scaled_y * time_diff)

    gyro_total_x += gyro_x_delta
    gyro_total_y += gyro_y_delta

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 

    rotation_x = get_x_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)
    rotation_y = get_y_rotation(accel_scaled_x, accel_scaled_y, accel_scaled_z)

Now the complementary filter is used to combine the data.

    last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
    last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)

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 ./filter-test.py > 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 web.py, 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
vi server.py

Use the following as a test

#!/usr/bin/python
import web

urls = (
    '/', 'index'
)

class index:
    def GET(self):
        return "Hello, world!"

if __name__ == "__main__":
    app = web.application(urls, globals())
    app.run()

Save the and then set it as executable with

chmod +x server.py 

and then run the code

./server.py

 you will see something like this showing the server is waiting for a request (pressing Ctrl+C will stop the server)

http://0.0.0.0:8080/

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 server.py with

#!/usr/bin/python
import web
import smbus
import math

urls = (
    '/', 'index'
)

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command


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 dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)


class index:
    def GET(self):
        accel_xout = read_word_2c(0x3b)
        accel_yout = read_word_2c(0x3d)
        accel_zout = read_word_2c(0x3f)

        accel_xout_scaled = accel_xout / 16384.0
        accel_yout_scaled = accel_yout / 16384.0
        accel_zout_scaled = accel_zout / 16384.0

        return str(get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))+" "+str(get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled))


if __name__ == "__main__":

    # Now wake the 6050 up as it starts in sleep mode
    bus.write_byte_data(address, power_mgmt_1, 0)

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

The server has to be run as sudo so you have permissions to read from the I²C

sudo ./server.py

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 level.py) and run it

#!/usr/bin/python

import pygame
import urllib
from OpenGL.GL import *
from OpenGL.GLU import *
from math import radians
from pygame.locals import *

SCREEN_SIZE = (800, 600)
SCALAR = .5
SCALAR2 = 0.2

def resize(width, height):
    glViewport(0, 0, width, height)
    glMatrixMode(GL_PROJECTION)
    glLoadIdentity()
    gluPerspective(45.0, float(width) / height, 0.001, 10.0)
    glMatrixMode(GL_MODELVIEW)
    glLoadIdentity()
    gluLookAt(0.0, 1.0, -5.0,
              0.0, 0.0, 0.0,
              0.0, 1.0, 0.0)
    
def init():
    glEnable(GL_DEPTH_TEST)
    glClearColor(0.0, 0.0, 0.0, 0.0)
    glShadeModel(GL_SMOOTH)
    glEnable(GL_BLEND)
    glEnable(GL_POLYGON_SMOOTH)
    glHint(GL_POLYGON_SMOOTH_HINT, GL_NICEST)
    glEnable(GL_COLOR_MATERIAL)
    glEnable(GL_LIGHTING)
    glEnable(GL_LIGHT0)
    glLightfv(GL_LIGHT0, GL_AMBIENT, (0.3, 0.3, 0.3, 1.0));

def read_values():
    link = "http://192.168.1.65:8080" # Change this address to your settings
    f = urllib.urlopen(link)
    myfile = f.read()
    return myfile.split(" ")

def run():
    pygame.init()
    screen = pygame.display.set_mode(SCREEN_SIZE, HWSURFACE | OPENGL | DOUBLEBUF)
    resize(*SCREEN_SIZE)
    init()
    clock = pygame.time.Clock()
    cube = Cube((0.0, 0.0, 0.0), (.5, .5, .7))
    angle = 0
    
    while True:
        then = pygame.time.get_ticks()
        for event in pygame.event.get():
            if event.type == QUIT:
                return
            if event.type == KEYUP and event.key == K_ESCAPE:
                return

        values = read_values()
        x_angle = values[0]
        y_angle = values[1]

        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)

        glColor((1.,1.,1.))
        glLineWidth(1)
        glBegin(GL_LINES)

        for x in range(-20, 22, 2):
            glVertex3f(x/10.,-1,-1)
            glVertex3f(x/10.,-1,1)
        
        for x in range(-20, 22, 2):
            glVertex3f(x/10.,-1, 1)
            glVertex3f(x/10., 1, 1)
        
        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z/10.)
            glVertex3f( 2, -1, z/10.)

        for z in range(-10, 12, 2):
            glVertex3f(-2, -1, z/10.)
            glVertex3f(-2,  1, z/10.)

        for z in range(-10, 12, 2):
            glVertex3f( 2, -1, z/10.)
            glVertex3f( 2,  1, z/10.)

        for y in range(-10, 12, 2):
            glVertex3f(-2, y/10., 1)
            glVertex3f( 2, y/10., 1)
        
        for y in range(-10, 12, 2):
            glVertex3f(-2, y/10., 1)
            glVertex3f(-2, y/10., -1)
        
        for y in range(-10, 12, 2):
            glVertex3f(2, y/10., 1)
            glVertex3f(2, y/10., -1)
        
        glEnd()
        glPushMatrix()
        glRotate(float(x_angle), 1, 0, 0)
        glRotate(-float(y_angle), 0, 0, 1)
        cube.render()
        glPopMatrix()
        pygame.display.flip()

class Cube(object):

    def __init__(self, position, color):
        self.position = position
        self.color = color

    # Cube information
    num_faces = 6

    vertices = [ (-1.0, -0.05, 0.5),
                 (1.0, -0.05, 0.5),
                 (1.0, 0.05, 0.5),
                 (-1.0, 0.05, 0.5),
                 (-1.0, -0.05, -0.5),
                 (1.0, -0.05, -0.5),
                 (1.0, 0.05, -0.5),
                 (-1.0, 0.05, -0.5) ]

    normals = [ (0.0, 0.0, +1.0),  # front
                (0.0, 0.0, -1.0),  # back
                (+1.0, 0.0, 0.0),  # right
                (-1.0, 0.0, 0.0),  # left
                (0.0, +1.0, 0.0),  # top
                (0.0, -1.0, 0.0) ]  # bottom

    vertex_indices = [ (0, 1, 2, 3),  # front
                       (4, 5, 6, 7),  # back
                       (1, 5, 6, 2),  # right
                       (0, 4, 7, 3),  # left
                       (3, 2, 6, 7),  # top
                       (0, 1, 5, 4) ]  # bottom

    def render(self):
        then = pygame.time.get_ticks()
        glColor(self.color)

        vertices = self.vertices

        # Draw all 6 faces of the cube
        glBegin(GL_QUADS)

        for face_no in xrange(self.num_faces):
            glNormal3dv(self.normals[face_no])
            v1, v2, v3, v4 = self.vertex_indices[face_no]
            glVertex(vertices[v1])
            glVertex(vertices[v2])
            glVertex(vertices[v3])
            glVertex(vertices[v4])
        glEnd()

if __name__ == "__main__":
    run()

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.

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 I²C 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

#!/usr/bin/python

import smbus
import math

# Power management registers
power_mgmt_1 = 0x6b
power_mgmt_2 = 0x6c

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 dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_y_rotation(x,y,z):
    radians = math.atan2(x, dist(y,z))
    return -math.degrees(radians)

def get_x_rotation(x,y,z):
    radians = math.atan2(y, dist(x,z))
    return math.degrees(radians)

bus = smbus.SMBus(0) # or bus = smbus.SMBus(1) for Revision 2 boards
address = 0x68       # This is the address value read via the i2cdetect command

# Now wake the 6050 up as it starts in sleep mode
bus.write_byte_data(address, power_mgmt_1, 0)

print "gyro data"
print "---------"

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)

print
print "accelerometer data"
print "------------------"

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

print "accel_xout: ", accel_xout, " scaled: ", accel_xout_scaled
print "accel_yout: ", accel_yout, " scaled: ", accel_yout_scaled
print "accel_zout: ", accel_zout, " scaled: ", accel_zout_scaled

print "x rotation: " , get_x_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)
print "y rotation: " , get_y_rotation(accel_xout_scaled, accel_yout_scaled, accel_zout_scaled)

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.

accel_xout = read_word_2c(0x3b)
accel_yout = read_word_2c(0x3d)
accel_zout = read_word_2c(0x3f)

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

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

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.

accel_xout_scaled = accel_xout / 16384.0
accel_yout_scaled = accel_yout / 16384.0
accel_zout_scaled = accel_zout / 16384.0

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.

def dist(a,b):
    return math.sqrt((a*a)+(b*b))

def get_x_rotation(x,y,z):
    radians = math.atan(x / dist(y,z))
    return math.degrees(radians)

def get_y_rotation(x,y,z):
    radians = math.atan(y / dist(x,z))
    return math.degrees(radians)

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.7^o around X and -0.1^o 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

gyro_xout = read_word_2c(0x43)
gyro_yout = read_word_2c(0x45)
gyro_zout = read_word_2c(0x47)

print "gyro_xout: ", gyro_xout, " scaled: ", (gyro_xout / 131)
print "gyro_yout: ", gyro_yout, " scaled: ", (gyro_yout / 131)
print "gyro_zout: ", gyro_zout, " scaled: ", (gyro_zout / 131)

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.

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 I²C 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

i2c-bcm2708
i2c-dev

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)

or

sudo i2cdetect -y 1 (for a Revision 2 board)

then you should see output showing any I²C 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.