Jason Shuler
3 years ago
commit
b9ca4d2854
19 changed files with 185 additions and 46 deletions
@ -1 +1 @@ |
||||
Subproject commit 44f048bc1f58ae9e28dfdeb98e40aea3e0f2b699 |
||||
Subproject commit 27a0d8a776fc8c1eaf8608d17ce81a00136f8bd0 |
||||
@ -0,0 +1,90 @@ |
||||
#!/usr/bin/env python3 |
||||
import capnp |
||||
import time |
||||
import traceback |
||||
from typing import Optional, Set, Tuple |
||||
|
||||
import cereal.messaging as messaging |
||||
from panda.python.uds import SERVICE_TYPE |
||||
from selfdrive.car import make_can_msg |
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp |
||||
from system.swaglog import cloudlog |
||||
|
||||
|
||||
def make_tester_present_msg(addr, bus, subaddr=None): |
||||
dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0] |
||||
if subaddr is not None: |
||||
dat.insert(0, subaddr) |
||||
|
||||
dat.extend([0x0] * (8 - len(dat))) |
||||
return make_can_msg(addr, bytes(dat), bus) |
||||
|
||||
|
||||
def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool: |
||||
# ISO-TP messages are always padded to 8 bytes |
||||
# tester present response is always a single frame |
||||
dat_offset = 1 if subaddr is not None else 0 |
||||
if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7: |
||||
# success response |
||||
if msg.dat[dat_offset + 1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40): |
||||
return True |
||||
# error response |
||||
if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == SERVICE_TYPE.TESTER_PRESENT: |
||||
return True |
||||
return False |
||||
|
||||
|
||||
def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]: |
||||
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] |
||||
queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list} |
||||
responses = queries |
||||
return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) |
||||
|
||||
|
||||
def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]], |
||||
responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]: |
||||
ecu_responses: Set[Tuple[int, Optional[int], int]] = set() # set((addr, subaddr, bus),) |
||||
try: |
||||
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries] |
||||
|
||||
messaging.drain_sock_raw(logcan) |
||||
sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) |
||||
start_time = time.monotonic() |
||||
while time.monotonic() - start_time < timeout: |
||||
can_packets = messaging.drain_sock(logcan, wait_for_one=True) |
||||
for packet in can_packets: |
||||
for msg in packet.can: |
||||
subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0] |
||||
if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr): |
||||
if debug: |
||||
print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}") |
||||
if (msg.address, subaddr, msg.src) in ecu_responses: |
||||
print(f"Duplicate ECU address: {hex(msg.address)}") |
||||
ecu_responses.add((msg.address, subaddr, msg.src)) |
||||
except Exception: |
||||
cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}") |
||||
return ecu_responses |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
import argparse |
||||
|
||||
parser = argparse.ArgumentParser(description='Get addresses of all ECUs') |
||||
parser.add_argument('--debug', action='store_true') |
||||
args = parser.parse_args() |
||||
|
||||
logcan = messaging.sub_sock('can') |
||||
sendcan = messaging.pub_sock('sendcan') |
||||
|
||||
time.sleep(1.0) |
||||
|
||||
print("Getting ECU addresses ...") |
||||
ecu_addrs = get_all_ecu_addrs(logcan, sendcan, 1, debug=args.debug) |
||||
|
||||
print() |
||||
print("Found ECUs on addresses:") |
||||
for addr, subaddr, bus in ecu_addrs: |
||||
msg = f" 0x{hex(addr)}" |
||||
if subaddr is not None: |
||||
msg += f" (sub-address: 0x{hex(subaddr)})" |
||||
print(msg) |
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:4c2cb3a3054f3292bbe538d6b793908dc2e234c200802d41b6766d3cb51b0b44 |
||||
size 101662751 |
||||
oid sha256:027cbb1fabae369878271cb0e3505071a8bdaa07473fad9a0b2e8d695c5dc1ff |
||||
size 76725611 |
||||
|
||||
@ -1,3 +1,3 @@ |
||||
version https://git-lfs.github.com/spec/v1 |
||||
oid sha256:96b60d0bfd1386c93b4f79195aa1c5e77b23e0250578a308ee2c58857ed5eb49 |
||||
size 102570834 |
||||
oid sha256:484976ea5bd4ddcabc82e95faf30d7311a27802c1e337472558699fa2395a499 |
||||
size 77472267 |
||||
|
||||
@ -1 +1 @@ |
||||
5434b3c1696554e9a889e77f794d80cd1cb0a7ec |
||||
df0ce74929dd6b5fa7a55224baefeff4bac6d785 |
||||
|
||||
Loading…
Reference in new issue