import queue import time from sensors.connection import globalArduinoSlave import logHandler conn = globalArduinoSlave() class MagneticSensor: def __init__(self, conf): self.conf = conf self.queue = queue.Queue() self.log_handler = logHandler.get_log_handler() self.gyro_x = [] self.gyro_y = [] self.gyro_z = [] self.offset_x = 0 # self.offset_y = 0 # self.offset_z = 0 # def start(self): if not conn.isConnected(): conn.open() conn.addRecvCallback(self._readCb) self.calibrate() # def _readCb(self, raw): #print("mag: ", conn.getMagneticField()) #print("accel: ", conn.getAccelValues()) print("gyro: ", conn.getGyroValues()) def calibrate(self): # Gyroscope Calibration i = 0 while i < 500: self.gyro_x.append(conn.getGyroValues()) # self.gyro_y.append(conn.getGyroValues()) # self.gyro_z.append(conn.getGyroValues()) # print("gyro_x: %d", self.gyro_x) i += 1 if i == 500: self.offset_x = sum(self.gyro_x) / len(self.gyro_x) self.offset_y = sum(self.gyro_y) / len(self.gyro_y) self.offset_z = sum(self.gyro_z) / len(self.gyro_z) #pass # Accelerometer Calibration # Magnetometer Calibration def read(self): return conn.getMagneticField() def stop(self): self.log_handler.log_and_print("stop magnetic sensor") conn.close