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 ReceiveResponse(self):
00259 "Receives a single command's response"
00260
00261 timeoutsToWait = 5
00262 timeoutsWaited = 0
00263
00264 while timeoutsWaited < timeoutsToWait:
00265 start = self.ser.read()
00266 if start == '\xFF':
00267 break
00268 timeoutsWaited += 1
00269
00270 if start != '\xFF':
00271 time.sleep(5)
00272 self.ser.flushInput()
00273 raise M5e_ReceiveError('Error in receive stream (start byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00274
00275 length = self.ser.read()
00276 if len(length) != 1:
00277 time.sleep(5)
00278 self.ser.flushInput()
00279 raise M5e_ReceiveError('Error in receive stream (length byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00280
00281 command = self.ser.read()
00282 if len(command) != 1:
00283 time.sleep(5)
00284 self.ser.flushInput()
00285 raise M5e_ReceiveError('Error in receive stream (command byte). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00286
00287 status = self.ser.read(2)
00288 if len(status) != 2:
00289 time.sleep(5)
00290 self.ser.flushInput()
00291 raise M5e_ReceiveError('Error in receive stream (status bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00292
00293 data = self.ser.read(ord(length))
00294 if len(data) != ord(length):
00295 time.sleep(5)
00296 self.ser.flushInput()
00297 raise M5e_ReceiveError('Error in receive stream (data bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00298
00299 CRC = self.ser.read(2)
00300 if len(CRC) != 2:
00301 time.sleep(5)
00302 self.ser.flushInput()
00303 raise M5e_ReceiveError('Error in receive stream (CRC bytes). Waited 5 seconds then flushed input. Try reissueing command and receive response.')
00304
00305
00306 validateCRC = length + command + status + data
00307 if self.CalculateCRC(validateCRC) != CRC:
00308 raise M5e_CRCError('Received response CRC failed')
00309
00310
00311 if status != '\x00\x00':
00312 raise M5e_CommandStatusError('Received response returned non-zero status',status)
00313
00314 return (start, length, command, status, data, CRC)
00315
00316 def QueryEnvironment(self, timeout=50):
00317
00318 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00319 timeoutLowByte = chr(timeout & 0x00FF)
00320 try:
00321 self.TransmitCommand('\x04\x22\x00\x00'+timeoutHighByte+timeoutLowByte)
00322 self.ReceiveResponse()
00323 except M5e_CommandStatusError, inst:
00324 flag = False
00325
00326
00327 if inst[1] == '\x04\x00':
00328 return []
00329 else:
00330 raise
00331
00332
00333
00334 self.TransmitCommand('\x00\x29')
00335 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00336
00337 readIndex = (ord(data[0]) << 8) + ord(data[1])
00338 writeIndex = (ord(data[2]) << 8) + ord(data[3])
00339 numTags = writeIndex - readIndex
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356 results = []
00357 while numTags > 0:
00358 self.TransmitCommand('\x03\x29\x00\x02\x00')
00359 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00360
00361 tagsRetrieved = ord(data[3])
00362 for i in xrange(tagsRetrieved):
00363 rssi = ord(data[4 + i*19])
00364 tagID = data[4 + i*19 + 5 : 4 + i*19 + 5 + 12]
00365 results.append( (tagID, rssi) )
00366
00367 numTags = numTags - tagsRetrieved
00368
00369
00370 self.TransmitCommand('\x00\x2A')
00371 self.ReceiveResponse()
00372
00373 return results
00374
00375 def TrackSingleTag(self, tagID, timeout=50):
00376
00377 if len(tagID) != 12:
00378 raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00379
00380 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00381 timeoutLowByte = chr(timeout & 0x00FF)
00382 try:
00383 self.TransmitCommand('\x12\x21'+timeoutHighByte+timeoutLowByte+'\x11\x00\x02\x60'+tagID)
00384 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00385
00386 except M5e_CommandStatusError, inst:
00387 flag = False
00388
00389
00390 if inst[1] == '\x04\x00':
00391 return -1
00392 else:
00393 raise
00394
00395 return ord(data[3])
00396
00397 def ChangeTagID(self, newTagID, timeout=50):
00398
00399
00400
00401 if len(newTagID) != 12:
00402 raise M5e_Error('Only 96-bit tags supported by TrackSingleTag')
00403
00404 timeoutHighByte = chr((timeout & 0xFFFF) >> 8)
00405 timeoutLowByte = chr(timeout & 0x00FF)
00406 try:
00407 cmdLen = chr(len(newTagID)+4)
00408 self.TransmitCommand(cmdLen+'\x23'+timeoutHighByte+timeoutLowByte+'\x00\x00'+newTagID)
00409 (start, length, command, status, data, CRC) = self.ReceiveResponse()
00410 except M5e_CommandStatusError, inst:
00411 return False
00412
00413 return True
00414
00415
00416 class M5e_error(Exception):
00417 "General Exception"
00418 pass
00419
00420 class M5e_SerialPortError(M5e_error):
00421 "If pyserial throws error"
00422 pass
00423
00424 class M5e_CRCError(M5e_error):
00425 "If CRC check from Mercury packet is incorrect (data corruption)"
00426 pass
00427
00428 class M5e_CommandStatusError(M5e_error):
00429 "If return response from Mercury is non-zero (error status)"
00430 pass
00431
00432 class M5e_TransmitTimeoutExceeded(M5e_error):
00433 "Something happened (probably power failure) to disable serial transmission"
00434 pass
00435
00436 class M5e_ReceiveError(M5e_error):
00437 "Serial input out of synch. Try waiting a few seconds, flush input stream, and reissue command"
00438 pass
00439
00440
00441
00442
00443
00444
00445
00446
00447
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460 if __name__ == '__main__':
00461 import time
00462
00463 def P1(M5e):
00464 M5e.ChangeAntennaPorts(1,1)
00465 return 'AntPort1'
00466
00467 def P2(M5e):
00468 M5e.ChangeAntennaPorts(2,2)
00469 return 'AntPort2'
00470
00471 def PrintDatum(data):
00472 ant, ids, rssi = data
00473 print data
00474
00475 print 'Starting with read power 2300 centi-dBm. Change to 3000 for M5e-full'
00476 r = M5e(readPwr=2300)
00477 q = M5e_Poller(r, antfuncs=[P1, P2], callbacks=[PrintDatum])
00478
00479 q.query_mode()
00480
00481 t0 = time.time()
00482 while time.time() - t0 < 3.0:
00483 blah = 0
00484
00485 q.track_mode('RedBottle ')
00486
00487 t0 = time.time()
00488 while time.time() - t0 < 3.0:
00489 blah = 0
00490
00491 q.stop()