retropilot
/
panda
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

fix rx message filtering bug

master
Greg Hogan 2018-12-17 00:45:10 -08:00
parent 80fb6a6fa0
commit b1a3195770
1 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
python/uds.py
View File

@ -277,26 +277,26 @@ class UdsClient():
self.rx_queue = Queue()
self.debug = debug
self.can_reader_t = threading.Thread(target=self._isotp_thread, args=(self.panda, self.bus, self.tx_addr, self.tx_queue, self.rx_queue, self.debug))
self.can_reader_t = threading.Thread(target=self._isotp_thread, args=(self.debug,))
self.can_reader_t.daemon = True
self.can_reader_t.start()
def _isotp_thread(self, panda, bus, tx_addr, tx_queue, rx_queue, debug):
def _isotp_thread(self, debug):
try:
rx_frame = {"size": 0, "data": "", "idx": 0, "done": True}
tx_frame = {"size": 0, "data": "", "idx": 0, "done": True}
# allow all output
panda.set_safety_mode(0x1337)
self.panda.set_safety_mode(0x1337)
# clear tx buffer
panda.can_clear(bus)
self.panda.can_clear(self.bus)
# clear rx buffer
panda.can_clear(0xFFFF)
self.panda.can_clear(0xFFFF)
time.sleep(1)
while True:
messages = panda.can_recv()
messages = self.panda.can_recv()
for rx_addr, rx_ts, rx_data, rx_bus in messages:
if rx_bus != bus or rx_addr != rx_addr or len(rx_data) == 0:
if rx_bus != self.bus or rx_addr != self.rx_addr or len(rx_data) == 0:
continue
if (debug): print("R: {} {}".format(hex(rx_addr), hexlify(rx_data)))
@ -306,7 +306,7 @@ class UdsClient():
rx_frame["data"] = rx_data[1:1+rx_frame["size"]]
rx_frame["idx"] = 0
rx_frame["done"] = True
rx_queue.put(rx_frame["data"])
self.rx_queue.put(rx_frame["data"])
elif rx_data[0] >> 4 == 0x1:
# first rx_frame
rx_frame["size"] = ((rx_data[0] & 0x0F) << 8) + rx_data[1]
@ -315,8 +315,8 @@ class UdsClient():
rx_frame["done"] = False
# send flow control message (send all bytes)
msg = "\x30\x00\x00".ljust(8, "\x00")
if (debug): print("S: {} {}".format(hex(tx_addr), hexlify(msg)))
panda.can_send(tx_addr, msg, bus)
if (debug): print("S: {} {}".format(hex(self.tx_addr), hexlify(msg)))
self.panda.can_send(self.tx_addr, msg, self.bus)
elif rx_data[0] >> 4 == 0x2:
# consecutive rx frame
assert rx_frame["done"] == False, "rx: no active frame"
@ -327,7 +327,7 @@ class UdsClient():
rx_frame["data"] += rx_data[1:1+min(rx_size, 7)]
if rx_frame["size"] == len(rx_frame["data"]):
rx_frame["done"] = True
rx_queue.put(rx_frame["data"])
self.rx_queue.put(rx_frame["data"])
elif rx_data[0] >> 4 == 0x3:
# flow control
assert tx_frame["done"] == False, "tx: no active frame"
@ -344,14 +344,14 @@ class UdsClient():
tx_frame["idx"] += 1
# consecutive tx frames
msg = (chr(0x20 | (tx_frame["idx"] & 0xF)) + tx_frame["data"][i:i+7]).ljust(8, "\x00")
if (debug): print("S: {} {}".format(hex(tx_addr), hexlify(msg)))
panda.can_send(tx_addr, msg, bus)
if (debug): print("S: {} {}".format(hex(self.tx_addr), hexlify(msg)))
self.panda.can_send(self.tx_addr, msg, self.bus)
if delay_ts > 0:
time.sleep(delay_ts / delay_div)
tx_frame["done"] = True
if not tx_queue.empty():
req = tx_queue.get(block=False)
if not self.tx_queue.empty():
req = self.tx_queue.get(block=False)
# reset rx and tx frames
rx_frame = {"size": 0, "data": "", "idx": 0, "done": True}
tx_frame = {"size": len(req), "data": req, "idx": 0, "done": False}
@ -359,19 +359,19 @@ class UdsClient():
# single frame
tx_frame["done"] = True
msg = (chr(tx_frame["size"]) + tx_frame["data"]).ljust(8, "\x00")
if (debug): print("S: {} {}".format(hex(tx_addr), hexlify(msg)))
panda.can_send(tx_addr, msg, bus)
if (debug): print("S: {} {}".format(hex(self.tx_addr), hexlify(msg)))
self.panda.can_send(self.tx_addr, msg, self.bus)
else:
# first rx_frame
tx_frame["done"] = False
msg = (struct.pack("!H", 0x1000 | tx_frame["size"]) + tx_frame["data"][:6]).ljust(8, "\x00")
if (debug): print("S: {} {}".format(hex(tx_addr), hexlify(msg)))
panda.can_send(tx_addr, msg, bus)
if (debug): print("S: {} {}".format(hex(self.tx_addr), hexlify(msg)))
self.panda.can_send(self.tx_addr, msg, self.bus)
else:
time.sleep(0.01)
finally:
panda.close()
rx_queue.put(None)
self.panda.close()
self.rx_queue.put(None)
# generic uds request
def _uds_request(self, service_type, subfunction=None, data=None):