diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 999bf2b5e5..9e2e44de62 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -8,7 +8,7 @@ from openpilot.selfdrive.car import carlog from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN -from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing +from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car from openpilot.selfdrive.car.mock.values import CAR as MOCK import cereal.messaging as messaging from openpilot.selfdrive.car import gen_empty_fingerprint @@ -90,7 +90,7 @@ def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: # **** for use live only **** -def fingerprint(logcan, sendcan, num_pandas): +def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas): 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) @@ -115,11 +115,11 @@ def fingerprint(logcan, sendcan, num_pandas): carlog.warning("Getting VIN & FW versions") # enable OBD multiplexing for VIN query # NOTE: this takes ~0.1s and is relied on to allow sendcan subscriber to connect in time - set_obd_multiplexing(params, True) + 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, num_pandas=num_pandas) - car_fw = get_fw_versions_ordered(logcan, sendcan, vin, ecu_rx_addrs, num_pandas=num_pandas) + 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) cached = False exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin) @@ -134,7 +134,7 @@ def fingerprint(logcan, sendcan, num_pandas): carlog.warning("VIN %s", vin) # disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts - set_obd_multiplexing(params, False) + set_obd_multiplexing(False) params.put_bool("FirmwareQueryDone", True) fw_query_time = time.monotonic() - start_time @@ -169,8 +169,8 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, experimental_long_allowed, num_pandas=1): - candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, num_pandas) +def get_car(logcan, sendcan, set_obd_multiplexing, experimental_long_allowed, num_pandas=1): + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas) 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 929d98fd9e..2aa5012dc2 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -26,6 +26,17 @@ EventName = car.CarEvent.EventName carlog.addHandler(ForwardingHandler(cloudlog)) +def obd_callback(params: Params): + def set_obd_multiplexing(obd_multiplexing: bool): + if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: + cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") + params.remove("ObdMultiplexingChanged") + params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) + params.get_bool("ObdMultiplexingChanged", block=True) + cloudlog.warning("OBD multiplexing set successfully") + return set_obd_multiplexing + + class Car: CI: CarInterfaceBase @@ -49,9 +60,9 @@ class Car: print("Waiting for CAN messages...") get_one_can(self.can_sock) - num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") - self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], experimental_long_allowed, num_pandas) + num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) + self.CI, self.CP = get_car(self.can_sock, self.pm.sock['sendcan'], obd_callback(self.params), experimental_long_allowed, num_pandas) else: self.CI, self.CP = CI, CI.CP diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index 2c79a4f633..e36c1dc981 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -62,7 +62,7 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que if __name__ == "__main__": import argparse from openpilot.common.params import Params - from openpilot.selfdrive.car.fw_versions import set_obd_multiplexing + from openpilot.selfdrive.car.card import obd_callback parser = argparse.ArgumentParser(description='Get addresses of all ECUs') parser.add_argument('--debug', action='store_true') @@ -81,7 +81,7 @@ if __name__ == "__main__": time.sleep(0.2) # thread is 10 Hz params.put_bool("IsOnroad", True) - set_obd_multiplexing(params, not args.no_obd) + obd_callback(params)(not args.no_obd) print("Getting ECU addresses ...") ecu_addrs = _get_all_ecu_addrs(logcan, sendcan, args.bus, args.timeout, debug=args.debug) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 317c5ca1f9..ef9a1e24d4 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from collections import defaultdict -from collections.abc import Iterator +from collections.abc import Callable, Iterator from typing import Any, Protocol, TypeVar from tqdm import tqdm @@ -27,6 +27,7 @@ MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e} REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests] T = TypeVar('T') +ObdCallback = Callable[[bool], None] def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]: @@ -171,8 +172,7 @@ def match_fw_to_car(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], vi return True, set() -def get_present_ecus(logcan, sendcan, num_pandas: int = 1) -> set[EcuAddrBusType]: - params = Params() +def get_present_ecus(logcan, sendcan, 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: []} @@ -204,7 +204,7 @@ def get_present_ecus(logcan, sendcan, num_pandas: int = 1) -> set[EcuAddrBusType ecu_responses = set() for obd_multiplexing in queries: - set_obd_multiplexing(params, obd_multiplexing) + 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)) return ecu_responses @@ -228,17 +228,8 @@ def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[Ad return brand_matches -def set_obd_multiplexing(params: Params, obd_multiplexing: bool): - if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: - carlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") - params.remove("ObdMultiplexingChanged") - params.put_bool("ObdMultiplexingEnabled", obd_multiplexing) - params.get_bool("ObdMultiplexingChanged", block=True) - carlog.warning("OBD multiplexing set successfully") - - -def get_fw_versions_ordered(logcan, sendcan, 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, 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]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -249,7 +240,7 @@ def get_fw_versions_ordered(logcan, sendcan, vin: str, ecu_rx_addrs: set[EcuAddr if not len(brand_matches[brand]): continue - car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress) + car_fw = get_fw_versions(logcan, sendcan, 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 @@ -260,10 +251,9 @@ def get_fw_versions_ordered(logcan, sendcan, vin: str, ecu_rx_addrs: set[EcuAddr return all_car_fw -def get_fw_versions(logcan, sendcan, 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, 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]: versions = VERSIONS.copy() - params = Params() if query_brand is not None: versions = {query_brand: versions[query_brand]} @@ -305,7 +295,7 @@ def get_fw_versions(logcan, sendcan, query_brand: str = None, extra: OfflineFwVe # Toggle OBD multiplexing for each request if r.bus % 4 == 1: - set_obd_multiplexing(params, r.obd_multiplexing) + set_obd_multiplexing(r.obd_multiplexing) try: query_addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and @@ -340,7 +330,9 @@ if __name__ == "__main__": import time import argparse import cereal.messaging as messaging + from openpilot.common.params import Params from openpilot.selfdrive.car.vin import get_vin + from openpilot.selfdrive.car.card import obd_callback parser = argparse.ArgumentParser(description='Get firmware version of ECUs') parser.add_argument('--scan', action='store_true') @@ -358,6 +350,7 @@ if __name__ == "__main__": params.put_bool("IsOnroad", False) time.sleep(0.2) # thread is 10 Hz params.put_bool("IsOnroad", True) + set_obd_multiplexing = obd_callback(params) extra: Any = None if args.scan: @@ -373,14 +366,14 @@ if __name__ == "__main__": t = time.time() print("Getting vin...") - set_obd_multiplexing(params, True) + set_obd_multiplexing(True) vin_rx_addr, vin_rx_bus, vin = get_vin(logcan, sendcan, (0, 1), debug=args.debug) print(f'RX: {hex(vin_rx_addr)}, BUS: {vin_rx_bus}, VIN: {vin}') print(f"Getting VIN took {time.time() - t:.3f} s") print() t = time.time() - fw_vers = get_fw_versions(logcan, sendcan, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True) + fw_vers = get_fw_versions(logcan, sendcan, set_obd_multiplexing, query_brand=args.brand, extra=extra, num_pandas=num_pandas, debug=args.debug, progress=True) _, candidates = match_fw_to_car(fw_vers, vin) print() diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index 02e13d3ab9..f5283acb1f 100644 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -9,7 +9,7 @@ from openpilot.selfdrive.car import make_can_msg from openpilot.selfdrive.car.car_helpers import interfaces from openpilot.selfdrive.car.fingerprints import FW_VERSIONS from openpilot.selfdrive.car.fw_versions import ESSENTIAL_ECUS, FW_QUERY_CONFIGS, FUZZY_EXCLUDE_ECUS, VERSIONS, build_fw_dict, \ - match_fw_to_car, get_brand_ecu_matches, get_fw_versions, get_present_ecus + match_fw_to_car, get_brand_ecu_matches, get_fw_versions, get_fw_versions_ordered, get_present_ecus from openpilot.selfdrive.car.vin import get_vin from openpilot.selfdrive.pandad import can_list_to_can_capnp @@ -214,7 +214,7 @@ class TestFwFingerprintTiming: current_obd_multiplexing: bool total_time: float - def fake_set_obd_multiplexing(self, _, obd_multiplexing): + def fake_set_obd_multiplexing(self, obd_multiplexing): """The 10Hz blocking params loop adds on average 50ms to the query time for each OBD multiplexing change""" if obd_multiplexing != self.current_obd_multiplexing: self.current_obd_multiplexing = obd_multiplexing @@ -227,14 +227,13 @@ class TestFwFingerprintTiming: def _benchmark_brand(self, brand, num_pandas, mocker): fake_socket = FakeSocket() self.total_time = 0 - mocker.patch("openpilot.selfdrive.car.fw_versions.set_obd_multiplexing", self.fake_set_obd_multiplexing) mocker.patch("openpilot.selfdrive.car.isotp_parallel_query.IsoTpParallelQuery.get_data", self.fake_get_data) for _ in range(self.N): # Treat each brand as the most likely (aka, the first) brand with OBD multiplexing initially on self.current_obd_multiplexing = True t = time.perf_counter() - get_fw_versions(fake_socket, fake_socket, brand, num_pandas=num_pandas) + get_fw_versions(fake_socket, fake_socket, self.fake_set_obd_multiplexing, brand, num_pandas=num_pandas) self.total_time += time.perf_counter() - t return self.total_time / self.N @@ -254,11 +253,10 @@ class TestFwFingerprintTiming: fake_socket = FakeSocket() self.total_time = 0.0 - mocker.patch("openpilot.selfdrive.car.fw_versions.set_obd_multiplexing", self.fake_set_obd_multiplexing) mocker.patch("openpilot.selfdrive.car.fw_versions.get_ecu_addrs", fake_get_ecu_addrs) for _ in range(self.N): self.current_obd_multiplexing = True - get_present_ecus(fake_socket, fake_socket, num_pandas=2) + get_present_ecus(fake_socket, fake_socket, self.fake_set_obd_multiplexing, num_pandas=2) self._assert_timing(self.total_time / self.N, present_ecu_ref_time) print(f'get_present_ecus, query time={self.total_time / self.N} seconds') @@ -325,9 +323,9 @@ class TestFwFingerprintTiming: def fake_carlog_exception(*args, **kwargs): raise - mocker.patch("openpilot.selfdrive.car.fw_versions.set_obd_multiplexing", lambda *args: None) mocker.patch("openpilot.selfdrive.car.carlog.exception", fake_carlog_exception) fake_socket = FakeSocket() + get_fw_versions_ordered(fake_socket, fake_socket, lambda obd: None, '0' * 17, set()) for brand in FW_QUERY_CONFIGS.keys(): with subtests.test(brand=brand): - get_fw_versions(fake_socket, fake_socket, brand, num_pandas=1) + get_fw_versions(fake_socket, fake_socket, lambda obd: None, brand) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 1731bf2e1e..0c9f98e647 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -355,7 +355,7 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) - _, CP = get_car(can, sendcan, Params().get_bool("ExperimentalLongitudinalEnabled")) + _, CP = get_car(can, sendcan, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled")) if not params.get_bool("DisengageOnAccelerator"): CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS