@@ -75,7 +75,7 @@ def update():
75
75
buff [i ] = i2c1 .readfrom_mem (LSM303AGR_MEG_ADDR , 0x68 + i , 1 )[0 ]
76
76
for i in range (3 ):
77
77
mag [i ] = b2i (buff [(i * 2 ) + 1 ], buff [(i * 2 ) + 0 ])
78
- mag [i ] = round (mag [i ] * 1.5 , 2 )
78
+ mag [i ] = round (mag [i ] * 1.5 * 0.1 , 2 )
79
79
80
80
81
81
def rotation ():
@@ -97,3 +97,122 @@ def rotation():
97
97
pitch = round (pitch , 2 )
98
98
99
99
return (roll , pitch )
100
+
101
+ calibrateMagFlag = False
102
+ mag_min = [ 99999 , 99999 , 99999 ]
103
+ mag_max = [ - 99999 , - 99999 , - 99999 ]
104
+ def heading ():
105
+ global calibrateMagFlag
106
+ if not calibrateMagFlag :
107
+ if not loadCalibrateFromSRAM ():
108
+ calibrate_compass ()
109
+ calibrateMagFlag = True
110
+
111
+ # use calibration values to shift and scale magnetometer measurements
112
+ x_mag = (0.0 + mag [0 ] - mag_min [0 ]) / (mag_max [0 ] - mag_min [0 ]) * 2 - 1
113
+ y_mag = (0.0 + mag [1 ] - mag_min [1 ]) / (mag_max [1 ] - mag_min [1 ]) * 2 - 1
114
+ z_mag = (0.0 + mag [2 ] - mag_min [2 ]) / (mag_max [2 ] - mag_min [2 ]) * 2 - 1
115
+
116
+ # Normalize acceleration measurements so they range from 0 to 1
117
+ s = math .sqrt (math .pow (acc [0 ], 2 ) + math .pow (acc [1 ], 2 ) + math .pow (acc [2 ], 2 ))
118
+ xAccelNorm = acc [0 ] / s
119
+ yAccelNorm = acc [1 ] / s
120
+ # DF("Acc norm (x, y): (%f, %f)\n", xAccelNorm, yAccelNorm)
121
+
122
+ pitch = math .asin (- xAccelNorm )
123
+ roll = math .asin (yAccelNorm / math .cos (pitch ))
124
+
125
+ # tilt compensated magnetic sensor measurements
126
+ x_mag_comp = x_mag * math .cos (pitch ) + z_mag * math .sin (pitch )
127
+ y_mag_comp = x_mag * math .sin (roll ) * math .sin (pitch ) + y_mag * math .cos (roll ) - z_mag * math .sin (roll ) * math .cos (pitch )
128
+
129
+ # arctangent of y/x converted to degrees
130
+ heading = 180 * math .atan2 (x_mag_comp , y_mag_comp ) / math .pi
131
+
132
+ heading = (- heading ) if heading <= 0 else (360 - heading )
133
+ return int (heading )
134
+
135
+
136
+ def calibrate_compass ():
137
+ global calibrateMagFlag
138
+ import display
139
+ from time import sleep
140
+
141
+ display .scroll ("TILT TO FILL SCREEN" )
142
+
143
+ buffer = bytearray (16 )
144
+
145
+ before_index_cols = - 1
146
+ before_index_rows = - 1
147
+
148
+ delay = 0
149
+ pixelShow = False
150
+ while True :
151
+ if mag [0 ] < mag_min [0 ]:
152
+ mag_min [0 ] = mag [0 ]
153
+ if mag [0 ] > mag_max [0 ]:
154
+ mag_max [0 ] = mag [0 ]
155
+
156
+ if mag [1 ] < mag_min [1 ]:
157
+ mag_min [1 ] = mag [1 ]
158
+ if mag [1 ] > mag_max [1 ]:
159
+ mag_max [1 ] = mag [1 ]
160
+
161
+ if mag [2 ] < mag_min [2 ]:
162
+ mag_min [2 ] = mag [2 ]
163
+ if mag [2 ] > mag_max [2 ]:
164
+ mag_max [2 ] = mag [2 ]
165
+
166
+ update ()
167
+ (roll , pitch ) = rotation ()
168
+
169
+ index_cols = int ((roll - - 60 ) * (15 - 0 ) / (60 - - 60 ) + 0 )
170
+ index_rows = int ((pitch - - 60 ) * (7 - 0 ) / (60 - - 60 ) + 0 )
171
+
172
+ if index_cols > 15 :
173
+ index_cols = 15
174
+ if index_rows > 7 :
175
+ index_rows = 7
176
+
177
+ if before_index_cols == - 1 or before_index_rows == - 1 :
178
+ before_index_cols = index_cols
179
+ before_index_rows = index_rows
180
+
181
+ if index_cols >= 0 and index_cols <= 15 and index_rows >= 0 and index_rows <= 7 :
182
+ if (index_cols != before_index_cols ) or (index_rows != before_index_rows ):
183
+ buffer [before_index_cols ] |= 0x80 >> before_index_rows
184
+
185
+ if pixelShow :
186
+ buffer [index_cols ] |= 0x80 >> index_rows
187
+ if buffer == b'\xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF \xFF ' :
188
+ break
189
+ else :
190
+ buffer [index_cols ] &= ~ (0x80 >> index_rows )
191
+ if delay >= 200 :
192
+ delay = 0
193
+ pixelShow = not pixelShow
194
+
195
+ before_index_cols = index_cols
196
+ before_index_rows = index_rows
197
+
198
+ display .raw (buffer )
199
+
200
+ sleep (0.05 )
201
+ delay += 50
202
+
203
+ display .scroll ("FINISH" )
204
+ saveCalibrateIntoSRAM ()
205
+ calibrateMagFlag = True
206
+
207
+ def loadCalibrateFromSRAM ():
208
+ return False
209
+
210
+ def saveCalibrateIntoSRAM ():
211
+ return True
212
+
213
+ def calCRC (data , size ):
214
+ sum = b'\0 '
215
+ for i in range (size ):
216
+ sum += data [i ]
217
+ sum = sum ^ 0xFF
218
+ return sum
0 commit comments