rpi_sensor/mag3110.py at master · ruier/rpi_sensor · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
#!/usr/bin/env python
import math
import time
import os
from ctypes import *
#cdll.LoadLibrary("./bcm2835.so")

sensor = CDLL("./sensor.so")

class mag3110:

	def __init__(self):

		if ( True == os.path.exists('mag_calibration.data')):
			f_cal = file("mag_calibration.data", 'r')
			s = f_cal.readline()
			[self.x_off, self.y_off, self.z_off] = s.split()
			f_cal.close()
		else:
			self.x_off = -565
			self.y_off = 700
			self.z_off = 0

		if (0 == sensor.bcm2835_init()):
			print "bcm3835 driver init failed."
			return

	def writeRegister(self, register, value):
	    sensor.MAG3110_WRITE_REGISTER(register, value)

	def readRegister(self, register):
		return sensor.MAG3110_READ_REGISTER(register)

	def __str__(self):
		ret_str = ""
		(x, y, z) = self.getAxes()
		ret_str += "X: "+str(x) + "  "
		ret_str += "Y: "+str(y) + "  "
		ret_str += "Z: "+str(z)

		return ret_str

	def init(self):
		sensor.MAG3110_Init()
	def readRawData_x(self):
		return sensor.MAG3110_ReadRawData_x()

	def readRawData_y(self):
		return sensor.MAG3110_ReadRawData_y()

	def readRawData_z(self):
		return sensor.MAG3110_ReadRawData_z()

	def getAxes(self):
		x = self.readRawData_x()
		y = self.readRawData_y()
		z = self.readRawData_z()

		return (x, y, z)

	def readAsInt(self):
		x = self.readRawData_x() / 40
		y = self.readRawData_y() / 40
		z = self.readRawData_z() / 40

		return (x, y, z)

	def getHeading(self):
		(x, y, z) = self.getAxes()

		heading = math.atan2((y-self.y_off), (x-self.x_off)) * 180/math.pi + 180

		return heading

	def calibrate(self, x, y, z):
		max_x = max(x)
		max_y = max(y)
		max_z = max(z)

		min_x = min(x)
		min_y = min(y)
		min_z = min(z)

		self.x_off = (max_x + min_x) / 2
		self.y_off = (max_y + min_y) / 2
		self.z_off = 0 #(max_z + min_z) / 2

mag = mag3110()
mag.init()

print mag.x_off, mag.y_off, mag.z_off
while 1:
	(x, y, z) = mag.readAsInt()
	print "MAG3110:\tX.", x, "uT", "\tY.", y, "uT", "\tZ.", z, "uT"
	#print mag.readRawData_x(), mag.readRawData_y(), mag.readRawData_z()