No Description

calibrate.py 1.4KB

12345678910111213141516171819202122232425262728293031323334353637383940414243
  1. import time
  2. import math
  3. import board
  4. import busio
  5. import adafruit_lsm9ds1
  6. from glitterpos_util import map_range
  7. i2c = busio.I2C(board.SCL, board.SDA)
  8. compass = adafruit_lsm9ds1.LSM9DS1_I2C(i2c)
  9. def calibrate_compass(compass):
  10. mag_min = [1000, 1000, 1000]
  11. mag_max = [-1000, -1000, -1000]
  12. print("Magnetometer Calibration")
  13. lastDisplayTime = time.monotonic()
  14. while True:
  15. x, y, z = compass.magnetometer
  16. mag_vals = [x, y, z]
  17. for i in range(3):
  18. mag_min[i] = min(mag_min[i], mag_vals[i])
  19. mag_max[i] = max(mag_max[i], mag_vals[i])
  20. # Display once every three seconds:
  21. if (time.monotonic() - lastDisplayTime >= 3):
  22. print("Uncalibrated:", x, y, z)
  23. cal_x = map_range(x, mag_min[0], mag_max[0], -1, 1)
  24. cal_y = map_range(y, mag_min[1], mag_max[1], -1, 1)
  25. cal_z = map_range(z, mag_min[2], mag_max[2], -1, 1)
  26. print("Calibrated: ", cal_x, cal_y, cal_z)
  27. print("MAG_MIN =", mag_min)
  28. print("MAG_MAX =", mag_max)
  29. compass_heading = (math.atan2(cal_y, cal_x) * 180) / math.pi
  30. if compass_heading < 0:
  31. compass_heading += 360
  32. print("Heading: ", compass_heading)
  33. print("----")
  34. lastDisplayTime = time.monotonic();
  35. calibrate_compass(compass)