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 class State:                                    #information stored
   9         acc  = [None]*3
  10         mag  = [None]*3
  11         lgt  = [None]*1
  12         tch  = [None]*2
  13         a2d  = [None]*6
  14         time = None
  15
  16         def __init__(self):
  17                 self.time = datetime.utcnow()
  18
  19 class Frame:
  20         # Sensor types
  21         SNS_ACC    = 0x00
  22         SNS_MAG    = 0x01
  23         SNS_LGT    = 0x02
  24         SNS_TCH    = 0x03
  25         SNS_A2D    = 0x04
  26
  27         SNS_NUM    = 0x05
  28         SNS_SHIFT  = 4
  29         SNS_MASK   = 0xF0
  30
  31         # Data types
  32         TYP_S8     = 0
  33         TYP_S16    = 1
  34         TYP_S32    = 2
  35         TYP_U8     = 3
  36         TYP_U16    = 4
  37         TYP_U32    = 5
  38         TYP_F32    = 6
  39         TYP_F64    = 7
  40
  41         TYP_NUM    = 8
  42         TYP_SHIFT  = 0
  43         TYP_MASK   = 0x0F
  44
  45         # Command codes
  46         CMD_STOP   = 0
  47         CMD_START  = 1
  48         CMD_RATE   = 2
  49
  50         CMD_NUM    = 3
  51         CMD_SHIFT  = 0
  52         CMD_MASK   = 0x0F
  53
  54         # Frame information
  55         HEADER     = 0x02
  56         TAIL       = 0x0A
  57
  58         HEADER_POS = 0
  59         BITS_POS   = 1
  60         COUNT_POS  = 2
  61         DATA_POS   = 3
  62
  63         # Maps
  64         snsMap   = {SNS_ACC:   'acc',
  65                     SNS_MAG:   'mag',
  66                     SNS_LGT:   'lgt',
  67                     SNS_TCH:   'tch',
  68                     SNS_A2D:   'a2d'}
  69
  70         cmdMap   = {CMD_START, 'start',
  71                     CMD_STOP,  'stop',
  72                     CMD_RATE,  'rate'}
  73
  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}
  77
  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'}
  81
  82         # Parser data
  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
  90
  91         # Constructor
  92         def __init__(self):
  93                 pass
  94
  95         # Converters
  96         @staticmethod
  97         def findCode(dataMap, name):
  98                 for code in dataMap:
  99                         if dataMap[code] == name:
 100                                 return code
 101                 print("[ERROR] No code found")
 102
 103         # Parse frame
 104         def parse(self, byte):
 105                 # save current pos and increment read index
 106                 # if we have an error we cna reset index below
 107                 pos = self.index
 108                 self.index += 1
 109
 110                 if pos == Frame.HEADER_POS:
 111                         if ord(byte) != Frame.HEADER:
 112                                 self.index = 0
 113                         #print('parse: header  %02x' % ord(byte))
 114
 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:
 119                                 self.index = 0
 120                         if self.bits_typ >= Frame.TYP_NUM:
 121                                 self.index = 0
 122                         #print('parse: bits    sns=%d typ=%d' %
 123                         #       (self.bits_sns, self.bits_typ))
 124
 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))
 131
 132                 elif pos < self.length-1:
 133                         self.binary += byte
 134                         #print('parse: data    %02x @%d' %
 135                         #       (ord(byte), pos-Frame.DATA_POS))
 136
 137                 elif pos == self.length-1:
 138                         #print('parse: tail    %02x' % ord(byte))
 139                         if ord(byte) == Frame.TAIL:
 140                                 state = self.convert()
 141                         else:
 142                                 state = None
 143                         self.binary = ""
 144                         self.index  = 0
 145                         return state
 146
 147                 elif pos > self.length-1:
 148                         print('Error parsing')
 149
 150         # Convert frame to state
 151         def convert(self):
 152                 # Covnert data
 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)))
 158
 159                 # Create state
 160                 state = State()
 161                 setattr(state, sns, self.values)
 162                 return state
 163
 164 class Device:
 165         # Constructors
 166         def __init__(self, config):
 167                 print('Defice.__init__')
 168                 self.config = config
 169                 self.serial = None
 170                 self.frame  = Frame()
 171
 172         # Methods
 173         def connect(self):
 174                 print('Device.connect')
 175                 buildingFrame = 0
 176                 try:
 177                         self.inbuf  = []
 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, \
 183                                 timeout  = 0)
 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:
 190                         return str(ex)
 191
 192         def disconnect(self):
 193                 print('Device.disconnect')
 194                 if self.serial and self.serial.isOpen():
 195                         self.serial.close()
 196
 197         def running(self):                                      # isRunning
 198                 if self.serial == None:
 199                         return False
 200                 if self.serial.isOpen() == False:
 201                         return False
 202                 return True
 203
 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)
 209
 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)
 216
 217         def process(self):
 218                 items = []
 219                 count = 0
 220                 limit = 1000
 221
 222                 if not self.running():
 223                         return items
 224
 225                 while self.serial.readable():
 226                         try:
 227                                 byte  = self.serial.read()
 228                         except Exception as ex:
 229                                 # Not sure why this is excepting
 230                                 # if it says it's readable
 231                                 break
 232                         if len(byte) == 0:
 233                                 break
 234
 235                         state = self.frame.parse(byte)
 236                         if state:
 237                                 items.append(state)
 238                         if count > limit:
 239                                 print('[ERROR] Exceeded Read Limit')
 240                                 break
 241
 242                         count += 1
 243
 244                 return items
 245
 246
 247         # auxilary function
 248         def frame_len(self, frame):
 249                 if len(frame) < 3:
 250                         return -1
 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)
 257
 258         def printHex(self, frame):
 259                 frameLen = self.frame_len(frame)
 260                 i = 0
 261                 while i<frameLen:
 262                         print(hex(ord(frame[i])))
 263                         i+=1
 264
 265         # Private methods
 266         def _write_binary(self, fmt, *args):
 267                 #print('Device._write_binary')
 268                 if self.serial:
 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)
 274                         self.serial.flush()
 275                         time.sleep(0.1)
 276
 277 def hexDump(frame):
 278         digits = ['%02x' % ord(byte) for byte in frame]
 279         return ' '.join(digits)
 280
 281 def fltDump(data):
 282         digits = ['%5.2f' % flt for flt in data]
 283         return ' '.join(digits)