|
|
- 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)
|