connection.py 4.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183
  1. import threading
  2. import time
  3. import serial
  4. import serial.tools.list_ports
  5. import logHandler
  6. class SerialConnection:
  7. def __init__(self):
  8. self._ser = None
  9. self.port = None
  10. self.log_handler = logHandler.get_log_handler()
  11. # try connecting to an available port
  12. def open(self, port = None, baudrate = 1000000):
  13. if port != None:
  14. self.port = port
  15. if self.port:
  16. try:
  17. self._ser = serial.Serial(self.port, baudrate)
  18. self.log_handler.log_and_print("SERIAL: connected to " + self.port)
  19. except serial.SerialException as e:
  20. self.log_handler.log_and_print("SERIAL:", e)
  21. else:
  22. for port, name, _ in serial.tools.list_ports.comports():
  23. try:
  24. self._ser = serial.Serial(port, baudrate)
  25. self.log_handler.log_and_print("SERIAL: connected to " + port + ", name: " + name)
  26. self.port = port
  27. break
  28. except serial.SerialException as e:
  29. self.log_handler.log_and_print("SERIAL:", e)
  30. if not self._ser:
  31. self.log_handler.log_and_print("SERIAL: no device available")
  32. def send(self, bytes):
  33. if self._ser:
  34. try:
  35. return self._ser.write(bytes)
  36. except serial.SerialException:
  37. self._ser = None
  38. else:
  39. time.sleep(10)
  40. self.open()
  41. return None
  42. def read(self, n=64):
  43. if self._ser:
  44. try:
  45. return self._ser.read(n)
  46. except AttributeError as e:
  47. self.log_handler.log_and_print("SERIAL:", e)
  48. return None
  49. except serial.SerialException as e:
  50. self.log_handler.log_and_print("SERIAL:", e)
  51. if self._ser.isOpen():
  52. self._ser.close()
  53. self._ser = None
  54. return None
  55. else:
  56. time.sleep(10)
  57. self.open()
  58. return None
  59. def readline(self):
  60. if self._ser:
  61. try:
  62. return self._ser.readline()
  63. except AttributeError as e:
  64. self.log_handler.log_and_print("SERIAL:", e)
  65. return None
  66. except serial.SerialException as e:
  67. self.log_handler.log_and_print("SERIAL:", e)
  68. if self._ser.isOpen():
  69. self._ser.close()
  70. self._ser = None
  71. return None
  72. else:
  73. time.sleep(10)
  74. self.open()
  75. return None
  76. def isConnected(self):
  77. return self._ser != None
  78. def close(self):
  79. if self._ser:
  80. self._ser.close()
  81. self._ser = None
  82. class ArduinoSlave(SerialConnection):
  83. def __init__(self):
  84. super().__init__()
  85. self.sensorData = [0] * 13
  86. self._recvCbs = []
  87. self._t = threading.Thread(target=self._readSensors, args=())
  88. self._t.daemon = True # thread dies when main thread (only non-daemon thread) exits.
  89. def open(self, port = None, baudrate = 1000000):
  90. super().open(port, baudrate)
  91. if not self._t.is_alive():
  92. self._t.start()
  93. def close(self):
  94. super().close()
  95. def getAcousticRTTs(self): # in microseconds
  96. return (
  97. int(self.sensorData[0]) / 16,
  98. int(self.sensorData[1]) / 16
  99. )
  100. def getMagneticField(self): # in mT
  101. return (
  102. int(self.sensorData[2]) / 1000,
  103. int(self.sensorData[3]) / 1000,
  104. int(self.sensorData[4]) / 1000
  105. )
  106. def getMagneticOffsets(self):
  107. return (
  108. int(self.sensorData[5]) / 1000,
  109. int(self.sensorData[6]) / 1000,
  110. int(self.sensorData[7]) / 1000,
  111. )
  112. def getAccelValues(self):
  113. return (
  114. int(self.sensorData[8]) / 1000,
  115. int(self.sensorData[9]) / 1000,
  116. int(self.sensorData[10]) / 1000,
  117. )
  118. def getGyroValues(self):
  119. return (
  120. int(self.sensorData[11]) / 1000, #
  121. int(self.sensorData[12]) / 1000, #
  122. int(self.sensorData[13]) / 1000 #
  123. )
  124. def getTemperature(self): # in °C
  125. return int(self.sensorData[14]) / 1000
  126. def addRecvCallback(self, cb):
  127. self._recvCbs.append(cb)
  128. def _readSensors(self):
  129. while True:
  130. data = super().readline()
  131. if data and len(data) > 2:
  132. data = str(data, encoding="ASCII", errors="ignore")
  133. vals = data[:-2].split('\t')
  134. if vals[0] == "DATA:" and len(vals) >= 13:
  135. self.sensorData = vals[1:]
  136. for cb in self._recvCbs:
  137. cb(self.sensorData)
  138. elif data.startswith("I2C ERROR"):
  139. pass
  140. else:
  141. self.log_handler.log_and_print("SERIAL: ", data[:-2])
  142. # this allows the usage of a single instance of ArduinoSlave
  143. _conn = None
  144. def globalArduinoSlave():
  145. global _conn
  146. if not _conn:
  147. _conn = ArduinoSlave()
  148. return _conn
  149. if __name__ == "__main__":
  150. arduino = ArduinoSlave()
  151. def cb(x):
  152. print(arduino.getAcousticRTTs(), arduino.getMagneticField(), arduino.getTemperature())
  153. arduino.addRecvCallback(cb)
  154. arduino.open()
  155. while True:
  156. time.sleep(1)