![]() |
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.pyThis 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.pyAll 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
- i2c-sensors/bitify/python/sensors/imu.py - IMU module which combines the data from both the HMC5883L and MPU6050
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.