diff --git a/Qualia/Qualia_S3_Compass/Roboto-Regular-47.pcf b/Qualia/Qualia_S3_Compass/Roboto-Regular-47.pcf new file mode 100644 index 000000000..ec0386846 Binary files /dev/null and b/Qualia/Qualia_S3_Compass/Roboto-Regular-47.pcf differ diff --git a/Qualia/Qualia_S3_Compass/ahrs_calibration/calibrate.py b/Qualia/Qualia_S3_Compass/ahrs_calibration/calibrate.py new file mode 100644 index 000000000..f9b933fed --- /dev/null +++ b/Qualia/Qualia_S3_Compass/ahrs_calibration/calibrate.py @@ -0,0 +1,125 @@ +# SPDX-FileCopyrightText: 2024 Liz Clark for Adafruit Industries +# SPDX-License-Identifier: MIT +# +# 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 +from adafruit_lsm6ds.lsm6dsox import LSM6DSOX +import adafruit_lis3mdl +from adafruit_qualia.graphics import Graphics, Displays + +graphics = Graphics(Displays.ROUND21, default_bg=None, auto_refresh=True) +i2c = graphics.i2c_bus +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("Calibrated: ", 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/Qualia/Qualia_S3_Compass/code.py b/Qualia/Qualia_S3_Compass/code.py new file mode 100644 index 000000000..52c0e70f5 --- /dev/null +++ b/Qualia/Qualia_S3_Compass/code.py @@ -0,0 +1,196 @@ +# SPDX-FileCopyrightText: 2024 Liz Clark for Adafruit Industries +# SPDX-License-Identifier: MIT +# Written by Liz Clark (Adafruit Industries) +# with OpenAI ChatGPT v4 Jan 10, 2024 build +# https://help.openai.com/en/articles/6825453-chatgpt-release-notes + +# https://chat.openai.com/share/19de8f24-3191-43b8-a9c9-95f1b8000e80 + +import time +from math import atan2, degrees, cos, sin, radians +import adafruit_lis3mdl +import vectorio +import displayio +from adafruit_display_text import bitmap_label +from adafruit_bitmap_font import bitmap_font +from adafruit_qualia.graphics import Graphics, Displays +from adafruit_lsm6ds.lsm6dsox import LSM6DSOX +from gamblor21_ahrs import mahony +import bitmaptools +from jpegio import JpegDecoder +# change these values to your calibration values +MAG_MIN = [-11.5902, -47.1353, -28.7635] +MAG_MAX = [79.7866, 48.0854, 63.461] +GYRO_CAL = [-7.3934, -0.000100605, 2.7703] +# use filter for more accurate, but slightly slower readings +# otherwise just reads from magnetometer +ahrs = True +center_x, center_y = 240, 240 + +graphics = Graphics(Displays.ROUND21, default_bg=None, auto_refresh=False) + +i2c = graphics.i2c_bus +accel_gyro = LSM6DSOX(i2c) +magnetometer = adafruit_lis3mdl.LIS3MDL(i2c) +# Create the AHRS filter +ahrs_filter = mahony.Mahony(50, 5, 100) + +group = displayio.Group() +# palette for vectorio graphics +pointer_pal = displayio.Palette(5) +pointer_pal[0] = 0xFFFF00 +pointer_pal[1] = 0x000000 +pointer_pal[2] = 0xFFFFFF +pointer_pal[3] = 0xFF0000 +pointer_pal[4] = 0x0000FF +pointer_pal.make_transparent(0) +# compass image is a jpeg +decoder = JpegDecoder() +width, height = decoder.open("/compass.jpg") +bitmap_compass = displayio.Bitmap(width, height, 20) +palette_compass = displayio.ColorConverter(input_colorspace = displayio.Colorspace.RGB565_SWAPPED) +decoder.decode(bitmap_compass) +# blank bitmap for rotozoom +compass_blank = displayio.Bitmap(width, height, 1) +# carrier bitmap for compass for rotozoom +compass_scribble = displayio.Bitmap(width, height, 20) +tile_grid = displayio.TileGrid(compass_scribble, pixel_shader=palette_compass) +# only tilegrid is added to group +group.append(tile_grid) + +radius = center_x +angle = 225 +rad_angle = radians(angle) + +# place small circle to denote heading direction from 9-DoF relative to display +circle_radius = 5 +header_angle = radians(135) +edge_x = center_x + radius * cos(header_angle) +edge_y = center_y + radius * sin(header_angle) +adjusted_x = edge_x - circle_radius * cos(header_angle) +adjusted_y = edge_y - circle_radius * sin(header_angle) +header = vectorio.Circle(pixel_shader=pointer_pal, color_index = 2, + radius=circle_radius, x=int(adjusted_x), y=int(adjusted_y)) + +center = vectorio.Circle(pixel_shader=pointer_pal, color_index = 2, radius=50, x=240, y=240) + +font_file = "/Roboto-Regular-47.pcf" +font = bitmap_font.load_font(font_file) + +direction_text = bitmap_label.Label(font, text="000", color=None) +direction_text.x = center.x - 40 +direction_text.y = center.y +direction_bitmap = direction_text.bitmap + +direction_blank = displayio.Bitmap(direction_text.bounding_box[2], + direction_text.bounding_box[2], 1) +direction_scribble = displayio.Bitmap(direction_text.bounding_box[2], + direction_text.bounding_box[2], len(pointer_pal)) +direction_grid = displayio.TileGrid(direction_scribble, pixel_shader=pointer_pal, x=200, y=200) + +def create_line_of_squares(l, n, color): + squares = [] + square_size = l // n + for i in range(n): + x = center_x + i * square_size + y = center_y - square_size // 2 + square_points = [(0, 0), (square_size, 0), + (square_size, square_size), (0, square_size)] + square = vectorio.Polygon(pixel_shader=pointer_pal, + color_index=color, points=square_points, x=x, y=y) + group.append(square) + squares.append(square) + return squares + +def update_line_of_squares(squares, h, l): + r = radians(-h) + n = len(squares) + square_size = l // n + x = center_x - (square_size - 2) + y = center_y - (square_size - 2) + for i, square in enumerate(squares): + offset_x = x + i * square_size - x + offset_y = -square_size // 2 + rotated_x = offset_x * cos(r) - offset_y * sin(r) + rotated_y = offset_x * sin(r) + offset_y * cos(r) + square.x = int(x + rotated_x) + square.y = int(y + rotated_y) + +length = 200 # Length of the lines +num_squares = 20 # Number of squares for each line + +vertical_squares = create_line_of_squares(length, num_squares, 3) +update_line_of_squares(vertical_squares, angle, length) + +def map_range(x, in_min, in_max, out_min, out_max): + 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) + +group.append(center) +group.append(header) +group.append(direction_text) +group.append(direction_grid) + +graphics.display.root_group = group + +last_heading = angle +heading = 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 +spin_rose = True + +graphics.display.refresh() + +while True: + if graphics.touch.touched: + spin_rose = not spin_rose + # reset last_heading to trigger an update + last_heading = heading + 5 + if spin_rose: + direction_text.color = None + update_line_of_squares(vertical_squares, angle, length) + else: + bitmaptools.rotozoom(compass_scribble, bitmap_compass, angle = radians(0)) + direction_text.color = pointer_pal[1] + graphics.display.refresh() + # touch debounce delay + time.sleep(0.2) + if (time.monotonic_ns() - timestamp) > 6500000: + mx, my, mz = magnetometer.magnetic + 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 ahrs: + ax, ay, az, gx, gy, gz = accel_gyro.acceleration + accel_gyro.gyro + gx += GYRO_CAL[0] + gy += GYRO_CAL[1] + gz += GYRO_CAL[2] + ahrs_filter.update(gx, gy, -gz, ax, ay, az, cal_x, -cal_y, cal_z) + yaw_degree = ahrs_filter.yaw + heading = degrees(yaw_degree) + else: + heading = degrees(atan2(cal_y, cal_x)) + timestamp = time.monotonic_ns() + if time.monotonic() > last_update + 0.2: + if heading < 0: + heading += 360 + if abs(last_heading - heading) >= 2: + direction_text.text = str(int(heading)) + if spin_rose: + direction_bitmap = direction_text.bitmap + + bitmaptools.rotozoom(compass_scribble, bitmap_compass, + angle = radians(-heading+angle)) + bitmaptools.rotozoom(direction_scribble, direction_bitmap, angle = rad_angle) + graphics.display.refresh() + bitmaptools.rotozoom(direction_scribble, direction_blank, angle = rad_angle) + bitmaptools.rotozoom(compass_scribble, compass_blank, + angle = radians(-heading+angle)) + else: + update_line_of_squares(vertical_squares, -heading + 90, length) + graphics.display.refresh() + last_heading = heading + last_update = time.monotonic() diff --git a/Qualia/Qualia_S3_Compass/compass.jpg b/Qualia/Qualia_S3_Compass/compass.jpg new file mode 100644 index 000000000..04c2d4f26 Binary files /dev/null and b/Qualia/Qualia_S3_Compass/compass.jpg differ