magneticSensor.py 1.4 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455
  1. import queue
  2. import time
  3. from sensors.connection import globalArduinoSlave
  4. import logHandler
  5. conn = globalArduinoSlave()
  6. class MagneticSensor:
  7. def __init__(self, conf):
  8. self.conf = conf
  9. self.queue = queue.Queue()
  10. self.log_handler = logHandler.get_log_handler()
  11. self.gyro_x = []
  12. self.gyro_y = []
  13. self.gyro_z = []
  14. self.offset_x = 0 #
  15. self.offset_y = 0 #
  16. self.offset_z = 0 #
  17. def start(self):
  18. if not conn.isConnected():
  19. conn.open()
  20. conn.addRecvCallback(self._readCb)
  21. self.calibrate() #
  22. def _readCb(self, raw):
  23. #print("mag: ", conn.getMagneticField())
  24. #print("accel: ", conn.getAccelValues())
  25. print("gyro: ", conn.getGyroValues())
  26. def calibrate(self):
  27. # Gyroscope Calibration
  28. i = 0
  29. while i < 500:
  30. self.gyro_x.append(conn.getGyroValues()) #
  31. self.gyro_y.append(conn.getGyroValues()) #
  32. self.gyro_z.append(conn.getGyroValues()) #
  33. print("gyro_x: %d", self.gyro_x)
  34. i += 1
  35. if i == 500:
  36. self.offset_x = sum(self.gyro_x) / len(self.gyro_x)
  37. self.offset_y = sum(self.gyro_y) / len(self.gyro_y)
  38. self.offset_z = sum(self.gyro_z) / len(self.gyro_z)
  39. #pass
  40. # Accelerometer Calibration
  41. # Magnetometer Calibration
  42. def read(self):
  43. return conn.getMagneticField()
  44. def stop(self):
  45. self.log_handler.log_and_print("stop magnetic sensor")
  46. conn.close