00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 import sys, serial, time, string
00036 from threading import Thread
00037
00038
00039 def prin(x): print x
00040
00041
00042
00043
00044
00045 class M5e_Poller(Thread):
00046 QUERY_MODE = 'query'
00047 TRACK_MODE = 'track'
00048
00049 def __init__(self, M5e, antfuncs=[], callbacks=[]):
00050 Thread.__init__(self)
00051 self.M5e = M5e
00052 self.antfuncs = antfuncs
00053 self.callbacks = callbacks
00054 self.mode = ''
00055 self.should_run = True
00056
00057 print 'Creating M5e Polling Thread'
00058 self.start()
00059
00060 def pause_poller(self):
00061 self.mode = ''
00062
00063 def query_mode(self):
00064 self.mode = self.QUERY_MODE
00065
00066 def track_mode(self, tag_id):
00067 self.mode = self.TRACK_MODE
00068 self.tag_to_track = tag_id
00069
00070 def run(self):
00071 while self.should_run:
00072 if self.mode == self.QUERY_MODE:
00073 for aF in self.antfuncs:
00074 antennaName = aF(self.M5e)
00075 results = self.M5e.QueryEnvironment()
00076 for tagid, rssi in results:
00077 datum = [antennaName, tagid, rssi]
00078 [cF(datum) for cF in self.callbacks]
00079 elif self.mode == self.TRACK_MODE:
00080 for aF in self.antfuncs:
00081 antennaName = aF(self.M5e)
00082 tagid = self.tag_to_track
00083 rssi = self.M5e.TrackSingleTag(tagid)
00084
00085 datum = [antennaName, tagid, rssi]
00086 [cF(datum) for cF in self.callbacks]
00087 else:
00088 time.sleep(0.005)
00089
00090 def stop(self):
00091 self.should_run = False
00092 self.join(3)
00093 if (self.isAlive()):
00094 raise RuntimeError("RFID M5e Poller: unable to stop thread")
00095
00096 class M5e:
00097 "Interface to Mercury M5e and M5e-Compact"
00098 def __init__(self, portSTR='/dev/robot/RFIDreader', baudrate=9600,
00099 TXport=1, RXport=1, readPwr=2300, protocol='GEN2', compact=True, verbosity_func=prin):
00100
00101
00102
00103
00104 try:
00105 self.port = string.atoi( portSTR )
00106 except:
00107 self.port = portSTR
00108
00109 self.baudrate = baudrate
00110 self.TXport = TXport
00111 self.RXport = RXport
00112 self.readPwr = readPwr
00113 self.protocol = protocol
00114 self.compact = compact
00115 self.prin = verbosity_func
00116 self.ser = None
00117
00118 self.prin( 'Initializing M5e (or M5e-Compact)' )
00119
00120
00121
00122
00123 failed = False
00124 try:
00125 self.prin( '\tAttempting 230400 bps' )
00126 self.ser = serial.Serial(self.port, 230400, timeout=2, writeTimeout=2)
00127
00128 self.TransmitCommand('\x00\x03')
00129 self.ReceiveResponse()
00130 self.prin( '\tSuccessful at 230400 bps' )
00131 except:
00132 self.ser = None
00133 failed = True
00134 self.prin( '\tFailed @ 230400 bps' )
00135 pass
00136
00137 if failed:
00138 self.prin( '\tAttempting 9600 bps' )
00139 try:
00140 self.ser = serial.Serial(self.port, 9600, timeout=2, writeTimeout=2)
00141 self.TransmitCommand('\x00\x03')
00142 self.ReceiveResponse()
00143 self.prin( '\tSuccessful @ 9600 bps' )
00144 except:
00145 self.prin( '\tFailed 9600 bps' )
00146 self.ser = None
00147 raise M5e_SerialPortError('Could not open serial port %s at baudrate %d or %d.' % (str(self.port),230400,9600))
00148
00149
00150 self.prin( '\tSwitching to 230400 bps' )
00151 self.TransmitCommand('\x04\x06\x00\x03\x84\x00')
00152 self.ReceiveResponse()
00153 self.ser = None
00154 try:
00155 self.prin( '\tAttempting to reconnect @ 230400 bps' )
00156 self.ser = serial.Serial(self.port, 230400, timeout=2, writeTimeout=2)
00157
00158 self.TransmitCommand('\x00\x03')
00159 self.ReceiveResponse()
00160 self.prin( '\tSuccessfully reconnected at 230400 bps' )
00161 except:
00162 self.ser = None
00163 self.prin( '\tFailed @ 230400 bps' )
00164 pass
00165
00166 if self.ser == None:
00167 raise M5e_SerialPortError('Could not open serial port %s at baudrate %d or %d.' % (str(self.port),230400,9600))
00168
00169
00170 self.TransmitCommand('\x00\x03')
00171 self.ReceiveResponse()
00172
00173
00174 self.TransmitCommand('\x00\x04')
00175 try:
00176 self.ReceiveResponse()
00177 except M5e_CommandStatusError, inst:
00178
00179
00180
00181 if inst[1] == '\x01\x01':
00182 pass
00183 else:
00184 raise
00185
00186 self.ChangeAntennaPorts(self.TXport, self.RXport)
00187 self.ChangeTXReadPower(self.readPwr)
00188
00189
00190 if self.protocol != 'GEN2':
00191 raise M5e_error('Sorry, GEN2 is only protocol supported at this time')
00192 self.TransmitCommand('\x02\x93\x00\x05')
00193 self.ReceiveResponse()
00194
00195
00196 self.TransmitCommand('\x01\x97\x01')
00197 self.ReceiveResponse()
00198
00199
00200
00201
00202
00203
00204 def ChangeAntennaPorts(self, TXport, RXport):
00205 "Changes TX and RX ports"
00206 self.TXport = TXport
00207 self.RXport = RXport
00208 self.TransmitCommand('\x02\x91' + chr(self.TXport) + chr(self.RXport))
00209 self.ReceiveResponse()
00210
00211 def ChangeTXReadPower(self, readPwr):
00212 "Sets the Read TX Power based on current value of readPwr (in centi-dBm)"
00213 self.readPwr = readPwr
00214 readTXPwrHighByte = chr((self.readPwr & 0xFFFF) >> 8)
00215 readTXPwrLowByte = chr(self.readPwr & 0x00FF)
00216 self.TransmitCommand('\x02\x92'+ readTXPwrHighByte + readTXPwrLowByte)
00217 self.ReceiveResponse()
00218
00219 def CalculateCRC(self, msg):
00220 "Implements CCITT CRC-16 defined in Mercury Embedded Module Dev Guide."
00221 crcResult = 0xFFFF
00222 for x in range(len(msg)):
00223 currChar = ord(msg[x])
00224 v = 0x80
00225 for y in range(8):
00226 xor_flag = 0
00227 if (crcResult & 0x8000):
00228 xor_flag = 1
00229 crcResult = crcResult << 1
00230 if (currChar & v):
00231 crcResult = crcResult + 1
00232 if (xor_flag):
00233 crcResult = crcResult ^ 0x1021
00234 v = v >> 1
00235 crcResult = crcResult & 0xFFFF
00236
00237 return chr((crcResult >> 8) & 0xFF) + chr(crcResult & 0xFF)
00238
00239
00240 def ReturnHexString(self, hexStr):
00241 "Helper function to visualize a hex string (such as a hexCommand)"
00242 result = ''
00243 for i in range(len(hexStr)):
00244 result = result + hex(ord(hexStr[i])) + ' '
00245 return result
00246
00247 def ConstructCommand(self, hexCommand):
00248 "Helper function to attach start byte and CRC to a command string"
00249 return '\xFF' + hexCommand + self.CalculateCRC(hexCommand)
00250
00251 def TransmitCommand(self, command):
00252 "Transmits a command. Should call ReceiveResponse before calling again."
00253 try:
00254 self.ser.write(self.ConstructCommand(command))
00255 except:
00256 raise M5e_TransmitTimeoutExceeded('Something happened (probably power failure) to disable serial transmission')
00257
00258 def QuickTrackResponse( self ):
00259 "Does almost no error checking, and can hang..."
00260
00261
00262 slcs = self.fr( 5 )
00263
00264 if slcs[3:] == '\x04\x00':
00265
00266 dc = self.sec( ord(slcs[1]) + 2 )
00267
00268
00269 return -1
00270
00271
00272 dc = self.sec( 20 )
00273
00274
00275 return ord( dc[3] )
00276
00277 def fr( self, chars ):
00278 return self.ser.read( chars )
00279
00280 def sec( self, chars ):
00281 return self.ser.read( chars )
00282
00283 def ReceiveResponse(self):
00284 "Receives a single command's response"
00285
00286 timeoutsToWait = 5
00287 timeoutsWaited = 0
00288
00289 while timeoutsWaited < timeoutsToWait:
00290 start = self.ser.read()
00291 if start == '\xFF':
00292 break
00293 timeoutsWaited += 1
00294
00295 if start != '\xFF':
00296 time.sleep(5)
00297 self.ser.flushInput()
00298 raise M5e_ReceiveError('Error in receive stream (start byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00299
00300 length = self.ser.read()
00301 if len(length) != 1:
00302 time.sleep(5)
00303 self.ser.flushInput()
00304 raise M5e_ReceiveError('Error in receive stream (length byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00305
00306 command = self.ser.read()
00307 if len(command) != 1:
00308 time.sleep(5)
00309 self.ser.flushInput()
00310 raise M5e_ReceiveError('Error in receive stream (command byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00311
00312 status = self.ser.read(2)
00313 if len(status) != 2:
00314 time.sleep(5)
00315 self.ser.flushInput()
00316 raise M5e_ReceiveError('Error in receive stream (status bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00317
00318 data = self.ser.read(ord(length))
00319 if len(data) != ord(length):
00320 time.sleep(5)
00321 self.ser.flushInput()
00322 raise M5e_ReceiveError('Error in receive stream (data bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00323
00324 CRC = self.ser.read(2)
00325 if len(CRC) != 2:
00326 time.sleep(5)
00327 self.ser.flushInput()
00328 raise M5e_ReceiveError('Error in receive stream (CRC bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00329
00330
00331 validateCRC = length + command + status + data
00332 if self.CalculateCRC(validateCRC) != CRC:
00333 raise M5e_CRCError('Received response CRC failed')
00334
00335
00336 if status != '\x00\x00':
00337 raise M5e_CommandStatusError('Received response returned non-zero status',status)
00338
00339 return (start, length, command, status, data, CRC)
00340
00341 def QueryEnvironment(self, timeout=50):
00342
00343 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00344 timeoutLowByte = chr(timeout & 0x00FF)
00345 try:
00346 self.TransmitCommand('\x04\x22\x00\x00'+timeoutHighByte+timeoutLowByte)
00347 self.ReceiveResponse()
00348 except M5e_CommandStatusError, inst:
00349 flag = False
00350
00351
00352 if inst[1] == '\x04\x00':
00353 return []
00354 else:
00355 raise
00356
00357
00358
00359 self.TransmitCommand('\x00\x29')
00360 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00361
00362 readIndex = (ord(data[0]) << 8) + ord(data[1])
00363 writeIndex = (ord(data[2]) << 8) + ord(data[3])
00364 numTags = writeIndex - readIndex
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381 results = []
00382 while numTags > 0:
00383 self.TransmitCommand('\x03\x29\x00\x02\x00')
00384 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00385
00386 tagsRetrieved = ord(data[3])
00387 for i in xrange(tagsRetrieved):
00388 rssi = ord(data[4 + i*19])
00389 tagID = data[4 + i*19 + 5 : 4 + i*19 + 5 + 12]
00390 results.append( (tagID, rssi) )
00391
00392 numTags = numTags - tagsRetrieved
00393
00394
00395 self.TransmitCommand('\x00\x2A')
00396 self.ReceiveResponse()
00397
00398 return results
00399
00400 def TrackSingleTag(self, tagID, timeout=50, safe_response = True):
00401
00402 if len(tagID) != 12:
00403 raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00404
00405 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00406 timeoutLowByte = chr(timeout & 0x00FF)
00407 try:
00408 self.TransmitCommand('\x12\x21'+timeoutHighByte+timeoutLowByte+'\x11\x00\x02\x60'+tagID)
00409 if safe_response:
00410 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00411
00412 else:
00413 return self.QuickTrackResponse()
00414
00415 except M5e_CommandStatusError, inst:
00416 flag = False
00417
00418
00419 if inst[1] == '\x04\x00':
00420 return -1
00421 else:
00422 raise
00423
00424 return ord(data[3])
00425
00426 def ChangeTagID(self, newTagID, timeout=50):
00427
00428
00429
00430 if len(newTagID) != 12:
00431 raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00432
00433 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00434 timeoutLowByte = chr(timeout & 0x00FF)
00435 try:
00436 cmdLen = chr(len(newTagID)+4)
00437 self.TransmitCommand(cmdLen+'\x23'+timeoutHighByte+timeoutLowByte+'\x00\x00'+newTagID)
00438 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00439 except M5e_CommandStatusError, inst:
00440 return False
00441
00442 return True
00443
00444
00445 class M5e_error(Exception):
00446 "General Exception"
00447 pass
00448
00449 class M5e_SerialPortError(M5e_error):
00450 "If pyserial throws error"
00451 pass
00452
00453 class M5e_CRCError(M5e_error):
00454 "If CRC check from Mercury packet is incorrect (data corruption)"
00455 pass
00456
00457 class M5e_CommandStatusError(M5e_error):
00458 "If return response from Mercury is non-zero (error status)"
00459 pass
00460
00461 class M5e_TransmitTimeoutExceeded(M5e_error):
00462 "Something happened (probably power failure) to disable serial transmission"
00463 pass
00464
00465 class M5e_ReceiveError(M5e_error):
00466 "Serial input out of synch. Try waiting a few seconds, flush input stream, and reissue command"
00467 pass
00468
00469
00470
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
00484
00485
00486
00487
00488
00489 if __name__ == '__main__':
00490 import time
00491
00492 def P1(M5e):
00493 M5e.ChangeAntennaPorts(1,1)
00494 return 'AntPort1'
00495
00496 def P2(M5e):
00497 M5e.ChangeAntennaPorts(2,2)
00498 return 'AntPort2'
00499
00500 def PrintDatum(data):
00501 ant, ids, rssi = data
00502 print data
00503
00504 print 'Starting with read power 2300 centi-dBm. Change to 3000 for M5e-full'
00505 r = M5e(readPwr=2300)
00506 q = M5e_Poller(r, antfuncs=[P1, P2], callbacks=[PrintDatum])
00507
00508 q.query_mode()
00509
00510 t0 = time.time()
00511 while time.time() - t0 < 3.0:
00512 blah = 0
00513
00514 q.track_mode('RedBottle ')
00515
00516 t0 = time.time()
00517 while time.time() - t0 < 3.0:
00518 blah = 0
00519
00520 q.stop()