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