Skip to content

Commit 67a2acb

Browse files
TheKittyTheKitty
authored
Merge pull request #2979 from FoamyGuy/qtpy_rounddisplay_compass
addding qtpy s2 round display compass
2 parents 726d005 + f32c65d commit 67a2acb

File tree

3 files changed

+298
-0
lines changed

3 files changed

+298
-0
lines changed

‎QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/calibrate.py

Lines changed: 129 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,129 @@
1+
# SPDX-FileCopyrightText: 2025 Tim Cocks for Adafruit Industries
2+
# SPDX-License-Identifier: MIT
3+
#
4+
# Adapted from Liz Clark's calibrate.py in the QualiaS3 Compass learn guide
5+
# https://learn.adafruit.com/qualia-s3-compass/code-the-compass
6+
#
7+
# Which was adapted from Gamblor21's calibrate.py in the Gamblor21_CircuitPython_AHRS library
8+
# https://github.com/gamblor21/Gamblor21_CircuitPython_AHRS/blob/master/examples/calibrate.py
9+
#
10+
# Gyro will be calibrated first, followed by magnetometer
11+
# Keep the board still for gyro, move around for magnetometer
12+
13+
import time
14+
15+
import board
16+
from adafruit_lsm6ds.lsm6dsox import LSM6DSOX
17+
import adafruit_lis3mdl
18+
19+
20+
i2c = board.STEMMA_I2C()
21+
accel_gyro = LSM6DSOX(i2c)
22+
magnetometer = adafruit_lis3mdl.LIS3MDL(i2c)
23+
MAG_MIN = [1000, 1000, 1000]
24+
MAG_MAX = [-1000, -1000, -1000]
25+
26+
def map_range(x, in_min, in_max, out_min, out_max):
27+
"""
28+
Maps a number from one range to another.
29+
:return: Returns value mapped to new range
30+
:rtype: float
31+
"""
32+
mapped = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
33+
if out_min <= out_max:
34+
return max(min(mapped, out_max), out_min)
35+
36+
return min(max(mapped, out_max), out_min)
37+
38+
def calibrate_gyro():
39+
"""
40+
Calibrates gyroscope
41+
Gyroscope values are in rads/s
42+
"""
43+
gx, gy, gz = accel_gyro.gyro
44+
min_gx = gx
45+
min_gy = gy
46+
min_gz = gz
47+
48+
max_gx = gx
49+
max_gy = gy
50+
max_gz = gz
51+
52+
mid_gx = gx
53+
mid_gy = gy
54+
mid_gz = gz
55+
56+
for _ in range(10):
57+
gx, gy, gz = accel_gyro.gyro
58+
59+
min_gx = min(min_gx, gx)
60+
min_gy = min(min_gy, gy)
61+
min_gz = min(min_gz, gz)
62+
63+
max_gx = max(max_gx, gx)
64+
max_gy = max(max_gy, gy)
65+
max_gz = max(max_gz, gz)
66+
67+
mid_gx = (max_gx + min_gx) / 2
68+
mid_gy = (max_gy + min_gy) / 2
69+
mid_gz = (max_gz + min_gz) / 2
70+
71+
print("Uncalibrated gyro: ", (gx, gy, gz))
72+
print("Calibrated gyro: ", (gx + mid_gx, gy + mid_gy, gz + mid_gz))
73+
print("Gyro calibration: ", (mid_gx, mid_gy, mid_gz))
74+
75+
time.sleep(1)
76+
mid_gx = float(f"{mid_gx:.4f}")
77+
mid_gy = float(f"{mid_gy:.4f}")
78+
mid_gz = float(f"{mid_gz:.4f}")
79+
_CAL = [mid_gx, mid_gy, mid_gz]
80+
return _CAL
81+
82+
def calibrate_mag():
83+
"""
84+
Calibrates a magnometer
85+
"""
86+
countavg = 0
87+
x, y, z = magnetometer.magnetic
88+
mag_vals = [x, y, z]
89+
for i in range(3):
90+
MAG_MIN[i] = min(MAG_MIN[i], mag_vals[i])
91+
MAG_MAX[i] = max(MAG_MAX[i], mag_vals[i])
92+
93+
for _ in range(10):
94+
x, y, z = magnetometer.magnetic
95+
mag_vals = [x, y, z]
96+
97+
for i in range(3):
98+
MAG_MIN[i] = min(MAG_MIN[i], mag_vals[i])
99+
MAG_MAX[i] = max(MAG_MAX[i], mag_vals[i])
100+
101+
countavg += 1
102+
print("Uncalibrated:", x, y, z)
103+
cal_x = map_range(x, MAG_MIN[0], MAG_MAX[0], -1, 1)
104+
cal_y = map_range(y, MAG_MIN[1], MAG_MAX[1], -1, 1)
105+
cal_z = map_range(z, MAG_MIN[2], MAG_MAX[2], -1, 1)
106+
print("Calibrate: ", cal_x, cal_y, cal_z)
107+
print("MAG_MIN =", MAG_MIN)
108+
print("MAG_MAX =", MAG_MAX)
109+
110+
time.sleep(1)
111+
return MAG_MIN, MAG_MAX
112+
113+
print("Preparing gyroscope calibration. Keep board perfectly still on flat surface.")
114+
time.sleep(5)
115+
print("Starting gyroscope calibration..")
116+
print()
117+
GYRO_CAL = calibrate_gyro()
118+
print("Gyroscope calibrated!")
119+
120+
print("Preparing magnetometer calibration. Move board around in 3D space.")
121+
time.sleep(5)
122+
print("Starting magnetometer calibration..")
123+
print()
124+
MAG_MIN, MAG_MAX = calibrate_mag()
125+
print("Magnetometer calibrated!")
126+
print()
127+
print("MAG_MIN =", MAG_MIN)
128+
print("MAG_MAX =", MAG_MAX)
129+
print("GYRO_CAL =", GYRO_CAL)

‎QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/code.py

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
1+
# SPDX-FileCopyrightText: 2025 Tim Cocks for Adafruit Industries
2+
# SPDX-License-Identifier: MIT
3+
#
4+
# Adapted from QualiaS3 Compass Learn Guide by Liz Clark (Adafruit Industries)
5+
# https://learn.adafruit.com/qualia-s3-compass/
6+
7+
import time
8+
from math import atan2, degrees, radians
9+
import adafruit_lis3mdl
10+
import board
11+
from adafruit_lsm6ds.lsm6dsox import LSM6DSOX
12+
from gamblor21_ahrs import mahony
13+
import bitmaptools
14+
from adafruit_gc9a01a import GC9A01A
15+
import adafruit_imageload
16+
import displayio
17+
from fourwire import FourWire
18+
19+
20+
# change these values to your calibration values
21+
MAG_MIN = [-75.1973, -22.5665, -34.5221]
22+
MAG_MAX = [-1.2131, 68.1379, 20.8126]
23+
GYRO_CAL = [-0.0038, -0.0026, -0.0011]
24+
25+
26+
# use filter for more accurate, but slightly slower readings
27+
# otherwise just reads from magnetometer
28+
ahrs = True
29+
center_x, center_y = 120, 120
30+
31+
i2c = board.STEMMA_I2C()
32+
accel_gyro = LSM6DSOX(i2c)
33+
magnetometer = adafruit_lis3mdl.LIS3MDL(i2c)
34+
# Create the AHRS filter
35+
ahrs_filter = mahony.Mahony(50, 5, 100)
36+
37+
# Variable to account for the offset between raw heading values
38+
# and the orientation of the display.
39+
offset_angle = 90
40+
41+
42+
def map_range(x, in_min, in_max, out_min, out_max):
43+
"""
44+
Maps a value from one range to another.
45+
46+
:param x: The value to map
47+
:param in_min: The minimum value of the input range
48+
:param in_max: The maximum value of the input range
49+
:param out_min: The minimum value of the output range
50+
:param out_max: The maximum value of the output range
51+
52+
:return: The value mapped to the output range
53+
"""
54+
mapped = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
55+
if out_min <= out_max:
56+
return max(min(mapped, out_max), out_min)
57+
58+
return min(max(mapped, out_max), out_min)
59+
60+
last_heading = offset_angle
61+
heading = offset_angle
62+
last_update = time.monotonic() # last time we printed the yaw/pitch/roll values
63+
timestamp = time.monotonic_ns() # used to tune the frequency to approx 100 Hz
64+
65+
# Display Setup
66+
spi = board.SPI()
67+
tft_cs = board.TX
68+
tft_dc = board.RX
69+
displayio.release_displays()
70+
display_bus = FourWire(spi, command=tft_dc, chip_select=tft_cs, reset=None)
71+
display = GC9A01A(display_bus, width=240, height=240)
72+
display.rotation = 90
73+
74+
# group to hold all of our visual elements
75+
main_group = displayio.Group()
76+
display.root_group = main_group
77+
78+
# load the compass rose background image
79+
rose_bmp, rose_palette = adafruit_imageload.load("compass_rose.png")
80+
rose_tg = displayio.TileGrid(bitmap=rose_bmp, pixel_shader=rose_palette)
81+
82+
# bitmap for the pointer needle
83+
pointer = displayio.Bitmap(5, 90, 2)
84+
85+
# bitmap for erasing the pointer needle
86+
pointer_eraser = displayio.Bitmap(5, 90, 1)
87+
88+
# pointer needle palette, red foreground, transparent background
89+
pointer_palette = displayio.Palette(2)
90+
pointer_palette[0] = 0x000000
91+
pointer_palette[1] = 0xFF0000
92+
pointer_palette.make_transparent(0)
93+
pointer.fill(1)
94+
95+
# display sized bitmap to paste the rotated needle into
96+
rotated_pointer = displayio.Bitmap(240, 240, 2)
97+
98+
# tilegrid for the rotated pointer needle
99+
pointer_tg = displayio.TileGrid(rotated_pointer, pixel_shader=pointer_palette)
100+
101+
# add rose then pointer to the displaying group
102+
main_group.append(rose_tg)
103+
main_group.append(pointer_tg)
104+
105+
while True:
106+
# if it's time to take a compass reading from the mag/gyro
107+
if (time.monotonic_ns() - timestamp) > 6500000:
108+
# read magnetic data
109+
mx, my, mz = magnetometer.magnetic
110+
111+
# map it to the calibrated values
112+
cal_x = map_range(mx, MAG_MIN[0], MAG_MAX[0], -1, 1)
113+
cal_y = map_range(my, MAG_MIN[1], MAG_MAX[1], -1, 1)
114+
cal_z = map_range(mz, MAG_MIN[2], MAG_MAX[2], -1, 1)
115+
116+
# if using ahrs filter
117+
if ahrs:
118+
# get accel/gyro data
119+
ax, ay, az, gx, gy, gz = accel_gyro.acceleration + accel_gyro.gyro
120+
121+
# apply callibration offset
122+
gx += GYRO_CAL[0]
123+
gy += GYRO_CAL[1]
124+
gz += GYRO_CAL[2]
125+
126+
# update filter
127+
ahrs_filter.update(gx, gy, -gz, ax, ay, az, cal_x, -cal_y, cal_z)
128+
129+
# get yaw
130+
yaw_degree = ahrs_filter.yaw
131+
132+
# convert radians to degrees
133+
heading = degrees(yaw_degree)
134+
135+
else: # not using ahrs filter
136+
# calculate heading from calibrated mag data
137+
# and convert from radians to degrees
138+
heading = degrees(atan2(cal_y, cal_x))
139+
140+
# save time to compare next iteration
141+
timestamp = time.monotonic_ns()
142+
143+
# if it's time to update the display
144+
if time.monotonic() > last_update + 0.2:
145+
# wrap negative heading values
146+
if heading < 0:
147+
heading += 360
148+
149+
# if the heading is sufficiently different from previous heading
150+
if abs(last_heading - heading) >= 2:
151+
#print(heading)
152+
153+
# erase the previous pointer needle
154+
bitmaptools.rotozoom(rotated_pointer, pointer_eraser,
155+
ox=120, oy=120,
156+
px=pointer.width // 2, py=pointer.height,
157+
angle=radians(last_heading + offset_angle))
158+
159+
# draw the new pointer needle
160+
bitmaptools.rotozoom(rotated_pointer, pointer,
161+
ox=120, oy=120,
162+
px=pointer.width // 2, py=pointer.height,
163+
angle=radians(heading + offset_angle))
164+
165+
# set the previous heading to compare next iteration
166+
last_heading = heading
167+
168+
# set the last update time to compare next iteration
169+
last_update = time.monotonic()

‎QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/compass_rose.png

9.86 KB
Loading

0 commit comments

Comments
 (0)