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         sampleNum= {SNS_ACC:   0,
  83                     SNS_MAG:   0,
  84                     SNS_LGT:   0,
  85                     SNS_TCH:   0,
  86                     SNS_A2D:   0}
  87
  88         # Parser data
  89         index    = 0   # read index
  90         count    = 0   # number of items in frame
  91         length   = 0   # length of frame (in bytes)
  92         bits_sns = 0   # sensor type
  93         bits_typ = 0   # data type
  94         binary   = ""  # binary read-in
  95         values   = []  # converted numeric data
  96
  97         # Constructor
  98         def __init__(self):
  99                 pass
 100
 101         # Converters
 102         @staticmethod
 103         def findCode(dataMap, name):
 104                 for code in dataMap:
 105                         if dataMap[code] == name:
 106                                 return code
 107                 print("[ERROR] No code found")
 108
 109         # Parse frame
 110         def parse(self, byte):
 111                 # save current pos and increment read index
 112                 # if we have an error we cna reset index below
 113                 pos = self.index
 114                 self.index += 1
 115
 116                 if pos == Frame.HEADER_POS:
 117                         if ord(byte) != Frame.HEADER:
 118                                 self.index = 0
 119                         #print('parse: header  %02x' % ord(byte))
 120
 121                 elif pos == Frame.BITS_POS:
 122                         self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT
 123                         self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT
 124                         if self.bits_sns >= Frame.SNS_NUM:
 125                                 self.index = 0
 126                         if self.bits_typ >= Frame.TYP_NUM:
 127                                 self.index = 0
 128                         #print('parse: bits    sns=%d typ=%d' %
 129                         #       (self.bits_sns, self.bits_typ))
 130
 131                 elif pos == Frame.COUNT_POS:
 132                         wordsize    = Frame.sizeMap[self.bits_typ]
 133                         self.count  = ord(byte)
 134                         self.length = Frame.DATA_POS + self.count*wordsize + 1
 135                         #print('parse: count   cnt=%d len=%d' %
 136                         #       (self.count, self.length))
 137
 138                 elif pos < self.length-1:
 139                         self.binary += byte
 140                         #print('parse: data    %02x @%d' %
 141                         #       (ord(byte), pos-Frame.DATA_POS))
 142
 143                 elif pos == self.length-1:
 144                         #print('parse: tail    %02x' % ord(byte))
 145                         if ord(byte) == Frame.TAIL:
 146                                 state = self.convert()
 147                         else:
 148                                 state = None
 149                         self.binary = ""
 150                         self.index  = 0
 151                         return state
 152
 153                 elif pos > self.length-1:
 154                         print('Error parsing')
 155
 156         # Convert frame to state
 157         def convert(self):
 158                 # Covnert data
 159                 fmt = Frame.fmtMap[self.bits_typ] * self.count
 160                 sns = Frame.snsMap[self.bits_sns]
 161                 self.values = unpack('<'+fmt, self.binary)
 162
 163                 # Print debug output
 164                 self.sampleNum[self.bits_sns] += 1
 165                 if self.sampleNum[self.bits_sns] == 1000:
 166                         print('convert: %3s = \'%3s\'%%[%s] -> [%s]' %
 167                               (sns, fmt, hexDump(self.binary), fltDump(self.values)))
 168                         self.sampleNum[self.bits_sns] = 0
 169
 170                 # Create state
 171                 state = State()
 172                 setattr(state, sns, self.values)
 173                 return state
 174
 175 class Device:
 176         # Constructors
 177         def __init__(self, config):
 178                 print('Defice.__init__')
 179                 self.config = config
 180                 self.serial = None
 181                 self.frame  = Frame()
 182
 183         # Methods
 184         def connect(self):
 185                 print('Device.connect')
 186                 buildingFrame = 0
 187                 try:
 188                         self.inbuf  = []
 189                         self.serial = Serial(self.config.device, \
 190                                 baudrate = self.config.baudrate, \
 191                                 parity   = self.config.parity,   \
 192                                 bytesize = self.config.databits, \
 193                                 stopbits = self.config.stopbits, \
 194                                 timeout  = 0)
 195                         for sns in self.config.rate:
 196                                 self.set_rate(sns, self.config.rate[sns])
 197                         for sns in self.config.enable:
 198                                 self.set_enable(sns, self.config.enable[sns])
 199                         self.serial.flushInput()
 200                 except Exception as ex:
 201                         return str(ex)
 202
 203         def disconnect(self):
 204                 print('Device.disconnect')
 205                 if self.serial and self.serial.isOpen():
 206                         self.serial.close()
 207
 208         def running(self):                                      # isRunning
 209                 if self.serial == None:
 210                         return False
 211                 if self.serial.isOpen() == False:
 212                         return False
 213                 return True
 214
 215         def set_rate(self, sensor, interval):
 216                 sns   = Frame.findCode(Frame.snsMap, sensor)
 217                 bits  = (sns            << Frame.SNS_SHIFT) | \
 218                         (Frame.CMD_RATE << Frame.CMD_SHIFT)
 219                 self._write_binary('Bf', bits, interval)
 220
 221         def set_enable(self, sensor, enabled):
 222                 sns   = Frame.findCode(Frame.snsMap, sensor)
 223                 cmd   = Frame.CMD_START if enabled else Frame.CMD_STOP
 224                 bits  = (sns << Frame.SNS_SHIFT) | \
 225                         (cmd << Frame.CMD_SHIFT)
 226                 self._write_binary('B', bits)
 227
 228         def process(self):
 229                 if not self.running():
 230                         return []
 231
 232                 items = []
 233                 while self.serial.inWaiting():
 234                         byte  = self.serial.read()
 235                         state = self.frame.parse(byte)
 236                         if state:
 237                                 items.append(state)
 238                 return items
 239
 240
 241         # auxilary function
 242         def frame_len(self, frame):
 243                 if len(frame) < 3:
 244                         return -1
 245                 dataType_snsType = ord(frame[1])
 246                 dataNum  = ord(frame[2])
 247                 dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT
 248                 snsType  = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT
 249                 dataSize = Frame.sizeMap[dataType]
 250                 return (dataSize*dataNum+4)
 251
 252         def printHex(self, frame):
 253                 frameLen = self.frame_len(frame)
 254                 i = 0
 255                 while i<frameLen:
 256                         print(hex(ord(frame[i])))
 257                         i+=1
 258
 259         # Private methods
 260         def _write_binary(self, fmt, *args):
 261                 #print('Device._write_binary')
 262                 if self.serial:
 263                         fmt   = 'B' + fmt + 'B'
 264                         args  = [Frame.HEADER] + list(args) + [Frame.TAIL]
 265                         frame = pack('<'+fmt, *args)
 266                         print('write: bin:[' + hexDump(frame) + ']')
 267                         self.serial.write(frame)
 268                         self.serial.flush()
 269                         time.sleep(0.1)
 270
 271 def hexDump(frame):
 272         digits = ['%02x' % ord(byte) for byte in frame]
 273         return ' '.join(digits)
 274
 275 def fltDump(data):
 276         digits = ['%5.2f' % flt for flt in data]
 277         return ' '.join(digits)