本文适合所有的电子指南针校准,不仅限于 HMC5883L. 硬件的连接和树莓派的搭建以后再写。
本文仅限于固定强度和方向磁场干扰的过滤,例如机器人自身其它设备产生的磁场。但对于环境如扩音器喇叭等磁场干扰无效.
首先进行X Y 方向的校准,将芯片平放固定到一个水平面,这时大地磁场是近似平行XY平面的,绕Z轴慢速的转动XY平面,电子指南针HMC5883L会测量出圆周内大地磁场的强度+固定干扰磁场。而固定磁场的强度就是所有测量值的平均值。
#!/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
for i in range(0,500):
x_out = read_word_2c(3)
y_out = read_word_2c(7)
z_out = read_word_2c(5)
bearing = math.atan2(y_out, x_out)
if (bearing < 0):
bearing += 2 * math.pi
print x_out, y_out, (x_out * scale), (y_out * scale)
time.sleep(0.1)
运行上面的程序,在50秒内不停地旋转装有芯片的水平面,360度的缓慢旋转,一定要保证水平面稳定,否则会得到很多奇怪的干扰数据。
sudo ./compass-test.py > compass-plot.dat
将输出结果导入到compas-plot.dat文件里面。接下来我们用GNUPlot来看一下数据结果,以每对儿X Y为坐标的图形效果。
set terminal wxt persist size 800,800 background'#000000'
set style line99 linecolor rgb "#ffffff" linetype 0 linewidth 2
set key topright textcolor linestyle 99
set gridlinestyle 99
set borderlinestyle 99
plot filenameusing 1:2 title "Raw compass values" linecolor rgb "green"
将上面命令文本保存到 gnuplot-compass.plg并执行下面的命令,可以看到图形:
gnuplot -e "filename='compass-plot.dat'"gnuplot-compass.plg
这时我们可以看到图形的圆心不是在 0,0 点
把上面程序的for循环内容更换成下面的代码,运行50秒不停地手动旋转水平面,
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
这一次会输出下面的值:
minx: -216
miny: -193
maxx: 197
maxy: 213
x offset: -10
y offset: 10
把输出的偏移加到程序当中,第一段程序的读取值的代码换成下面:
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
Z 方向的校准同上面的方法,只是要把XZ平面固定到水平面上做同样的事情而已。
版权声明:本文为博主原创文章,未经博主允许不得转载。