diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 1da53d4cfa..7776a47c64 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -193,8 +193,8 @@ def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, fric return friction -def make_can_msg(addr, dat, bus): - return [addr, dat, bus] +def make_can_msg(addr: int, dat: bytes, bus: int) -> tuple[int, bytes, int]: + return addr, dat, bus def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): diff --git a/selfdrive/car/can_definitions.py b/selfdrive/car/can_definitions.py new file mode 100644 index 0000000000..ab60aa40d5 --- /dev/null +++ b/selfdrive/car/can_definitions.py @@ -0,0 +1,3 @@ +from collections.abc import Callable + +CanSendCallable = Callable[[list[tuple[int, bytes, int]]], None] diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 5ee19214a3..921e4a5c96 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -48,9 +48,7 @@ def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: done = False while not done: - a = next_can() - - for can in a.can: + for can in next_can(): # The fingerprint dict is generated for all buses, this way the car interface # can use it to detect a (valid) multipanda setup and initialize accordingly if can.src < 128: @@ -81,7 +79,7 @@ def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: # **** for use live only **** -def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params_raw): +def fingerprint(logcan, can_send, set_obd_multiplexing, num_pandas, cached_params_raw): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False) @@ -107,9 +105,9 @@ def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params # NOTE: this takes ~0.1s and is relied on to allow sendcan subscriber to connect in time set_obd_multiplexing(True) # VIN query only reliably works through OBDII - vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1)) - ecu_rx_addrs = get_present_ecus(logcan, sendcan, set_obd_multiplexing, num_pandas=num_pandas) - car_fw = get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas) + vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, can_send, (0, 1)) + ecu_rx_addrs = get_present_ecus(logcan, can_send, set_obd_multiplexing, num_pandas=num_pandas) + car_fw = get_fw_versions_ordered(logcan, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas) cached = False exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin) @@ -158,8 +156,8 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, set_obd_multiplexing, experimental_long_allowed, num_pandas=1, cached_params=None): - candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params) +def get_car(logcan, can_send, set_obd_multiplexing, experimental_long_allowed, num_pandas=1, cached_params=None): + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, can_send, set_obd_multiplexing, num_pandas, cached_params) if candidate is None: carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)}) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 5c32fd391d..34eef027e2 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import os import time +from dataclasses import dataclass from types import SimpleNamespace import cereal.messaging as messaging @@ -14,7 +15,8 @@ from openpilot.common.realtime import config_realtime_process, Priority, Ratekee from openpilot.common.swaglog import cloudlog, ForwardingHandler from openpilot.selfdrive.pandad import can_list_to_can_capnp -from openpilot.selfdrive.car import DT_CTRL, carlog, make_can_msg +from openpilot.selfdrive.car import DT_CTRL, carlog +from openpilot.selfdrive.car.can_definitions import CanSendCallable from openpilot.selfdrive.car.fw_versions import ObdCallback from openpilot.selfdrive.car.car_helpers import get_car from openpilot.selfdrive.car.interfaces import CarInterfaceBase @@ -39,22 +41,33 @@ def obd_callback(params: Params) -> ObdCallback: return set_obd_multiplexing -def get_one_can(logcan): +@dataclass +class CanData: + address: int + dat: bytes + src: int + + +def get_one_can(logcan: messaging.SubSocket) -> list[CanData]: while True: can = messaging.recv_one_retry(logcan) if len(can.can) > 0: - return can + return [CanData(msg.address, msg.dat, msg.src) for msg in can.can] -def can_recv_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket): - def can_recv(wait_for_one: bool = False) -> list[list[int, bytes, int]]: # call rx/tx? - can_packets = messaging.drain_sock(logcan, wait_for_one=wait_for_one) - return [make_can_msg(msg.address, msg.dat, msg.src) for msg in can_packets] +def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket) -> tuple[SimpleNamespace, CanSendCallable]: + def can_drain(wait_for_one: bool = False) -> list[list[CanData]]: + ret = [] + for can in messaging.drain_sock(logcan, wait_for_one=wait_for_one): + ret.append([CanData(msg.address, msg.dat, msg.src) for msg in can.can]) + return ret - def can_send(msg: list[int, bytes, int]) -> None: - sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan')) + def can_send(msgs: list[tuple[int, bytes, int]]) -> None: + """Input is N messages as created by selfdrive.car.make_can_msg""" + sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) - return SimpleNamespace(drain=can_recv, get_one_can=lambda: get_one_can(logcan)), SimpleNamespace(send=can_send) + # TODO: do we want can_send as a function as well? remove SimpleNamespace? + return SimpleNamespace(drain=can_drain, get_one_can=lambda: get_one_can(logcan)), can_send class Car: @@ -83,7 +96,7 @@ class Car: experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) cached_params = self.params.get("CarParamsCache") - self.CI = get_car(*can_recv_callbacks(self.can_sock, self.pm.sock['sendcan']), obd_callback(self.params), + self.CI = get_car(*can_comm_callbacks(self.can_sock, self.pm.sock['sendcan']), obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) self.CP = self.CI.CP diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index e36c1dc981..a4dac5fb6f 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 import capnp import time +from types import SimpleNamespace -import cereal.messaging as messaging from panda.python.uds import SERVICE_TYPE from openpilot.selfdrive.car import make_tester_present_msg, carlog +from openpilot.selfdrive.car.can_definitions import CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType -from openpilot.selfdrive.pandad import can_list_to_can_capnp def _is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: int = None) -> bool: @@ -23,26 +23,26 @@ def _is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subad return False -def _get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]: +def _get_all_ecu_addrs(logcan: SimpleNamespace, can_send: CanSendCallable, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]: addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)] queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list} responses = queries - return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) + return get_ecu_addrs(logcan, can_send, queries, responses, timeout=timeout, debug=debug) -def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: set[EcuAddrBusType], +def get_ecu_addrs(logcan: SimpleNamespace, can_send: CanSendCallable, queries: set[EcuAddrBusType], responses: set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> set[EcuAddrBusType]: ecu_responses: set[EcuAddrBusType] = 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')) + logcan.drain() + can_send(msgs) start_time = time.monotonic() while time.monotonic() - start_time < timeout: - can_packets = messaging.drain_sock(logcan, wait_for_one=True) + can_packets = logcan.drain(wait_for_one=True) for packet in can_packets: - for msg in packet.can: + for msg in packet: if not len(msg.dat): carlog.warning("ECU addr scan: skipping empty remote frame") continue @@ -61,6 +61,7 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que if __name__ == "__main__": import argparse + import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.selfdrive.car.card import obd_callback diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 87f5f3defd..155cc9cbb4 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 from collections import defaultdict from collections.abc import Callable, Iterator +from types import SimpleNamespace from typing import Any, Protocol, TypeVar from tqdm import tqdm @@ -11,6 +12,7 @@ from cereal import car from openpilot.selfdrive.car import carlog from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs from openpilot.selfdrive.car.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.can_definitions import CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery @@ -171,7 +173,7 @@ def match_fw_to_car(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], vi return True, set() -def get_present_ecus(logcan, sendcan, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]: +def get_present_ecus(logcan: SimpleNamespace, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]: # queries are split by OBD multiplexing mode queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []} parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []} @@ -205,7 +207,7 @@ def get_present_ecus(logcan, sendcan, set_obd_multiplexing: ObdCallback, num_pan for obd_multiplexing in queries: set_obd_multiplexing(obd_multiplexing) for query in queries[obd_multiplexing]: - ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) + ecu_responses.update(get_ecu_addrs(logcan, can_send, set(query), responses, timeout=0.1)) return ecu_responses @@ -227,8 +229,9 @@ def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[Ad return brand_matches -def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, vin: str, ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, - num_pandas: int = 1, debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions_ordered(logcan: SimpleNamespace, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str, + ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, debug: bool = False, + progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -239,7 +242,7 @@ def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, if not len(brand_matches[brand]): continue - car_fw = get_fw_versions(logcan, sendcan, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress) + car_fw = get_fw_versions(logcan, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress) all_car_fw.extend(car_fw) # If there is a match using this brand's FW alone, finish querying early @@ -250,8 +253,9 @@ def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, return all_car_fw -def get_fw_versions(logcan, sendcan, set_obd_multiplexing: ObdCallback, query_brand: str = None, extra: OfflineFwVersions = None, timeout: float = 0.1, - num_pandas: int = 1, debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions(logcan: SimpleNamespace, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str = None, + extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, debug: bool = False, + progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: versions = VERSIONS.copy() if query_brand is not None: @@ -301,7 +305,7 @@ def get_fw_versions(logcan, sendcan, set_obd_multiplexing: ObdCallback, query_br (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] if query_addrs: - query = IsoTpParallelQuery(sendcan, logcan, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug) + query = IsoTpParallelQuery(can_send, logcan, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug) for (tx_addr, sub_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index c5f74a8156..813c6067c2 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -1,19 +1,19 @@ import time from collections import defaultdict from functools import partial +from types import SimpleNamespace -import cereal.messaging as messaging from openpilot.selfdrive.car import carlog +from openpilot.selfdrive.car.can_definitions import CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import AddrType -from openpilot.selfdrive.pandad import can_list_to_can_capnp from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr class IsoTpParallelQuery: - def __init__(self, sendcan: messaging.PubSocket, logcan: messaging.SubSocket, bus: int, addrs: list[int] | list[AddrType], + def __init__(self, can_send: CanSendCallable, logcan: SimpleNamespace, bus: int, addrs: list[int] | list[AddrType], request: list[bytes], response: list[bytes], response_offset: int = 0x8, functional_addrs: list[int] = None, debug: bool = False, response_pending_timeout: float = 10) -> None: - self.sendcan = sendcan + self.can_send = can_send self.logcan = logcan self.bus = bus self.request = request @@ -31,17 +31,17 @@ class IsoTpParallelQuery: def rx(self): """Drain can socket and sort messages into buffers based on address""" - can_packets = messaging.drain_sock(self.logcan, wait_for_one=True) + can_packets = self.logcan.drain(wait_for_one=True) for packet in can_packets: - for msg in packet.can: + for msg in packet: if msg.src == self.bus and msg.address in self.msg_addrs.values(): self.msg_buffer[msg.address].append((msg.address, msg.dat, msg.src)) - def _can_tx(self, tx_addr, dat, bus): + def _can_tx(self, tx_addr: int, dat: bytes, bus: int): """Helper function to send single message""" - msg = [tx_addr, dat, bus] - self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan')) + msg = (tx_addr, dat, bus) + self.can_send([msg]) def _can_rx(self, addr, sub_addr=None): """Helper function to retrieve message with specified address and subadress from buffer""" @@ -63,7 +63,7 @@ class IsoTpParallelQuery: return msgs def _drain_rx(self): - messaging.drain_sock_raw(self.logcan) + self.logcan.drain() self.msg_buffer = defaultdict(list) def _create_isotp_msg(self, tx_addr: int, sub_addr: int | None, rx_addr: int): diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index d662f9a7de..9a7538f60d 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,7 +1,6 @@ #!/usr/bin/env python3 import re -import cereal.messaging as messaging from panda.python.uds import get_rx_addr_for_tx_addr, FUNCTIONAL_ADDRS from openpilot.selfdrive.car import carlog from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery @@ -15,7 +14,7 @@ def is_valid_vin(vin: str): return re.fullmatch(VIN_RE, vin) is not None -def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): +def get_vin(logcan, can_send, buses, timeout=0.1, retry=2, debug=False): for i in range(retry): for bus in buses: for request, response, valid_buses, vin_addrs, functional_addrs, rx_offset in ( @@ -35,7 +34,7 @@ def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): tx_addrs = [a for a in range(0x700, 0x800) if a != 0x7DF] + list(range(0x18DA00F1, 0x18DB00F1, 0x100)) try: - query = IsoTpParallelQuery(sendcan, logcan, bus, tx_addrs, [request, ], [response, ], response_offset=rx_offset, + query = IsoTpParallelQuery(can_send, logcan, bus, tx_addrs, [request, ], [response, ], response_offset=rx_offset, functional_addrs=functional_addrs, debug=debug) results = query.get_data(timeout) @@ -63,6 +62,7 @@ def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): if __name__ == "__main__": import argparse import time + import cereal.messaging as messaging parser = argparse.ArgumentParser(description='Get VIN of the car') parser.add_argument('--debug', action='store_true')