Pileus Git - ~andy/csm213a-hw/blob - vis/device.py

   1 import time
   2
   3 from re       import compile
   4 from serial   import Serial
   5 from datetime import datetime
   6 from struct   import *
   7
   8
   9
  10 #buildingFrame = 0;               #if it is a start of new frame
  11
  12 class Const:
  13         HEADER = 0x02
  14         SNS_BITS = 5
  15         LGT_SNS = 0x00
  16         ACC_SNS = 0x01
  17         MAG_SNS = 0x02
  18         TCH_SNS = 0x03
  19         ADC_SNS_1 = 0x04
  20
  21         INT = 0
  22         LONG = 1
  23         FLOAT = 2
  24         DOUBLE = 3
  25         TAIL = 0x0A
  26
  27         START = 0
  28         STOP = 1
  29         SET_INT = 2
  30         buildingFrame = 0
  31
  32         snsCode = {'light':LGT_SNS,'touch':TCH_SNS,'acc':ACC_SNS,'mag':MAG_SNS,'a2d':ADC_SNS_1}
  33         cmdCode = {'start':START, 'stop':STOP, 'set':SET_INT}
  34         sizeMap = {INT:2, FLOAT:4, LONG:4, DOUBLE:8}
  35         typeMap = {0:INT, 1:LONG, FLOAT:2, DOUBLE:3}
  36
  37 class Static:
  38
  39         count = 0
  40
  41
  42 class State:                                    #information stored
  43         acc   = [None]*3
  44         mag   = [None]*3
  45         touch = [None]*2
  46         light = [None]*1
  47         a2d   = [None]*6
  48         time  = None
  49
  50         def __init__(self):
  51                 self.time = datetime.utcnow()
  52
  53 class Device:
  54         # Constructors
  55         def __init__(self, config):
  56                 print("IN")
  57                 self.config = config
  58                 self.serial = None
  59
  60         # Methods
  61         def connect(self):
  62                 print("C")
  63                 buildingFrame = 0
  64                 try:
  65                         self.inbuf  = []
  66                         self.serial = Serial(self.config.device, \
  67                                 baudrate = self.config.baudrate, \
  68                                 parity   = self.config.parity,   \
  69                                 bytesize = self.config.databits, \
  70                                 stopbits = self.config.stopbits, \
  71                                 timeout  = 0)
  72                         self.control()
  73                         self.serial.flushInput()
  74                 except Exception as ex:
  75                         return str(ex)
  76
  77         def disconnect(self):
  78                 print("DC")
  79                 if self.serial and self.serial.isOpen():
  80                         self.serial.close()
  81
  82         def running(self):                                      # isRunning
  83                 if self.serial == None:
  84                         return False
  85                 if self.serial.isOpen() == False:
  86                         return False
  87                 return True
  88
  89         def control(self):
  90                 print("CT")
  91                 frame = [None]*7
  92                 for key in list(self.config.enable.keys()):
  93                         state = self.config.enable[key]
  94                         rate  = self.config.rate[key]
  95                         cmd   = Const.cmdCode['start'] if state else Const.cmdCode['stop']
  96                         sns   = Const.snsCode[key]
  97                         frame[0] = chr(Const.HEADER)
  98                         frame[1] = chr(cmd<<(Const.SNS_BITS)|(0x1F&sns))
  99                         frame[2:6] = pack('f',float(rate))
 100                         frame[6] = Const.TAIL
 101                         self.serial.write(frame)
 102                         print('[SEND1] ',frame)
 103                         frame[1] = Const.cmdCode['set']<<(Const.SNS_BITS)|(0x1F&sns)
 104                         self.serial.write(frame)
 105                         print('[SEND2] ',frame)
 106                         self.serial.flush()
 107
 108
 109         def process(self):
 110                 items = []
 111                 count = 0
 112                 limit = 1000
 113                 if not self.running():
 114                         return items;
 115                 if self.serial.readable():
 116                         buildingFrame = 0
 117                 while (self.serial.inWaiting() or count<(self.frame_len(self.inbuf)-1)):                ######
 118                         char = self.serial.read()
 119                         count +=1
 120                         if char == chr(Const.TAIL) and buildingFrame and (len(self.inbuf))== self.frame_len(self.inbuf)-1:
 121                                 self.inbuf.append(char)
 122                                 #print("[TAIL]")
 123                                 line = "".join(self.inbuf)
 124                                 print(self.inbuf)
 125                                 time.sleep(0.001)
 126                                 #self.printHex(line)
 127                                 item = self._parse_ascii(line)  # analyze the received data
 128                                 items.append(item)
 129                                 buildingFrame = 0               # finished building one frame
 130                                 #print ("BF set to 0")
 131                                 self.inbuf = []
 132                                 count = 0
 133                         elif char == chr(Const.HEADER) and buildingFrame==0:
 134                                 self.inbuf = []
 135                                 buildingFrame = 1
 136                                 #print ("BF set to 1")
 137                                 self.inbuf.append(char)
 138                                 #print("[HEADER]")
 139                                 #count +=1
 140                         elif buildingFrame:
 141                                 self.inbuf.append(char)
 142                                 #print("[DT]")
 143                                 #count +=1
 144                         else:
 145                                 #print("[ERROR] Byte Going Nowhere")
 146                                 count = 0
 147                                 buildingFrame = 0               # reset the construction
 148                                 #print ("BF set to 0!")
 149                                 self.inbuf = []
 150                         if count > limit:
 151                                 count = 0
 152                                 print("[ERROR] Exceeded Read Limit")
 153                                 break
 154                 return items
 155
 156
 157         # auxilary function
 158         def frame_len(self, frame):
 159                 if len(frame) < 3:
 160                         return -1
 161                 dataType_snsType = ord(frame[1])
 162                 dataNum  = ord(frame[2])
 163                 dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
 164                 snsType  = 0x1F&dataType_snsType
 165                 #print(dataType_snsType)
 166                 #print(dataType)
 167                 #print(snsType)
 168                 #print(dataNum)
 169                 dataSize = Const.sizeMap[Const.typeMap[dataType]]
 170                 return (dataSize*dataNum+4)
 171
 172
 173         def printHex(self, frame):
 174                 #print("PH")
 175                 frameLen = self.frame_len(frame)
 176                 i = 0
 177                 while i<frameLen:
 178                         print(hex(ord(frame[i])))
 179                         i+=1
 180
 181
 182         # Private methods
 183         def _write_ascii(self, line):                           # to be changed
 184                 #print("WA")
 185                 if self.serial:
 186                         print('write: [' + line + ']')
 187                         self.serial.write(line + '\n')
 188                         self.serial.flush()
 189                         time.sleep(0.1)
 190
 191         def _parse_ascii(self, line):##############
 192                 #print("PA")
 193                 dataType_snsType = ord(line[1]);
 194                 dataNum  = ord(line[2]);
 195                 dataType = (0xE0&dataType_snsType)>>Const.SNS_BITS
 196                 snsType  = 0x1F&dataType_snsType
 197                 state = State()
 198                 if snsType == Const.ACC_SNS:
 199                         line = line[3:15]
 200                         state.acc = unpack('3f',line)
 201                 elif snsType == Const.MAG_SNS:
 202                         line = line[3:9]
 203                         state.mag = unpack('3h',line)
 204                 elif snsType == Const.LGT_SNS:
 205                         state.light[0] = ord(line[3])
 206                 elif snsType == Const.TCH_SNS:
 207                         line = line[3:11]
 208                         state.touch = sunpack('2f',line)
 209                 elif snsType == Const.ADC_SNS_1:
 210                         line = line[3:15]
 211                         state.a2d = unpack('6h', line)
 212                 else:
 213                         print('[ERROR] Nothing Happened!')
 214                 return state