本文適合所有的電子指南針校準,不僅限於 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平面固定到水平面上做同樣的事情而已。
版權聲明:本文爲博主原創文章,未經博主允許不得轉載。