Skip to content

Commit 069a57e

Browse files
FoamyGuyFoamyGuy
committed
add qtpy s2 round display compass
1 parent 726d005 commit 069a57e

File tree

3 files changed

+299
-0
lines changed

3 files changed

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

‎QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/compass_rose.png

9.86 KB
Loading

0 commit comments

Comments
 (0)