HMC5883L 电子指南针用树莓派进行磁场干扰过滤 校准

本文适合所有的电子指南针校准，不仅限于 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平面固定到水平面上做同样的事情而已。