selfdrive/car: OBD callback (#33200)

* finish

* pass empty cb

* last fix!

* i keep messing this up

* even more coverage!

* not needed

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)})

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
 

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)

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()

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)

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