crashtest-r0ket/tools/game/r0ketrem0te/bridge.py

241 lines
7.4 KiB
Python
Raw Normal View History

2011-12-12 17:02:24 +00:00
import serialinterface
import threading
import Queue
import crcmod
2011-12-14 16:59:32 +00:00
import packets
2011-12-16 02:04:31 +00:00
import traceback
2011-12-12 17:02:24 +00:00
2011-12-14 16:59:32 +00:00
class QueuePacket:
def __init__(self, channel, mac, acked, packet, callback=None):
2011-12-14 16:59:32 +00:00
self.channel = channel
self.mac = mac
self.acked = acked
self.packet = packet
2011-12-15 18:09:05 +00:00
self.priority = packet.priority
2011-12-14 16:59:32 +00:00
self.retriesleft = 5
self.timeout = 0.1
self.timer = None
self.timedout = False
self.lock = threading.RLock()
self.isdone = False
self.callback = callback
2011-12-14 16:59:32 +00:00
def __cmp__(self, other):
if not isinstance(other,QueuePacket):
return 1
if self.priority < other.priority:
return -1
if self.priority > other.priority:
return 1
return 0
def valid(self):
with self.lock:
return self.retriesleft > 0 and not self.acked
def sent(self, timeoutcallback):
with self.lock:
self.timedout = False
2011-12-15 18:09:05 +00:00
if self.retriesleft > 0:
self.retriesleft-=1
2011-12-14 16:59:32 +00:00
if self.acked:
self.timeoutcallback = timeoutcallback
self.timer = threading.Timer(self.timeout, self.timercallback)
self.timer.start()
elif self.callback:
self.callback('done')
2011-12-14 16:59:32 +00:00
def done(self):
with self.lock:
if self.timer != None:
self.timer.cancel()
self.timer = None
self.isdone = True
if self.callback:
self.callback('done')
2011-12-14 16:59:32 +00:00
def timercallback(self):
with self.lock:
self.timedout = True
self.timeoutcallback(self)
if retriesleft == 0:
self.callback('timeout')
2011-12-14 16:59:32 +00:00
class Bridge:
2011-12-15 18:09:05 +00:00
def __init__(self, path2device, channel, rxmac):
2011-12-12 17:02:24 +00:00
self.ser = serialinterface.SerialInterface(path2device, 115200, 0)
self.free = threading.Lock()
2011-12-14 16:59:32 +00:00
self.queueslock = threading.Lock()
self.packets = Queue.PriorityQueue()
self.outpackets = Queue.Queue()
2011-12-12 17:02:24 +00:00
self.crc = crcmod.predefined.mkCrcFun('crc-ccitt-false')
2011-12-14 16:59:32 +00:00
self.queues = {}
2011-12-15 18:09:05 +00:00
self.callbacks = []
2011-12-15 18:54:56 +00:00
self.ctrs = {}
2011-12-14 16:59:32 +00:00
self.reader = threading.Thread(target = self.readerThread)
self.reader.daemon = True
self.writer = threading.Thread(target = self.writerThread)
self.writer.daemon = True
self.writer.start()
self.reader.start()
self.packetlength = None
self.txmac = None
self.rxmac = None
self.channel = None
2011-12-15 18:09:05 +00:00
self.gameChannel = channel
2011-12-12 17:02:24 +00:00
2011-12-14 16:59:32 +00:00
self.setPacketLength(0x20)
2011-12-15 18:09:05 +00:00
self.setRxMAC(rxmac)
self.setChannel(channel)
self.ctr = 0
2011-12-15 18:54:56 +00:00
2011-12-15 18:09:05 +00:00
def registerCallback(self, callback):
if callback not in self.callbacks:
self.callbacks.append(callback)
2011-12-14 16:59:32 +00:00
def registerQueue(self, queue):
if queue not in self.queues:
self.queues[queue] = None
def putInQueue(self, queue, qp):
if queue in self.queues:
queue.put(qp);
self.checkQueues()
def processAck(self, ack):
#find the corresponding packet in the queues
found = False
for pq in self.queues.values():
if pq.packet.id == ack.id and pq.packet.ctr == ack.ctr:
#notify it
pq.done()
found = True
#notify the queue system
if found:
self.checkQueues()
else:
print "got an ack for an unknown packet"
def packetTimeout(self, qp):
self.checkQueues()
def checkQueues(self):
with self.queueslock:
for q in self.queues:
#check if a packet has to be resent
#remove it from the packet slot if it has been resent to often
qp = self.queues[q]
if qp != None:
if qp.valid():
self.queues[q] = None
elif qp.timedout:
print "packet timed out" + qp.packet
self.outpackets.put(qp)
#check if a idle queue has a new packet in line
qp = self.queues[q]
if qp == None and not q.empty():
qp = q.get()
2011-12-15 18:09:05 +00:00
if not isinstance(qp.packet,packets.Ack):
self.ctr+=1
qp.packet.ctr = self.ctr
2011-12-14 16:59:32 +00:00
self.queues[q] = qp
self.outpackets.put(qp)
def writerThread(self):
while True:
try:
#wait until we have packets to take care of
qp = self.outpackets.get()
#send it and notify the queuepacket
2011-12-15 18:09:05 +00:00
self.setTxMAC(qp.mac)
self.setChannel(qp.channel)
2011-12-15 18:09:05 +00:00
self.sendPacket(qp.packet)
2011-12-14 16:59:32 +00:00
qp.sent(self.packetTimeout)
self.setChannel(self.gameChannel)
2011-12-14 16:59:32 +00:00
except Exception as e:
print e
2011-12-16 02:04:31 +00:00
traceback.print_stack()
2011-12-12 17:02:24 +00:00
def readerThread(self):
while True:
try:
(command, data) = self.ser.readMessage()
if command == '1':
self.newPacket(data)
elif command == '2':
self.free.release()
except Exception as e:
print e
2011-12-16 02:04:31 +00:00
traceback.print_stack()
2011-12-12 17:02:24 +00:00
def newPacket(self, data):
2011-12-16 02:04:31 +00:00
print "received:", list(data)
2011-12-14 16:59:32 +00:00
crc = self.crc(data[:-2])
if data[-2:] == chr(crc>>8) + chr(crc&0xFF):
packet = packets.fromMessage(data)
print "received:", packet
2011-12-16 02:04:31 +00:00
if packet == None:
2011-12-15 18:54:56 +00:00
return
2011-12-16 02:04:31 +00:00
#if packet.id in self.ctrs and self.ctrs[packet.id] == packet.ctr:
# print 'ignoring duplicate'
# return
2011-12-14 16:59:32 +00:00
if isinstance(packet,packets.Ack):
self.ProcessAck(packet)
else:
2011-12-15 18:09:05 +00:00
for callback in self.callbacks:
callback(packet)
#self.packets.put(packet)
2011-12-12 17:02:24 +00:00
2011-12-15 18:09:05 +00:00
# def gotPacket(self):
# return not self.packets.empty()
2011-12-12 17:02:24 +00:00
2011-12-15 18:09:05 +00:00
# def getPacket(self):
# return self.packets.get()
2011-12-12 17:02:24 +00:00
def sendPacket(self, packet):
2011-12-14 16:59:32 +00:00
print 'sending', packet
data = packet.toMessage()
crc = self.crc(data)
data += chr(crc>>8);
data += chr(crc&0xFF);
self.free.acquire()
#print 'sending packet: len:', len(data), 'data:', list(data)
self.ser.writeMessage('1',data);
2011-12-12 17:02:24 +00:00
def setPacketLength(self, length):
if length == self.packetlength:
return
2011-12-12 17:02:24 +00:00
self.free.acquire()
self.ser.writeMessage('6', '%c'%length)
self.packetLength = length
2011-12-14 16:59:32 +00:00
def setTxMAC(self, mac):
2011-12-15 18:09:05 +00:00
if mac == self.txmac:
return
2011-12-14 16:59:32 +00:00
self.free.acquire()
2011-12-15 18:09:05 +00:00
print "setting tx mac", mac
2011-12-14 16:59:32 +00:00
self.ser.writeMessage('3', ''.join([chr(x) for x in mac]))
self.txmac = mac
2011-12-14 16:59:32 +00:00
def setRxMAC(self, mac):
if mac == self.rxmac:
return
2011-12-14 16:59:32 +00:00
self.free.acquire()
2011-12-15 18:09:05 +00:00
print "setting rx mac", mac
2011-12-14 16:59:32 +00:00
self.ser.writeMessage('4', ''.join([chr(x) for x in mac]))
self.rxmac = mac
2011-12-14 16:59:32 +00:00
def setChannel(self, channel):
if channel == self.channel:
return
2011-12-14 16:59:32 +00:00
self.free.acquire()
2011-12-15 18:09:05 +00:00
print "setting channel", channel
2011-12-14 16:59:32 +00:00
self.ser.writeMessage('5', '%c'%channel)
self.channel = channel
2011-12-14 16:59:32 +00:00