1  # SPDX-FileCopyrightText: 2020 Melissa LeBlanc-Williams for Adafruit Industries
  2  #
  3  # SPDX-License-Identifier: MIT
  4  
  5  import threading
  6  import time
  7  import board
  8  import busio
  9  from adafruit_lsm6ds import LSM6DSOX
 10  from adafruit_lis3mdl import LIS3MDL
 11  
 12  SAMPLE_SIZE = 500
 13  
 14  
 15  class KeyListener:
 16      """Object for listening for input in a separate thread"""
 17  
 18      def __init__(self):
 19          self._input_key = None
 20          self._listener_thread = None
 21  
 22      def _key_listener(self):
 23          while True:
 24              self._input_key = input()
 25  
 26      def start(self):
 27          """Start Listening"""
 28          if self._listener_thread is None:
 29              self._listener_thread = threading.Thread(
 30                  target=self._key_listener, daemon=True
 31              )
 32          if not self._listener_thread.is_alive():
 33              self._listener_thread.start()
 34  
 35      def stop(self):
 36          """Stop Listening"""
 37          if self._listener_thread is not None and self._listener_thread.is_alive():
 38              self._listener_thread.join()
 39  
 40      @property
 41      def pressed(self):
 42          "Return whether enter was pressed since last checked" ""
 43          result = False
 44          if self._input_key is not None:
 45              self._input_key = None
 46              result = True
 47          return result
 48  
 49  
 50  def main():
 51      # pylint: disable=too-many-locals, too-many-statements
 52      i2c = busio.I2C(board.SCL, board.SDA)
 53  
 54      gyro_accel = LSM6DSOX(i2c)
 55      magnetometer = LIS3MDL(i2c)
 56      key_listener = KeyListener()
 57      key_listener.start()
 58  
 59      ############################
 60      # Magnetometer Calibration #
 61      ############################
 62  
 63      print("Magnetometer Calibration")
 64      print("Start moving the board in all directions")
 65      print("When the magnetic Hard Offset values stop")
 66      print("changing, press ENTER to go to the next step")
 67      print("Press ENTER to continue...")
 68      while not key_listener.pressed:
 69          pass
 70  
 71      mag_x, mag_y, mag_z = magnetometer.magnetic
 72      min_x = max_x = mag_x
 73      min_y = max_y = mag_y
 74      min_z = max_z = mag_z
 75  
 76      while not key_listener.pressed:
 77          mag_x, mag_y, mag_z = magnetometer.magnetic
 78  
 79          print(
 80              "Magnetometer: X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} uT".format(
 81                  mag_x, mag_y, mag_z
 82              )
 83          )
 84  
 85          min_x = min(min_x, mag_x)
 86          min_y = min(min_y, mag_y)
 87          min_z = min(min_z, mag_z)
 88  
 89          max_x = max(max_x, mag_x)
 90          max_y = max(max_y, mag_y)
 91          max_z = max(max_z, mag_z)
 92  
 93          offset_x = (max_x + min_x) / 2
 94          offset_y = (max_y + min_y) / 2
 95          offset_z = (max_z + min_z) / 2
 96  
 97          field_x = (max_x - min_x) / 2
 98          field_y = (max_y - min_y) / 2
 99          field_z = (max_z - min_z) / 2
100  
101          print(
102              "Hard Offset:  X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} uT".format(
103                  offset_x, offset_y, offset_z
104              )
105          )
106          print(
107              "Field:        X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} uT".format(
108                  field_x, field_y, field_z
109              )
110          )
111          print("")
112          time.sleep(0.01)
113  
114      mag_calibration = (offset_x, offset_y, offset_z)
115      print(
116          "Final Magnetometer Calibration: X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} uT".format(
117              offset_x, offset_y, offset_z
118          )
119      )
120  
121      #########################
122      # Gyroscope Calibration #
123      #########################
124  
125      gyro_x, gyro_y, gyro_z = gyro_accel.gyro
126      min_x = max_x = gyro_x
127      min_y = max_y = gyro_y
128      min_z = max_z = gyro_z
129  
130      print("")
131      print("")
132      print("Gyro Calibration")
133      print("Place your gyro on a FLAT stable surface.")
134      print("Press ENTER to continue...")
135      while not key_listener.pressed:
136          pass
137  
138      for _ in range(SAMPLE_SIZE):
139          gyro_x, gyro_y, gyro_z = gyro_accel.gyro
140  
141          print(
142              "Gyroscope: X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} rad/s".format(
143                  gyro_x, gyro_y, gyro_z
144              )
145          )
146  
147          min_x = min(min_x, gyro_x)
148          min_y = min(min_y, gyro_y)
149          min_z = min(min_z, gyro_z)
150  
151          max_x = max(max_x, gyro_x)
152          max_y = max(max_y, gyro_y)
153          max_z = max(max_z, gyro_z)
154  
155          offset_x = (max_x + min_x) / 2
156          offset_y = (max_y + min_y) / 2
157          offset_z = (max_z + min_z) / 2
158  
159          noise_x = max_x - min_x
160          noise_y = max_y - min_y
161          noise_z = max_z - min_z
162  
163          print(
164              "Zero Rate Offset:  X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} rad/s".format(
165                  offset_x, offset_y, offset_z
166              )
167          )
168          print(
169              "Rad/s Noise:       X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} rad/s".format(
170                  noise_x, noise_y, noise_z
171              )
172          )
173          print("")
174  
175      gyro_calibration = (offset_x, offset_y, offset_z)
176      print(
177          "Final Zero Rate Offset: X: {0:8.2f}, Y:{1:8.2f}, Z:{2:8.2f} rad/s".format(
178              offset_x, offset_y, offset_z
179          )
180      )
181      print("")
182      print("------------------------------------------------------------------------")
183      print("Final Magnetometer Calibration Values: ", mag_calibration)
184      print("Final Gyro Calibration Values: ", gyro_calibration)
185  
186  
187  if __name__ == "__main__":
188      main()