forked from martinr1000/AliensMotionTracker
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcalibration.py
146 lines (113 loc) · 4.17 KB
/
calibration.py
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
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
smbusAvailable = True
try:
import smbus
except:
smbusAvailable = False
import time, math, os, random
class Calibration:
xOffset = 0
yOffset = 0
def __init__(self, scope, resources):
current_file_path = __file__
self.offsetFilePath = os.path.dirname(__file__) + "//offsets.txt"
#initialise smbus and read existing values
self.smbusAvailable = smbusAvailable
if self.smbusAvailable:
#setup the digital compass
try:
self.bus=smbus.SMBus(1)
self.address=0x1e
except:
self.smbusAvailable = False
return
self.xOffset = 0
self.yOffset = 0
self.readOffsetsFromFile()
def read_byte(self, adr):
return self.bus.read_byte_data(address, adr)
def read_word(self, adr):
high = self.bus.read_byte_data(self.address, adr)
low = self.bus.read_byte_data(self.address, adr+1)
val = (high << 8) + low
return val
def read_word_2c(self, adr):
val = self.read_word(adr)
if (val >= 0x8000):
return -((65535 - val) + 1)
else:
return val
def write_byte(self, adr, value):
self.bus.write_byte_data(self.address, adr, value)
#define a method to read the existing x and y offset values from file
def readOffsetsFromFile(self):
try:
target = open(self.offsetFilePath, 'r')
except:
self.xOffset = 0
self.yOffset = 0
return
contents = target.readlines()
target.close()
try:
self.xOffset = float(contents[0].rsplit('=', 1)[-1])
self.yOffset = float(contents[1].rsplit('=', 1)[-1])
except:
self.xOffset = 0
self.yOffset = 0
#define a method to write the new x and y offset values back to file
def writeOffsetsToFile(self):
try:
target = open(self.offsetFilePath, 'w')
except:
return
xOff = self.xOffset
yOff = self.yOffset
xOffsetString = "xOffset=" + str(xOff) + "\n"
yOffsetString = "yOffset=" + str(yOff)
target.write(xOffsetString)
target.write(yOffsetString)
target.close()
#init calibration
def initCalibration(self):
if self.smbusAvailable:
self.write_byte(0, 0b01110000) # Set to 8 samples @ 15Hz
self.write_byte(1, 0b00100000) # 1.3 gain LSb / Gauss 1090 (default)
self.write_byte(2, 0b00000000) # Continuous sampling
#scale = 0.92
self.minx = 0
self.maxx = 0
self.miny = 0
self.maxy = 0
#calibration method
def calibrate(self, index):
#read calibration steps
if index<500:
if self.smbusAvailable:
x_out = self.read_word_2c(3)
y_out = self.read_word_2c(7)
z_out = self.read_word_2c(5)
else:
#compass is unavailable so provide random values to test
#calibration graphics
angle = random.randint(0, 359)
dist = random.randint(100,150)
xmultiplier = math.sin(math.radians(angle))
x_out = dist*xmultiplier
ymultiplier = math.cos(math.radians(angle))
y_out = dist*ymultiplier
if x_out < self.minx:
self.minx=x_out
if y_out < self.miny:
self.miny=y_out
if x_out > self.maxx:
self.maxx=x_out
if y_out > self.maxy:
self.maxy=y_out
return x_out,y_out
else:
#update the x and y offset values
self.xOffset = (self.maxx + self.minx) / 2
self.yOffset = (self.maxy + self.miny) / 2
#update the file offset values
self.writeOffsetsToFile()
return self.xOffset,self.yOffset