4 from serial import Serial
5 from datetime import datetime
8 class State: #information stored
17 self.time = datetime.utcnow()
64 snsMap = {SNS_ACC: 'acc',
70 cmdMap = {CMD_START, 'start',
74 sizeMap = {TYP_S8: 1, TYP_S16: 2, TYP_S32: 4,
75 TYP_U8: 1, TYP_U16: 2, TYP_U32: 4,
76 TYP_F32: 4, TYP_F64: 8}
78 fmtMap = {TYP_S8: 'b', TYP_S16: 'h', TYP_S32: 'i',
79 TYP_U8: 'B', TYP_U16: 'H', TYP_U32: 'I',
80 TYP_F32: 'f', TYP_F64: 'd'}
83 index = 0 # read index
84 count = 0 # number of items in frame
85 length = 0 # length of frame (in bytes)
86 bits_sns = 0 # sensor type
87 bits_typ = 0 # data type
88 binary = "" # binary read-in
89 values = [] # converted numeric data
97 def findCode(dataMap, name):
99 if dataMap[code] == name:
101 print("[ERROR] No code found")
104 def parse(self, byte):
105 # save current pos and increment read index
106 # if we have an error we cna reset index below
110 if pos == Frame.HEADER_POS:
111 if ord(byte) != Frame.HEADER:
113 #print('parse: header %02x' % ord(byte))
115 elif pos == Frame.BITS_POS:
116 self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT
117 self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT
118 if self.bits_sns >= Frame.SNS_NUM:
120 if self.bits_typ >= Frame.TYP_NUM:
122 #print('parse: bits sns=%d typ=%d' %
123 # (self.bits_sns, self.bits_typ))
125 elif pos == Frame.COUNT_POS:
126 wordsize = Frame.sizeMap[self.bits_typ]
127 self.count = ord(byte)
128 self.length = Frame.DATA_POS + self.count*wordsize + 1
129 #print('parse: count cnt=%d len=%d' %
130 # (self.count, self.length))
132 elif pos < self.length-1:
134 #print('parse: data %02x @%d' %
135 # (ord(byte), pos-Frame.DATA_POS))
137 elif pos == self.length-1:
138 #print('parse: tail %02x' % ord(byte))
139 if ord(byte) == Frame.TAIL:
140 state = self.convert()
147 elif pos > self.length-1:
148 print('Error parsing')
150 # Convert frame to state
153 fmt = Frame.fmtMap[self.bits_typ] * self.count
154 sns = Frame.snsMap[self.bits_sns]
155 self.values = unpack('<'+fmt, self.binary)
156 print('convert: %3s = \'%3s\'%%[%s] -> [%s]' %
157 (sns, fmt, hexDump(self.binary), fltDump(self.values)))
161 setattr(state, sns, self.values)
166 def __init__(self, config):
167 print('Defice.__init__')
174 print('Device.connect')
178 self.serial = Serial(self.config.device, \
179 baudrate = self.config.baudrate, \
180 parity = self.config.parity, \
181 bytesize = self.config.databits, \
182 stopbits = self.config.stopbits, \
184 for sns in self.config.rate:
185 self.set_rate(sns, self.config.rate[sns])
186 for sns in self.config.enable:
187 self.set_enable(sns, self.config.enable[sns])
188 self.serial.flushInput()
189 except Exception as ex:
192 def disconnect(self):
193 print('Device.disconnect')
194 if self.serial and self.serial.isOpen():
197 def running(self): # isRunning
198 if self.serial == None:
200 if self.serial.isOpen() == False:
204 def set_rate(self, sensor, interval):
205 sns = Frame.findCode(Frame.snsMap, sensor)
206 bits = (sns << Frame.SNS_SHIFT) | \
207 (Frame.CMD_RATE << Frame.CMD_SHIFT)
208 self._write_binary('Bf', bits, interval)
210 def set_enable(self, sensor, enabled):
211 sns = Frame.findCode(Frame.snsMap, sensor)
212 cmd = Frame.CMD_START if enabled else Frame.CMD_STOP
213 bits = (sns << Frame.SNS_SHIFT) | \
214 (cmd << Frame.CMD_SHIFT)
215 self._write_binary('B', bits)
222 if not self.running():
225 while self.serial.readable():
227 byte = self.serial.read()
228 except Exception as ex:
229 # Not sure why this is excepting
230 # if it says it's readable
235 state = self.frame.parse(byte)
239 print('[ERROR] Exceeded Read Limit')
248 def frame_len(self, frame):
251 dataType_snsType = ord(frame[1])
252 dataNum = ord(frame[2])
253 dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT
254 snsType = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT
255 dataSize = Frame.sizeMap[dataType]
256 return (dataSize*dataNum+4)
258 def printHex(self, frame):
259 frameLen = self.frame_len(frame)
262 print(hex(ord(frame[i])))
266 def _write_binary(self, fmt, *args):
267 #print('Device._write_binary')
269 fmt = 'B' + fmt + 'B'
270 args = [Frame.HEADER] + list(args) + [Frame.TAIL]
271 frame = pack('<'+fmt, *args)
272 print('write: bin:[' + hexDump(frame) + ']')
273 self.serial.write(frame)
278 digits = ['%02x' % ord(byte) for byte in frame]
279 return ' '.join(digits)
282 digits = ['%5.2f' % flt for flt in data]
283 return ' '.join(digits)