You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
43 lines
1.4 KiB
43 lines
1.4 KiB
import time
|
|
import math
|
|
import board
|
|
import busio
|
|
import adafruit_lsm9ds1
|
|
from glitterpos_util import map_range
|
|
|
|
i2c = busio.I2C(board.SCL, board.SDA)
|
|
compass = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
|
|
|
|
def calibrate_compass(compass):
|
|
mag_min = [1000, 1000, 1000]
|
|
mag_max = [-1000, -1000, -1000]
|
|
|
|
print("Magnetometer Calibration")
|
|
|
|
lastDisplayTime = time.monotonic()
|
|
while True:
|
|
x, y, z = compass.magnetometer
|
|
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])
|
|
|
|
# Display once every three seconds:
|
|
if (time.monotonic() - lastDisplayTime >= 3):
|
|
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)
|
|
|
|
compass_heading = (math.atan2(cal_y, cal_x) * 180) / math.pi
|
|
if compass_heading < 0:
|
|
compass_heading += 360
|
|
print("Heading: ", compass_heading)
|
|
print("----")
|
|
|
|
lastDisplayTime = time.monotonic();
|
|
|
|
calibrate_compass(compass)
|