|
|
@@ -0,0 +1,170 @@
|
|
|
+import serial
|
|
|
+import time
|
|
|
+import struct
|
|
|
+
|
|
|
+usleep = lambda x: time.sleep(x/1000000.0)
|
|
|
+
|
|
|
+SET = 0x20
|
|
|
+GET = 0x00
|
|
|
+
|
|
|
+MODULE_ADDR = 0x05
|
|
|
+
|
|
|
+
|
|
|
+CMD_VERSION = 0x00
|
|
|
+CMD_ECHO = 0x01
|
|
|
+CMD_PARAM = 0x04
|
|
|
+
|
|
|
+CMD_STOP = 0x08
|
|
|
+CMD_START = 0x0C
|
|
|
+CMD_SAMPLERATE = 0x10
|
|
|
+CMD_LOWPASSFIL = 0x14
|
|
|
+CMD_RANGE = 0x18
|
|
|
+CMD_SENSOR_CNT = 0x1C
|
|
|
+CMD_SLEEP = 0x20
|
|
|
+CMD_WAKEUP = 0x24
|
|
|
+CMD_CALIBRATE = 0x28
|
|
|
+CMD_RESET = 0x2C
|
|
|
+CMD_STORE = 0x30
|
|
|
+CMD_DATA = 0x34
|
|
|
+
|
|
|
+crc8x_table = [
|
|
|
+ 0x00,0x07,0x0E,0x09,0x1C,0x1B,0x12,0x15,0x38,0x3F,0x36,0x31,0x24,0x23,0x2A,0x2D,
|
|
|
+ 0x70,0x77,0x7E,0x79,0x6C,0x6B,0x62,0x65,0x48,0x4F,0x46,0x41,0x54,0x53,0x5A,0x5D,
|
|
|
+ 0xE0,0xE7,0xEE,0xE9,0xFC,0xFB,0xF2,0xF5,0xD8,0xDF,0xD6,0xD1,0xC4,0xC3,0xCA,0xCD,
|
|
|
+ 0x90,0x97,0x9E,0x99,0x8C,0x8B,0x82,0x85,0xA8,0xAF,0xA6,0xA1,0xB4,0xB3,0xBA,0xBD,
|
|
|
+ 0xC7,0xC0,0xC9,0xCE,0xDB,0xDC,0xD5,0xD2,0xFF,0xF8,0xF1,0xF6,0xE3,0xE4,0xED,0xEA,
|
|
|
+ 0xB7,0xB0,0xB9,0xBE,0xAB,0xAC,0xA5,0xA2,0x8F,0x88,0x81,0x86,0x93,0x94,0x9D,0x9A,
|
|
|
+ 0x27,0x20,0x29,0x2E,0x3B,0x3C,0x35,0x32,0x1F,0x18,0x11,0x16,0x03,0x04,0x0D,0x0A,
|
|
|
+ 0x57,0x50,0x59,0x5E,0x4B,0x4C,0x45,0x42,0x6F,0x68,0x61,0x66,0x73,0x74,0x7D,0x7A,
|
|
|
+ 0x89,0x8E,0x87,0x80,0x95,0x92,0x9B,0x9C,0xB1,0xB6,0xBF,0xB8,0xAD,0xAA,0xA3,0xA4,
|
|
|
+ 0xF9,0xFE,0xF7,0xF0,0xE5,0xE2,0xEB,0xEC,0xC1,0xC6,0xCF,0xC8,0xDD,0xDA,0xD3,0xD4,
|
|
|
+ 0x69,0x6E,0x67,0x60,0x75,0x72,0x7B,0x7C,0x51,0x56,0x5F,0x58,0x4D,0x4A,0x43,0x44,
|
|
|
+ 0x19,0x1E,0x17,0x10,0x05,0x02,0x0B,0x0C,0x21,0x26,0x2F,0x28,0x3D,0x3A,0x33,0x34,
|
|
|
+ 0x4E,0x49,0x40,0x47,0x52,0x55,0x5C,0x5B,0x76,0x71,0x78,0x7F,0x6A,0x6D,0x64,0x63,
|
|
|
+ 0x3E,0x39,0x30,0x37,0x22,0x25,0x2C,0x2B,0x06,0x01,0x08,0x0F,0x1A,0x1D,0x14,0x13,
|
|
|
+ 0xAE,0xA9,0xA0,0xA7,0xB2,0xB5,0xBC,0xBB,0x96,0x91,0x98,0x9F,0x8A,0x8D,0x84,0x83,
|
|
|
+ 0xDE,0xD9,0xD0,0xD7,0xC2,0xC5,0xCC,0xCB,0xE6,0xE1,0xE8,0xEF,0xFA,0xFD,0xF4,0xF3
|
|
|
+ ]
|
|
|
+
|
|
|
+
|
|
|
+def crc8(data):
|
|
|
+ crc = 0
|
|
|
+ length = len(data)
|
|
|
+ if length ==0:
|
|
|
+ return 0xff
|
|
|
+ crc &= 0xff;
|
|
|
+ i = 0
|
|
|
+ while length>0:
|
|
|
+ crc = crc8x_table[crc ^ data[i]];
|
|
|
+ length=length-1
|
|
|
+ i=i+1
|
|
|
+ return crc;
|
|
|
+
|
|
|
+
|
|
|
+def local_logger(*message):
|
|
|
+ for i in range(len(message)):
|
|
|
+ print(message[i], end=" ")
|
|
|
+ print()
|
|
|
+
|
|
|
+
|
|
|
+class SerialComm:
|
|
|
+ def __init__(self, port_name):
|
|
|
+ self.port = serial.Serial(timeout=0.05)
|
|
|
+ if port_name != "":
|
|
|
+ self.port.port = port_name
|
|
|
+ else:
|
|
|
+ self.port.port = '/dev/ttyACM0'
|
|
|
+
|
|
|
+ self.port.parity = serial.PARITY_NONE
|
|
|
+ self.port.baudrate = 921600
|
|
|
+ self.comm_thread = None
|
|
|
+ self.callback = local_logger
|
|
|
+ self.measure_active = False
|
|
|
+ self.data_file = None
|
|
|
+
|
|
|
+ def open(self):
|
|
|
+ self.port.open()
|
|
|
+ self.port.flush()
|
|
|
+ self.callback('i', 0, 'Open communication port')
|
|
|
+
|
|
|
+ def close(self):
|
|
|
+ self.port.close()
|
|
|
+
|
|
|
+ def is_connected(self):
|
|
|
+ return self.port.is_open
|
|
|
+
|
|
|
+ def set_logger(self, callback):
|
|
|
+ self.callback = callback
|
|
|
+
|
|
|
+ def create_packet(self, start, data):
|
|
|
+ if len(data) > 0:
|
|
|
+ for c in data:
|
|
|
+ start.append(c)
|
|
|
+
|
|
|
+ if not self.port.is_open:
|
|
|
+ self.open()
|
|
|
+ self.port.flush()
|
|
|
+
|
|
|
+ start[0] = len(start)
|
|
|
+ crc_sum = crc8(start[1:])
|
|
|
+
|
|
|
+ start.append(crc_sum)
|
|
|
+ return start
|
|
|
+
|
|
|
+ def wait_to_response(self, extra_delay):
|
|
|
+ communication_delay = 0.001
|
|
|
+ t0 = time.time() # timeout for response
|
|
|
+ response_length = self.port.inWaiting()
|
|
|
+ timeout = 0.05
|
|
|
+ if extra_delay != 0:
|
|
|
+ timeout = 0.5
|
|
|
+ while response_length == 0:
|
|
|
+ time.sleep(communication_delay)
|
|
|
+ response_length = self.port.inWaiting()
|
|
|
+ if time.time() - t0 > timeout:
|
|
|
+ self.callback('d', 0, "SKIP response")
|
|
|
+ return 0
|
|
|
+ return 1
|
|
|
+
|
|
|
+ def receive_response(self):
|
|
|
+ data = []
|
|
|
+ response_l = self.port.read(1)
|
|
|
+ response = self.port.read(ord(response_l))
|
|
|
+
|
|
|
+ for bajt in response:
|
|
|
+ data.append(bajt)
|
|
|
+ self.callback('d', 0, "\tRS=>", data)
|
|
|
+ return data
|
|
|
+
|
|
|
+ def request(self, module, sensor, command, data, skip_check=False, long_answer=0):
|
|
|
+ m = self.create_packet([0, module, sensor, command], data)
|
|
|
+ self.callback('d', sensor, "\tRQ>", m)
|
|
|
+ # SEND THE REQUEST
|
|
|
+ # self.port.write(m)
|
|
|
+ self.port.write([m[0]]) # send legth of message
|
|
|
+ # usleep(5)
|
|
|
+ self.port.write(m[1:]) # send body of message
|
|
|
+
|
|
|
+# if self.wait_to_response(long_answer) == 0:
|
|
|
+# return []
|
|
|
+
|
|
|
+ data = self.receive_response()
|
|
|
+# if not skip_check:
|
|
|
+# self.check_respose(data)
|
|
|
+ return data
|
|
|
+
|
|
|
+ def read_bytes(self, num):
|
|
|
+ return self.port.read(num)
|
|
|
+
|
|
|
+ def prepare_read_data(self):
|
|
|
+ if not self.port.is_open:
|
|
|
+ self.open()
|
|
|
+ self.flush()
|
|
|
+
|
|
|
+def to_int16(bytes_data):
|
|
|
+ uint16 = bytes_data[0] * 256 + bytes_data[1]
|
|
|
+ if uint16 > 2 ** 15:
|
|
|
+ uint16 = uint16 - 2 ** 16
|
|
|
+ return uint16
|
|
|
+
|
|
|
+
|