Skip to content

Commit 14d85aa

Browse files
committed
Add heading() and calibrate_compass()
1 parent 01e87a3 commit 14d85aa

File tree

1 file changed

+120
-1
lines changed
  • ports/esp32/boards/KidBright32/modules

1 file changed

+120
-1
lines changed

ports/esp32/boards/KidBright32/modules/imu.py

Lines changed: 120 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,7 +75,7 @@ def update():
7575
buff[i] = i2c1.readfrom_mem(LSM303AGR_MEG_ADDR, 0x68 + i, 1)[0]
7676
for i in range(3):
7777
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)
7979

8080

8181
def rotation():
@@ -97,3 +97,122 @@ def rotation():
9797
pitch = round(pitch, 2)
9898

9999
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

Comments
 (0)