from datetime import datetime
from struct import *
+class State: #information stored
+ acc = [None]*3
+ mag = [None]*3
+ lgt = [None]*1
+ tch = [None]*2
+ a2d = [None]*6
+ time = None
+ def __init__(self):
+ self.time = datetime.utcnow()
-#buildingFrame = 0; #if it is a start of new frame
+class Frame:
+ # Sensor types
+ SNS_ACC = 0x00
+ SNS_MAG = 0x01
+ SNS_LGT = 0x02
+ SNS_TCH = 0x03
+ SNS_A2D = 0x04
-class Const:
- HEADER = 0x02
- SNS_BITS = 5
- LGT_SNS = 0x00
- ACC_SNS = 0x01
- MAG_SNS = 0x02
- TCH_SNS = 0x03
- ADC_SNS_1 = 0x04
+ SNS_NUM = 0x05
+ SNS_SHIFT = 4
+ SNS_MASK = 0xF0
- INT = 0
- LONG = 1
- FLOAT = 2
- DOUBLE = 3
- TAIL = 0x0A
+ # Data types
+ TYP_S8 = 0
+ TYP_S16 = 1
+ TYP_S32 = 2
+ TYP_U8 = 3
+ TYP_U16 = 4
+ TYP_U32 = 5
+ TYP_F32 = 6
+ TYP_F64 = 7
- START = 0
- STOP = 1
- SET_INT = 2
- buildingFrame = 0
+ TYP_NUM = 8
+ TYP_SHIFT = 0
+ TYP_MASK = 0x0F
- snsCode = {'light':LGT_SNS,'touch':TCH_SNS,'acc':ACC_SNS,'mag':MAG_SNS,'a2d':ADC_SNS_1}
- cmdCode = {'start':START, 'stop':STOP, 'set':SET_INT}
- sizeMap = {INT:2, FLOAT:4, LONG:4, DOUBLE:8}
- typeMap = {0:INT, 1:LONG, FLOAT:2, DOUBLE:3}
+ # Command codes
+ CMD_STOP = 0
+ CMD_START = 1
+ CMD_RATE = 2
-class Static:
+ CMD_NUM = 3
+ CMD_SHIFT = 0
+ CMD_MASK = 0x0F
- count = 0
+ # Frame information
+ HEADER = 0x02
+ TAIL = 0x0A
+ HEADER_POS = 0
+ BITS_POS = 1
+ COUNT_POS = 2
+ DATA_POS = 3
-class State: #information stored
- acc = [None]*3
- mag = [None]*3
- touch = [None]*2
- light = [None]*1
- a2d = [None]*6
- time = None
+ # Maps
+ snsMap = {SNS_ACC: 'acc',
+ SNS_MAG: 'mag',
+ SNS_LGT: 'lgt',
+ SNS_TCH: 'tch',
+ SNS_A2D: 'a2d'}
+
+ cmdMap = {CMD_START, 'start',
+ CMD_STOP, 'stop',
+ CMD_RATE, 'rate'}
+
+ sizeMap = {TYP_S8: 1, TYP_S16: 2, TYP_S32: 4,
+ TYP_U8: 1, TYP_U16: 2, TYP_U32: 4,
+ TYP_F32: 4, TYP_F64: 8}
+
+ fmtMap = {TYP_S8: 'b', TYP_S16: 'h', TYP_S32: 'i',
+ TYP_U8: 'B', TYP_U16: 'H', TYP_U32: 'I',
+ TYP_F32: 'f', TYP_F64: 'd'}
+
+ sampleNum= {SNS_ACC: 0,
+ SNS_MAG: 0,
+ SNS_LGT: 0,
+ SNS_TCH: 0,
+ SNS_A2D: 0}
+ # Parser data
+ index = 0 # read index
+ count = 0 # number of items in frame
+ length = 0 # length of frame (in bytes)
+ bits_sns = 0 # sensor type
+ bits_typ = 0 # data type
+ binary = "" # binary read-in
+ values = [] # converted numeric data
+
+ # Constructor
def __init__(self):
- self.time = datetime.utcnow()
+ pass
+
+ # Converters
+ @staticmethod
+ def findCode(dataMap, name):
+ for code in dataMap:
+ if dataMap[code] == name:
+ return code
+ print("[ERROR] No code found")
+
+ # Parse frame
+ def parse(self, byte):
+ # save current pos and increment read index
+ # if we have an error we cna reset index below
+ pos = self.index
+ self.index += 1
+
+ if pos == Frame.HEADER_POS:
+ if ord(byte) != Frame.HEADER:
+ self.index = 0
+ #print('parse: header %02x' % ord(byte))
+
+ elif pos == Frame.BITS_POS:
+ self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT
+ self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT
+ if self.bits_sns >= Frame.SNS_NUM:
+ self.index = 0
+ if self.bits_typ >= Frame.TYP_NUM:
+ self.index = 0
+ #print('parse: bits sns=%d typ=%d' %
+ # (self.bits_sns, self.bits_typ))
+
+ elif pos == Frame.COUNT_POS:
+ wordsize = Frame.sizeMap[self.bits_typ]
+ self.count = ord(byte)
+ self.length = Frame.DATA_POS + self.count*wordsize + 1
+ #print('parse: count cnt=%d len=%d' %
+ # (self.count, self.length))
+
+ elif pos < self.length-1:
+ self.binary += byte
+ #print('parse: data %02x @%d' %
+ # (ord(byte), pos-Frame.DATA_POS))
+
+ elif pos == self.length-1:
+ #print('parse: tail %02x' % ord(byte))
+ if ord(byte) == Frame.TAIL:
+ state = self.convert()
+ else:
+ state = None
+ self.binary = ""
+ self.index = 0
+ return state
+
+ elif pos > self.length-1:
+ print('Error parsing')
+
+ # Convert frame to state
+ def convert(self):
+ # Covnert data
+ fmt = Frame.fmtMap[self.bits_typ] * self.count
+ sns = Frame.snsMap[self.bits_sns]
+ self.values = unpack('<'+fmt, self.binary)
+
+ # Print debug output
+ self.sampleNum[self.bits_sns] += 1
+ if self.sampleNum[self.bits_sns] == 1000:
+ print('convert: %3s = \'%3s\'%%[%s] -> [%s]' %
+ (sns, fmt, hexDump(self.binary), fltDump(self.values)))
+ self.sampleNum[self.bits_sns] = 0
+
+ # Create state
+ state = State()
+ setattr(state, sns, self.values)
+ return state
class Device:
# Constructors
def __init__(self, config):
- print("IN")
+ print('Defice.__init__')
self.config = config
self.serial = None
+ self.frame = Frame()
# Methods
def connect(self):
- print("C")
- buildingFrame = 0
+ print('Device.connect')
+ buildingFrame = 0
try:
self.inbuf = []
self.serial = Serial(self.config.device, \
bytesize = self.config.databits, \
stopbits = self.config.stopbits, \
timeout = 0)
- self.control()
+ for sns in self.config.rate:
+ self.set_rate(sns, self.config.rate[sns])
+ for sns in self.config.enable:
+ self.set_enable(sns, self.config.enable[sns])
self.serial.flushInput()
except Exception as ex:
return str(ex)
def disconnect(self):
- print("DC")
+ print('Device.disconnect')
if self.serial and self.serial.isOpen():
self.serial.close()
return False
return True
- def control(self):#################
- print("CT")
- frame = [None]*7
- for key in list(self.config.enable.keys()):
- state = self.config.enable[key]
- rate = self.config.rate[key]
- cmd = Const.cmdCode['start'] if state else Const.cmdCode['stop']
- sns = Const.snsCode[key]
- frame[0] = chr(Const.HEADER)
- frame[1] = chr(cmd<<(Const.SNS_BITS)|(0x1F&sns))
- frame[2:6] = pack('f',float(rate))
- frame[6] = Const.TAIL
- self.serial.write(frame)
- print('[SEND1] ',frame)
- frame[1] = Const.cmdCode['set']<<(Const.SNS_BITS)|(0x1F&sns)
- self.serial.write(frame)
- print('[SEND2] ',frame)
- self.serial.flush()
+ def set_rate(self, sensor, interval):
+ sns = Frame.findCode(Frame.snsMap, sensor)
+ bits = (sns << Frame.SNS_SHIFT) | \
+ (Frame.CMD_RATE << Frame.CMD_SHIFT)
+ self._write_binary('Bf', bits, interval)
+ def set_enable(self, sensor, enabled):
+ sns = Frame.findCode(Frame.snsMap, sensor)
+ cmd = Frame.CMD_START if enabled else Frame.CMD_STOP
+ bits = (sns << Frame.SNS_SHIFT) | \
+ (cmd << Frame.CMD_SHIFT)
+ self._write_binary('B', bits)
def process(self):
+ if not self.running():
+ return []
+
items = []
- count = 0
- limit = 1000
- if not self.running():
- return items;
- if self.serial.readable():
- buildingFrame = 0
- while (self.serial.inWaiting() or count<(self.frame_len(self.inbuf)-1)): ######
- char = self.serial.read()
- count +=1
- if char == chr(Const.TAIL) and buildingFrame and (len(self.inbuf))== self.frame_len(self.inbuf)-1:
- self.inbuf.append(char)
- #print("[TAIL]")
- line = "".join(self.inbuf)
- print(self.inbuf)
- time.sleep(0.001)
- #self.printHex(line)
- item = self._parse_ascii(line) # analyze the received data
- items.append(item)
- buildingFrame = 0 # finished building one frame
- #print ("BF set to 0")
- self.inbuf = []
- count = 0
- elif char == chr(Const.HEADER) and buildingFrame==0:
- self.inbuf = []
- buildingFrame = 1
- #print ("BF set to 1")
- self.inbuf.append(char)
- #print("[HEADER]")
- #count +=1
- elif buildingFrame:
- self.inbuf.append(char)
- #print("[DT]")
- #count +=1
- else:
- #print("[ERROR] Byte Going Nowhere")
- count = 0
- buildingFrame = 0 # reset the construction
- #print ("BF set to 0!")
- self.inbuf = []
- if count > limit:
- count = 0
- print("[ERROR] Exceeded Read Limit")
- break
- return items
-
-
- # auxilary function
- def frame_len(self, frame):
- if len(frame) < 3:
- return -1
- dataType_snsType = ord(frame[1])
- dataNum = ord(frame[2])
- dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
- snsType = 0x1F&dataType_snsType
- #print(dataType_snsType)
- #print(dataType)
- #print(snsType)
- #print(dataNum)
- dataSize = Const.sizeMap[Const.typeMap[dataType]]
- return (dataSize*dataNum+4)
-
-
- def printHex(self, frame):
- #print("PH")
- frameLen = self.frame_len(frame)
- i = 0
- while i<frameLen:
- print(hex(ord(frame[i])))
- i+=1
+ while self.serial.inWaiting():
+ byte = self.serial.read()
+ state = self.frame.parse(byte)
+ if state:
+ items.append(state)
+ return items
+
+ # auxilary function
+ def frame_len(self, frame):
+ if len(frame) < 3:
+ return -1
+ dataType_snsType = ord(frame[1])
+ dataNum = ord(frame[2])
+ dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT
+ snsType = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT
+ dataSize = Frame.sizeMap[dataType]
+ return (dataSize*dataNum+4)
+
+ def printHex(self, frame):
+ frameLen = self.frame_len(frame)
+ i = 0
+ while i<frameLen:
+ print(hex(ord(frame[i])))
+ i+=1
# Private methods
- def _write_ascii(self, line): # to be changed
- #print("WA")
+ def _write_binary(self, fmt, *args):
+ #print('Device._write_binary')
if self.serial:
- print('write: [' + line + ']')
- self.serial.write(line + '\n')
+ fmt = 'B' + fmt + 'B'
+ args = [Frame.HEADER] + list(args) + [Frame.TAIL]
+ frame = pack('<'+fmt, *args)
+ print('write: bin:[' + hexDump(frame) + ']')
+ self.serial.write(frame)
self.serial.flush()
time.sleep(0.1)
- def _parse_ascii(self, line):##############
- #print("PA")
- dataType_snsType = ord(line[1]);
- dataNum = ord(line[2]);
- dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
- snsType = 0x1F&dataType_snsType
- state = State()
- if snsType == Const.ACC_SNS:
- line = line[3:15]
- state.acc = unpack('3f',line)
- elif snsType == Const.MAG_SNS:
- line = line[3:9]
- state.mag = unpack('3h',line)
- elif snsType == Const.LGT_SNS:
- state.light[0] = ord(line[3])
- elif snsType == Const.TCH_SNS:
- line = line[3:11]
- state.touch = sunpack('2f',line)
- elif snsType == Const.ADC_SNS_1:
- line = line[3:15]
- state.a2d = unpack('6h', line)
- else:
- print('[ERROR] Nothing Happened!')
- return state
+def hexDump(frame):
+ digits = ['%02x' % ord(byte) for byte in frame]
+ return ' '.join(digits)
+
+def fltDump(data):
+ digits = ['%5.2f' % flt for flt in data]
+ return ' '.join(digits)
projects / ~andy / csm213a-hw / blobdiff