## 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.
``````#!/usr/bin/python
import smbus
import time

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

val = (high << 8) + low
return val

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

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)

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:
write_byte(0xF4, 0x2E)          # Tell the sensor to take a temperature reading
time.sleep(temp_wait_period)    # Wait for the conversion to take place

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) \

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 (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
``````#!/usr/bin/python
import smbus
import time
import math

bus = smbus.SMBus(0)

val = (high << 8) + low
return val

if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val

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

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):

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
``````
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.

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

gyro_scaled_x = twos_compliment((raw_gyro_data << 8) + raw_gyro_data) / gyro_scale
gyro_scaled_y = twos_compliment((raw_gyro_data << 8) + raw_gyro_data) / gyro_scale
gyro_scaled_z = twos_compliment((raw_gyro_data << 8) + raw_gyro_data) / gyro_scale

accel_scaled_x = twos_compliment((raw_accel_data << 8) + raw_accel_data) / accel_scale
accel_scaled_y = twos_compliment((raw_accel_data << 8) + raw_accel_data) / accel_scale
accel_scaled_z = twos_compliment((raw_accel_data << 8) + raw_accel_data) / 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):

def get_x_rotation(x,y,z):

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

# Now wake the 6050 up as it starts in sleep mode

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

val = (high << 8) + low
return 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):

def get_x_rotation(x,y,z):

class index:
def GET(self):

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

app = web.application(urls, globals())
app.run()
``````
The server has to be run as sudo so you have permissions to read from the I2C
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 pygame.locals import *

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

def resize(width, height):
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
gluPerspective(45.0, float(width) / height, 0.001, 10.0)
glMatrixMode(GL_MODELVIEW)
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)
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));

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

x_angle = values
y_angle = values

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

for face_no in range(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.

## 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
``````#!/usr/bin/python

import smbus
import math

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

val = (high << 8) + low
return 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):

def get_x_rotation(x,y,z):

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

# Now wake the 6050 up as it starts in sleep mode

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

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_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)
``````
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):
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):

def get_y_rotation(x,y,z):
``````
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
``````gyro_xout = read_word_2c(0x43)

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.

## 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
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 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.

### 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... 