diff --git a/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/calibrate.py b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/calibrate.py new file mode 100644 index 000000000..8629e6216 --- /dev/null +++ b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/calibrate.py @@ -0,0 +1,129 @@ +# SPDX-FileCopyrightText: 2025 Tim Cocks for Adafruit Industries +# SPDX-License-Identifier: MIT +# +# Adapted from Liz Clark's calibrate.py in the QualiaS3 Compass learn guide +# https://learn.adafruit.com/qualia-s3-compass/code-the-compass +# +# Which was adapted from Gamblor21's calibrate.py in the Gamblor21_CircuitPython_AHRS library +# https://github.com/gamblor21/Gamblor21_CircuitPython_AHRS/blob/master/examples/calibrate.py +# +# Gyro will be calibrated first, followed by magnetometer +# Keep the board still for gyro, move around for magnetometer + +import time + +import board +from adafruit_lsm6ds.lsm6dsox import LSM6DSOX +import adafruit_lis3mdl + + +i2c = board.STEMMA_I2C() +accel_gyro = LSM6DSOX(i2c) +magnetometer = adafruit_lis3mdl.LIS3MDL(i2c) +MAG_MIN = [1000, 1000, 1000] +MAG_MAX = [-1000, -1000, -1000] + +def map_range(x, in_min, in_max, out_min, out_max): + """ + Maps a number from one range to another. + :return: Returns value mapped to new range + :rtype: float + """ + mapped = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + if out_min <= out_max: + return max(min(mapped, out_max), out_min) + + return min(max(mapped, out_max), out_min) + +def calibrate_gyro(): + """ + Calibrates gyroscope + Gyroscope values are in rads/s + """ + gx, gy, gz = accel_gyro.gyro + min_gx = gx + min_gy = gy + min_gz = gz + + max_gx = gx + max_gy = gy + max_gz = gz + + mid_gx = gx + mid_gy = gy + mid_gz = gz + + for _ in range(10): + gx, gy, gz = accel_gyro.gyro + + min_gx = min(min_gx, gx) + min_gy = min(min_gy, gy) + min_gz = min(min_gz, gz) + + max_gx = max(max_gx, gx) + max_gy = max(max_gy, gy) + max_gz = max(max_gz, gz) + + mid_gx = (max_gx + min_gx) / 2 + mid_gy = (max_gy + min_gy) / 2 + mid_gz = (max_gz + min_gz) / 2 + + print("Uncalibrated gyro: ", (gx, gy, gz)) + print("Calibrated gyro: ", (gx + mid_gx, gy + mid_gy, gz + mid_gz)) + print("Gyro calibration: ", (mid_gx, mid_gy, mid_gz)) + + time.sleep(1) + mid_gx = float(f"{mid_gx:.4f}") + mid_gy = float(f"{mid_gy:.4f}") + mid_gz = float(f"{mid_gz:.4f}") + _CAL = [mid_gx, mid_gy, mid_gz] + return _CAL + +def calibrate_mag(): + """ + Calibrates a magnometer + """ + countavg = 0 + x, y, z = magnetometer.magnetic + mag_vals = [x, y, z] + for i in range(3): + MAG_MIN[i] = min(MAG_MIN[i], mag_vals[i]) + MAG_MAX[i] = max(MAG_MAX[i], mag_vals[i]) + + for _ in range(10): + x, y, z = magnetometer.magnetic + mag_vals = [x, y, z] + + for i in range(3): + MAG_MIN[i] = min(MAG_MIN[i], mag_vals[i]) + MAG_MAX[i] = max(MAG_MAX[i], mag_vals[i]) + + countavg += 1 + print("Uncalibrated:", x, y, z) + cal_x = map_range(x, MAG_MIN[0], MAG_MAX[0], -1, 1) + cal_y = map_range(y, MAG_MIN[1], MAG_MAX[1], -1, 1) + cal_z = map_range(z, MAG_MIN[2], MAG_MAX[2], -1, 1) + print("Calibrate: ", cal_x, cal_y, cal_z) + print("MAG_MIN =", MAG_MIN) + print("MAG_MAX =", MAG_MAX) + + time.sleep(1) + return MAG_MIN, MAG_MAX + +print("Preparing gyroscope calibration. Keep board perfectly still on flat surface.") +time.sleep(5) +print("Starting gyroscope calibration..") +print() +GYRO_CAL = calibrate_gyro() +print("Gyroscope calibrated!") + +print("Preparing magnetometer calibration. Move board around in 3D space.") +time.sleep(5) +print("Starting magnetometer calibration..") +print() +MAG_MIN, MAG_MAX = calibrate_mag() +print("Magnetometer calibrated!") +print() +print("MAG_MIN =", MAG_MIN) +print("MAG_MAX =", MAG_MAX) +print("GYRO_CAL =", GYRO_CAL) diff --git a/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/code.py b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/code.py new file mode 100644 index 000000000..50068826c --- /dev/null +++ b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/code.py @@ -0,0 +1,169 @@ +# SPDX-FileCopyrightText: 2025 Tim Cocks for Adafruit Industries +# SPDX-License-Identifier: MIT +# +# Adapted from QualiaS3 Compass Learn Guide by Liz Clark (Adafruit Industries) +# https://learn.adafruit.com/qualia-s3-compass/ + +import time +from math import atan2, degrees, radians +import adafruit_lis3mdl +import board +from adafruit_lsm6ds.lsm6dsox import LSM6DSOX +from gamblor21_ahrs import mahony +import bitmaptools +from adafruit_gc9a01a import GC9A01A +import adafruit_imageload +import displayio +from fourwire import FourWire + + +# change these values to your calibration values +MAG_MIN = [-75.1973, -22.5665, -34.5221] +MAG_MAX = [-1.2131, 68.1379, 20.8126] +GYRO_CAL = [-0.0038, -0.0026, -0.0011] + + +# use filter for more accurate, but slightly slower readings +# otherwise just reads from magnetometer +ahrs = True +center_x, center_y = 120, 120 + +i2c = board.STEMMA_I2C() +accel_gyro = LSM6DSOX(i2c) +magnetometer = adafruit_lis3mdl.LIS3MDL(i2c) +# Create the AHRS filter +ahrs_filter = mahony.Mahony(50, 5, 100) + +# Variable to account for the offset between raw heading values +# and the orientation of the display. +offset_angle = 90 + + +def map_range(x, in_min, in_max, out_min, out_max): + """ + Maps a value from one range to another. + + :param x: The value to map + :param in_min: The minimum value of the input range + :param in_max: The maximum value of the input range + :param out_min: The minimum value of the output range + :param out_max: The maximum value of the output range + + :return: The value mapped to the output range + """ + mapped = (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + if out_min <= out_max: + return max(min(mapped, out_max), out_min) + + return min(max(mapped, out_max), out_min) + +last_heading = offset_angle +heading = offset_angle +last_update = time.monotonic() # last time we printed the yaw/pitch/roll values +timestamp = time.monotonic_ns() # used to tune the frequency to approx 100 Hz + +# Display Setup +spi = board.SPI() +tft_cs = board.TX +tft_dc = board.RX +displayio.release_displays() +display_bus = FourWire(spi, command=tft_dc, chip_select=tft_cs, reset=None) +display = GC9A01A(display_bus, width=240, height=240) +display.rotation = 90 + +# group to hold all of our visual elements +main_group = displayio.Group() +display.root_group = main_group + +# load the compass rose background image +rose_bmp, rose_palette = adafruit_imageload.load("compass_rose.png") +rose_tg = displayio.TileGrid(bitmap=rose_bmp, pixel_shader=rose_palette) + +# bitmap for the pointer needle +pointer = displayio.Bitmap(5, 90, 2) + +# bitmap for erasing the pointer needle +pointer_eraser = displayio.Bitmap(5, 90, 1) + +# pointer needle palette, red foreground, transparent background +pointer_palette = displayio.Palette(2) +pointer_palette[0] = 0x000000 +pointer_palette[1] = 0xFF0000 +pointer_palette.make_transparent(0) +pointer.fill(1) + +# display sized bitmap to paste the rotated needle into +rotated_pointer = displayio.Bitmap(240, 240, 2) + +# tilegrid for the rotated pointer needle +pointer_tg = displayio.TileGrid(rotated_pointer, pixel_shader=pointer_palette) + +# add rose then pointer to the displaying group +main_group.append(rose_tg) +main_group.append(pointer_tg) + +while True: + # if it's time to take a compass reading from the mag/gyro + if (time.monotonic_ns() - timestamp) > 6500000: + # read magnetic data + mx, my, mz = magnetometer.magnetic + + # map it to the calibrated values + cal_x = map_range(mx, MAG_MIN[0], MAG_MAX[0], -1, 1) + cal_y = map_range(my, MAG_MIN[1], MAG_MAX[1], -1, 1) + cal_z = map_range(mz, MAG_MIN[2], MAG_MAX[2], -1, 1) + + # if using ahrs filter + if ahrs: + # get accel/gyro data + ax, ay, az, gx, gy, gz = accel_gyro.acceleration + accel_gyro.gyro + + # apply callibration offset + gx += GYRO_CAL[0] + gy += GYRO_CAL[1] + gz += GYRO_CAL[2] + + # update filter + ahrs_filter.update(gx, gy, -gz, ax, ay, az, cal_x, -cal_y, cal_z) + + # get yaw + yaw_degree = ahrs_filter.yaw + + # convert radians to degrees + heading = degrees(yaw_degree) + + else: # not using ahrs filter + # calculate heading from calibrated mag data + # and convert from radians to degrees + heading = degrees(atan2(cal_y, cal_x)) + + # save time to compare next iteration + timestamp = time.monotonic_ns() + + # if it's time to update the display + if time.monotonic() > last_update + 0.2: + # wrap negative heading values + if heading < 0: + heading += 360 + + # if the heading is sufficiently different from previous heading + if abs(last_heading - heading) >= 2: + #print(heading) + + # erase the previous pointer needle + bitmaptools.rotozoom(rotated_pointer, pointer_eraser, + ox=120, oy=120, + px=pointer.width // 2, py=pointer.height, + angle=radians(last_heading + offset_angle)) + + # draw the new pointer needle + bitmaptools.rotozoom(rotated_pointer, pointer, + ox=120, oy=120, + px=pointer.width // 2, py=pointer.height, + angle=radians(heading + offset_angle)) + + # set the previous heading to compare next iteration + last_heading = heading + + # set the last update time to compare next iteration + last_update = time.monotonic() diff --git a/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/compass_rose.png b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/compass_rose.png new file mode 100644 index 000000000..41c782a04 Binary files /dev/null and b/QT_Py/QT_Py_ESP32_S2_RoundDisplay_Compass/compass_rose.png differ