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         ACC_SNS    = 0x00
  22         MAG_SNS    = 0x01
  23         LGT_SNS    = 0x02
  24         TCH_SNS    = 0x03
  25         A2D_SNS    = 0x04
  26
  27         SNS_NUM    = 0x05
  28         SNS_SHIFT  = 4
  29         SNS_MASK   = 0xF0
  30
  31         # Data types
  32         INT        = 0
  33         LONG       = 1
  34         FLOAT      = 2
  35         DOUBLE     = 3
  36
  37         TYP_NUM    = 4
  38         TYP_SHIFT  = 0
  39         TYP_MASK   = 0x0F
  40
  41         # Command codes
  42         STOP       = 0
  43         START      = 1
  44         RATE       = 2
  45
  46         OPER_NUM   = 3
  47         OPER_SHIFT = 0
  48         OPER_MASK  = 0x0F
  49
  50         # Frame information
  51         HEADER     = 0x02
  52         TAIL       = 0x0A
  53
  54         HEADER_POS = 0
  55         BITS_POS   = 1
  56         COUNT_POS  = 2
  57         DATA_POS   = 3
  58
  59         # Maps
  60         snsMap   = {ACC_SNS: 'acc',
  61                     MAG_SNS: 'mag',
  62                     LGT_SNS: 'lgt',
  63                     TCH_SNS: 'tch',
  64                     A2D_SNS: 'a2d'}
  65
  66         cmdMap   = {START,   'start',
  67                     STOP,    'stop',
  68                     RATE,    'rate'}
  69
  70         sizeMap  = {INT:      2,
  71                     LONG:     4,
  72                     FLOAT:    4,
  73                     DOUBLE:   8}
  74
  75         fmtMap   = {INT:     'h',
  76                     LONG:    'i',
  77                     FLOAT:   'f',
  78                     DOUBLE:  'd'}
  79
  80         # Parser data
  81         index    = 0   # read index
  82         count    = 0   # number of items in frame
  83         length   = 0   # length of frame (in bytes)
  84         bits_sns = 0   # sensor type
  85         bits_typ = 0   # data type
  86         binary   = ""  # binary read-in
  87         values   = []  # converted numeric data
  88
  89         # Constructor
  90         def __init__(self):
  91                 pass
  92
  93         # Converters
  94         @staticmethod
  95         def findCode(dataMap, name):
  96                 for code in dataMap:
  97                         if dataMap[code] == name:
  98                                 return code
  99                 print("[ERROR] No code found")
 100
 101         # Parse frame
 102         def parse(self, byte):
 103                 # save current pos and increment read index
 104                 # if we have an error we cna reset index below
 105                 pos = self.index
 106                 self.index += 1
 107
 108                 if pos == Frame.HEADER_POS:
 109                         if ord(byte) != Frame.HEADER:
 110                                 self.index = 0
 111                         #print('parse: header  %02x' % ord(byte))
 112
 113                 elif pos == Frame.BITS_POS:
 114                         self.bits_sns = (ord(byte) & Frame.SNS_MASK) >> Frame.SNS_SHIFT
 115                         self.bits_typ = (ord(byte) & Frame.TYP_MASK) >> Frame.TYP_SHIFT
 116                         if self.bits_sns >= Frame.SNS_NUM:
 117                                 self.index = 0
 118                         if self.bits_typ >= Frame.TYP_NUM:
 119                                 self.index = 0
 120                         #print('parse: bits    sns=%d typ=%d' %
 121                         #       (self.bits_sns, self.bits_typ))
 122
 123                 elif pos == Frame.COUNT_POS:
 124                         wordsize    = Frame.sizeMap[self.bits_typ]
 125                         self.count  = ord(byte)
 126                         self.length = Frame.DATA_POS + self.count*wordsize + 1
 127                         #print('parse: count   cnt=%d len=%d' %
 128                         #       (self.count, self.length))
 129
 130                 elif pos < self.length-1:
 131                         self.binary += byte
 132                         #print('parse: data    %02x @%d' %
 133                         #       (ord(byte), pos-Frame.DATA_POS))
 134
 135                 elif pos == self.length-1:
 136                         #print('parse: tail    %02x' % ord(byte))
 137                         if ord(byte) == Frame.TAIL:
 138                                 state = self.convert()
 139                         else:
 140                                 state = None
 141                         self.binary = ""
 142                         self.index  = 0
 143                         return state
 144
 145                 elif pos > self.length-1:
 146                         print('Error parsing')
 147
 148         # Convert frame to state
 149         def convert(self):
 150                 # Covnert data
 151                 fmt = Frame.fmtMap[self.bits_typ] * self.count
 152                 sns = Frame.snsMap[self.bits_sns]
 153                 self.values = unpack('<'+fmt, self.binary)
 154                 print('convert: %3s = \'%3s\'%%[%s] -> [%s]' %
 155                         (sns, fmt, hexDump(self.binary), fltDump(self.values)))
 156
 157                 # Create state
 158                 state = State()
 159                 setattr(state, sns, self.values)
 160                 return state
 161
 162 class Device:
 163         # Constructors
 164         def __init__(self, config):
 165                 print('Defice.__init__')
 166                 self.config = config
 167                 self.serial = None
 168                 self.frame  = Frame()
 169
 170         # Methods
 171         def connect(self):
 172                 print('Device.connect')
 173                 buildingFrame = 0
 174                 try:
 175                         self.inbuf  = []
 176                         self.serial = Serial(self.config.device, \
 177                                 baudrate = self.config.baudrate, \
 178                                 parity   = self.config.parity,   \
 179                                 bytesize = self.config.databits, \
 180                                 stopbits = self.config.stopbits, \
 181                                 timeout  = 0)
 182                         for sns in self.config.rate:
 183                                 self.set_rate(sns, self.config.rate[sns])
 184                         for sns in self.config.enable:
 185                                 self.set_enable(sns, self.config.enable[sns])
 186                         self.serial.flushInput()
 187                 except Exception as ex:
 188                         return str(ex)
 189
 190         def disconnect(self):
 191                 print('Device.disconnect')
 192                 if self.serial and self.serial.isOpen():
 193                         self.serial.close()
 194
 195         def running(self):                                      # isRunning
 196                 if self.serial == None:
 197                         return False
 198                 if self.serial.isOpen() == False:
 199                         return False
 200                 return True
 201
 202         def set_rate(self, sensor, interval):
 203                 sns   = Frame.findCode(Frame.snsMap, sensor)
 204                 bits  = (sns        << Frame.SNS_SHIFT) | \
 205                         (Frame.RATE << Frame.OPER_SHIFT)
 206                 self._write_binary('Bf', bits, interval)
 207
 208         def set_enable(self, sensor, enabled):
 209                 sns   = Frame.findCode(Frame.snsMap, sensor)
 210                 oper  = Frame.START if enabled else Frame.STOP
 211                 bits  = (sns  << Frame.SNS_SHIFT) | \
 212                         (oper << Frame.OPER_SHIFT)
 213                 self._write_binary('B', bits)
 214
 215         def process(self):
 216                 items = []
 217                 count = 0
 218                 limit = 1000
 219
 220                 if not self.running():
 221                         return items
 222
 223                 while self.serial.readable():
 224                         try:
 225                                 byte  = self.serial.read()
 226                         except Exception as ex:
 227                                 # Not sure why this is excepting
 228                                 # if it says it's readable
 229                                 break
 230                         if len(byte) == 0:
 231                                 break
 232
 233                         state = self.frame.parse(byte)
 234                         if state:
 235                                 items.append(state)
 236                         if count > limit:
 237                                 print('[ERROR] Exceeded Read Limit')
 238                                 break
 239
 240                         count += 1
 241
 242                 return items
 243
 244
 245         # auxilary function
 246         def frame_len(self, frame):
 247                 if len(frame) < 3:
 248                         return -1
 249                 dataType_snsType = ord(frame[1])
 250                 dataNum  = ord(frame[2])
 251                 dataType = (dataType_snsType & Frame.TYP_MASK) >> Frame.TYP_SHIFT
 252                 snsType  = (dataType_snsType & Frame.SNS_MASK) >> Frame.SNS_SHIFT
 253                 dataSize = Frame.sizeMap[dataType]
 254                 return (dataSize*dataNum+4)
 255
 256         def printHex(self, frame):
 257                 frameLen = self.frame_len(frame)
 258                 i = 0
 259                 while i<frameLen:
 260                         print(hex(ord(frame[i])))
 261                         i+=1
 262
 263         # Private methods
 264         def _write_binary(self, fmt, *args):
 265                 #print('Device._write_binary')
 266                 if self.serial:
 267                         fmt   = 'B' + fmt + 'B'
 268                         args  = [Frame.HEADER] + list(args) + [Frame.TAIL]
 269                         frame = pack('<'+fmt, *args)
 270                         print('write: bin:[' + hexDump(frame) + ']')
 271                         self.serial.write(frame)
 272                         self.serial.flush()
 273                         time.sleep(0.1)
 274
 275 def hexDump(frame):
 276         digits = ['%02x' % ord(byte) for byte in frame]
 277         return ' '.join(digits)
 278
 279 def fltDump(data):
 280         digits = ['%5.2f' % flt for flt in data]
 281         return ' '.join(digits)