21-05-22：
前言：最近课业十分紧张所以内容显得十分凌乱，注释也没来的及补充，还请多见谅。
博主现在在大三下学期，学校课设是一个很变态的智能小车项目，老师ban了树莓派，Arduino， K210这种算力比较高且发展比较完善的平台。所以不得不使用Openmv踩坑。
Openmv在二进制运算和二进制十进制转换方面做的让人十分想吐槽，大概本来也就不是用来干其他事情的？
先附上参考的代码链接，是树莓派上实现的链接，本人做了少许移植和后期参数校正。 1.bitbucket.org/thinkbowl/i… 2.bitbucket.org/thinkbowl/i…
代码由本人编辑的主要为class类中的getxes()和bin2int()，其余内容基本照搬，仅仅修改了I2C的库函数。后面对X和Y进行了offset修正以适应本地地理磁场环境。
HMC5883l参考手册强烈推荐这个版本(比大部分能找到的手册都要齐全)： www.digikey.com/en/datashee…
另外HMC5883l在IIC上遇到的很奇怪的bug是只能使用0x1E作为设备地址，文档里的0x3C和0x3D似乎没什么用。
最终结果和实际手机指南针结果差距基本在10°以内，对于大部分角度都是1°的误差。
import math
from pyb import I2C, Pin, Timer
from time import *
class i2c_hmc5883l:
ConfigurationRegisterA = 0x00
ConfigurationRegisterB = 0x01
ModeRegister = 0x02
AxisXDataRegisterMSB = 0x03
AxisXDataRegisterLSB = 0x04
AxisZDataRegisterMSB = 0x05
AxisZDataRegisterLSB = 0x06
AxisYDataRegisterMSB = 0x07
AxisYDataRegisterLSB = 0x08
StatusRegister = 0x09
IdentificationRegisterA = 0x10
IdentificationRegisterB = 0x11
IdentificationRegisterC = 0x12
MeasurementContinuous = 0x00
MeasurementSingleShot = 0x01
MeasurementIdle = 0x03
def __init__(self, gauss=1.3):
#def __init__(self, port, addr=0x1e, gauss=1.3):
#self.bus = i2c.i2c(port, addr)
self.bus = I2C(2, I2C.MASTER)
self.setScale(gauss)
def __str__(self):
ret_str = ""
(x, y, z) = self.getAxes()
ret_str += "Axis X: "+str(x)+"\n"
ret_str += "Axis Y: "+str(y)+"\n"
ret_str += "Axis Z: "+str(z)+"\n"
ret_str += "Declination: "+self.getDeclinationString()+"\n"
ret_str += "Heading: "+self.getHeadingString()+"\n"
return ret_str
def setContinuousMode(self):
self.setOption(self.ModeRegister, self.MeasurementContinuous)
def setScale(self, gauss):
if gauss == 0.88:
self.scale_reg = 0x00
self.scale = 0.73
elif gauss == 1.3:
self.scale_reg = 0x01
self.scale = 0.92
elif gauss == 1.9:
self.scale_reg = 0x02
self.scale = 1.22
elif gauss == 2.5:
self.scale_reg = 0x03
self.scale = 1.52
elif gauss == 4.0:
self.scale_reg = 0x04
self.scale = 2.27
elif gauss == 4.7:
self.scale_reg = 0x05
self.scale = 2.56
elif gauss == 5.6:
self.scale_reg = 0x06
self.scale = 3.03
elif gauss == 8.1:
self.scale_reg = 0x07
self.scale = 4.35
self.scale_reg = self.scale_reg << 5
self.setOption(self.ConfigurationRegisterB, self.scale_reg)
def setDeclination(self, degree, min = 0):
self.declinationDeg = degree
self.declinationMin = min
self.declination = (degree+min/60) * (math.pi/180)
def setOption(self, register, *function_set):
options = 0x00
for function in function_set:
options = options | function
#self.bus.write_byte(register, options)
self.bus.mem_write(addr = 0x1E, memaddr = register, data = options)
def getDeclination(self):
return (self.declinationDeg, self.declinationMin)
def getDeclinationString(self):
return str(self.declinationDeg)+"\u00b0 "+str(self.declinationMin)+"'"
# Returns heading in degrees and minutes
def getHeading(self):
(scaled_x, scaled_y, scaled_z) = self.getAxes()
headingRad = math.atan2(scaled_y, scaled_x)
headingRad += self.declination
# Correct for reversed heading
if(headingRad < 0):
headingRad += 2*math.pi
# Check for wrap and compensate
if(headingRad > 2*math.pi):
headingRad -= 2*math.pi
# Convert to degrees from radians
headingDeg = headingRad * 180/math.pi
degrees = math.floor(headingDeg)
minutes = round(((headingDeg - degrees) * 60))
return (degrees, minutes)
def getHeadingString(self):
(degrees, minutes) = self.getHeading()
return str(degrees)+"\u00b0 "+str(minutes)+"'"
def getAxes(self):
(magno_x, magno_z,magno_y) = self.bus.mem_read(data=self.AxisXDataRegisterMSB, addr = 0x30)
if (magno_x == -4096):
magno_x = None
else:
magno_x = round(magno_x * self.scale, 4)
if (magno_y == -4096):
magno_y = None
else:
magno_y = round(magno_y * self.scale, 4)
if (magno_z == -4096):
magno_z = None
else:
magno_z = round(magno_z * self.scale, 4)
return (magno_x, magno_y, magno_z)
def getxes(self):
m = self.bus.mem_read(memaddr=0x03, addr = 0x1E, data=6)
#print(m[1]))
#print(self.bus.mem_read(memaddr=0x03, addr = 0x1E, data=1))
#print(self.bus.mem_read(memaddr=0x04, addr = 0x1E, data=1))
rawDataArray=[]
dataArray = []
for item in m:
rawDataArray.append(bin(item))
#for item in m:
#dataArray.append(int(item))
for index in range(3):
resultValue = self.bin2int(rawDataArray[index*2], rawDataArray[index*2+1])
dataArray.append(resultValue)
#print(len(rawDataArray[2]), '\t\t\t', rawDataArray[2])
#print('x\t',dataArray[0]+104, 'y\t',dataArray[2]+151)
#print('y\t',dataArray[2])
#此处的104和151是根据实际测试环境添加的offset
heading = math.atan2(dataArray[2]+104,dataArray[0]+151)
heading = heading/0.0174532925
if(heading <0):
heading = heading + 360
heading= 360-heading
print(heading)
#print(self.bus.is_ready(0x1E))
#bin2int进行二进制补码转十进制操作，由于Openmv缺少移位等操作这个过程显得异常漫长。。
def bin2int(self, bin1, bin2):
dataBin1 = ''
dataBin2 = ''
dataList = []
lenBin1 = len(bin1)
lenBin2 = len(bin2)
#print('bin1:', bin1, '\n', 'bin2:', bin2 )
if lenBin1 <10:
dataBin1 = dataBin1 + (10 - lenBin1)*'0'
if lenBin2 <10:
dataBin2 = dataBin2 + (10 - lenBin2)*'0'
for i in range(2, lenBin1):
dataBin1 = dataBin1 + bin1[i]
for i in range(2, lenBin2):
dataBin2 = dataBin2 + bin2[i]
data = dataBin1 + dataBin2
if data[0] == '0':
data = '0b' + data
#print('data:', data)
output = int(data)
else:
for num in data:
if num == '0':
dataList.append('1')
else:
dataList.append('0')
data = ''.join(dataList)
#print('data:', data)
output = 0 - int('0b' + data) - 1
return output
a = i2c_hmc5883l()
a.setScale(gauss=0.88)
a.setContinuousMode()
while(1):
a.getxes()
