boardd: ability to switch between ELM safety params (#27656)

* indecisive

* rename to generic FW query

* remove code and update comment

* we need this to start off, unless we set multiplexing immediately

* draft

* draft 2

* try that

* can't do this either, boardd might read Enabled after removing, but before setting new Request param

* this should work

* use one less param

* fix params

* match behavior (set all pandas to safety param of 1, disabling multiplexing for fingerprinting

* clean up (some tests may temp break)

* fix param name and sort

* time it

* yes it does matter

* add to hyundai's bus 5 query

* remove hyundai for now

* this should work

* clean up

* clean up

* flip argument around, clean up

* fix test_startup

* some clean up

* rm line

* comment makes more sense

* required typing

* clean up common type

* comments

* Update selfdrive/car/car_helpers.py

* line

* whoops, need to set before vin!

* fix debug

* annoying

* more debugging

* bug fix (needs both keys always)

* debuGG

debuGG

* Revert "debuGG"

This reverts commit 55b2f429324c0b92d5cfb2cabf8b20db1e166248.

* Revert "more debugging"

This reverts commit 02934c3403ad5270f03093508b704c151d1ccb2a.

* Revert "annoying"

This reverts commit 8b4e5e09989f9a0217e3ec1c0ba68735929b7366.

* clean that up

* bumpback

* bumpback

* every second write param

* flip

* stuff

* move up?

* fix timing out in CI

* rm
old-commit-hash: 42449b482d46565242341ca2d7e3a7255572f6a2

common/params.cc

@@ -111,7 +111,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
-    {"FirmwareObdQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
+    {"FirmwareQueryDone", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"ForcePowerDown", CLEAR_ON_MANAGER_START},
     {"GitBranch", PERSISTENT},
     {"GitCommit", PERSISTENT},
@@ -155,7 +155,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NavSettingTime24h", PERSISTENT},
     {"NavSettingLeftSide", PERSISTENT},
     {"NavdRender", PERSISTENT},
-    {"ObdMultiplexingDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
+    {"ObdMultiplexingChanged", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
+    {"ObdMultiplexingEnabled", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
     {"OpenpilotEnabledToggle", PERSISTENT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"PandaSignatures", CLEAR_ON_MANAGER_START},

selfdrive/boardd/boardd.cc

@@ -113,32 +113,35 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
     return false;
   }
 
-  // set to ELM327 for fingerprinting
+  // initialize to ELM327 without OBD multiplexing for fingerprinting
+  bool obd_multiplexing_enabled = false;
   for (int i = 0; i < pandas.size(); i++) {
-    const uint16_t safety_param = (i > 0) ? 1U : 0U;
-    pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
+    pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
   }
 
-  // wait for FW query at OBD port to finish
+  // openpilot can switch between multiplexing modes for different FW queries
   while (true) {
     if (do_exit || !check_all_connected(pandas) || !ignition) {
       return false;
     }
 
-    if (p.getBool("FirmwareObdQueryDone")) {
-      LOGW("finished FW query at OBD port");
+    bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled");
+    if (obd_multiplexing_requested != obd_multiplexing_enabled) {
+      const uint16_t safety_param = obd_multiplexing_requested ? 0U : 1U;
+      for (int i = 0; i < pandas.size(); i++) {
+        pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
+      }
+      obd_multiplexing_enabled = obd_multiplexing_requested;
+      p.putBool("ObdMultiplexingChanged", true);
+    }
+
+    if (p.getBool("FirmwareQueryDone")) {
+      LOGW("finished FW query");
       break;
     }
     util::sleep_for(20);
   }
 
-  // set to ELM327 to finish fingerprinting and for potential ECU knockouts
-  for (Panda *panda : pandas) {
-    panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
-  }
-
-  p.putBool("ObdMultiplexingDisabled", true);
-
   std::string params;
   LOGW("waiting for params to set safety model");
   while (true) {

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -51,7 +51,7 @@ class TestBoardd(unittest.TestCase):
     cp.safetyConfigs = [safety_config]*num_pandas
 
     params = Params()
-    params.put_bool("FirmwareObdQueryDone", True)
+    params.put_bool("FirmwareQueryDone", True)
     params.put_bool("ControlsReady", True)
     params.put("CarParams", cp.to_bytes())
 

selfdrive/car/car_helpers.py

@@ -8,7 +8,7 @@ from system.version import is_comma_remote, is_tested_branch
 from selfdrive.car.interfaces import get_interface_attr
 from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
 from selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
-from selfdrive.car.fw_versions import disable_obd_multiplexing, get_fw_versions_ordered, match_fw_to_car, get_present_ecus
+from selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car, set_obd_multiplexing
 from system.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.car import gen_empty_fingerprint
@@ -80,12 +80,13 @@ def fingerprint(logcan, sendcan, num_pandas):
   fixed_fingerprint = os.environ.get('FINGERPRINT', "")
   skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
   ecu_rx_addrs = set()
+  params = Params()
 
   if not skip_fw_query:
     # Vin query only reliably works through OBDII
     bus = 1
 
-    cached_params = Params().get("CarParamsCache")
+    cached_params = params.get("CarParamsCache")
     if cached_params is not None:
       cached_params = car.CarParams.from_bytes(cached_params)
       if cached_params.carName == "mock":
@@ -98,6 +99,7 @@ def fingerprint(logcan, sendcan, num_pandas):
       cached = True
     else:
       cloudlog.warning("Getting VIN & FW versions")
+      set_obd_multiplexing(params, True)
       vin_rx_addr, vin = get_vin(logcan, sendcan, bus)
       ecu_rx_addrs = get_present_ecus(logcan, sendcan, num_pandas=num_pandas)
       car_fw = get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, num_pandas=num_pandas)
@@ -113,10 +115,11 @@ def fingerprint(logcan, sendcan, num_pandas):
     cloudlog.event("Malformed VIN", vin=vin, error=True)
     vin = VIN_UNKNOWN
   cloudlog.warning("VIN %s", vin)
-
-  params = Params()
   params.put("CarVin", vin)
-  disable_obd_multiplexing(params)
+
+  # disable OBD multiplexing for potential ECU knockouts
+  set_obd_multiplexing(params, False)
+  params.put_bool("FirmwareQueryDone", True)
 
   finger = gen_empty_fingerprint()
   candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]}  # attempt fingerprint on both bus 0 and 1

selfdrive/car/fw_query_definitions.py

@@ -59,8 +59,8 @@ class Request:
   bus: int = 1
   # FW responses from these queries will not be used for fingerprinting
   logging: bool = False
-  # These requests are done once OBD multiplexing is disabled, after all others
-  non_obd: bool = False
+  # boardd toggles OBD multiplexing on/off as needed
+  obd_multiplexing: bool = True
 
 
 @dataclass

selfdrive/car/fw_versions.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 from collections import defaultdict
-from typing import Any, List, Optional, Set
+from typing import Any, Dict, List, Set
 from tqdm import tqdm
 
 import panda.python.uds as uds
@@ -147,8 +147,10 @@ def match_fw_to_car(fw_versions, allow_exact=True, allow_fuzzy=True):
 
 
 def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
-  queries: List[List[EcuAddrBusType]] = list()
-  parallel_queries: List[EcuAddrBusType] = list()
+  params = Params()
+  # queries are split by OBD multiplexing mode
+  queries: Dict[bool, List[List[EcuAddrBusType]]] = {True: [], False: []}
+  parallel_queries: Dict[bool, List[EcuAddrBusType]] = {True: [], False: []}
   responses = set()
 
   for brand, r in REQUESTS:
@@ -163,21 +165,24 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
           a = (addr, sub_addr, r.bus)
           # Build set of queries
           if sub_addr is None:
-            if a not in parallel_queries:
-              parallel_queries.append(a)
+            if a not in parallel_queries[r.obd_multiplexing]:
+              parallel_queries[r.obd_multiplexing].append(a)
           else:  # subaddresses must be queried one by one
-            if [a] not in queries:
-              queries.append([a])
+            if [a] not in queries[r.obd_multiplexing]:
+              queries[r.obd_multiplexing].append([a])
 
           # Build set of expected responses to filter
           response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
           responses.add((response_addr, sub_addr, r.bus))
 
-  queries.insert(0, parallel_queries)
+  for obd_multiplexing in queries:
+    queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
 
   ecu_responses = set()
-  for query in queries:
-    ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
+  for obd_multiplexing in queries:
+    set_obd_multiplexing(params, 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
 
 
@@ -198,13 +203,13 @@ def get_brand_ecu_matches(ecu_rx_addrs):
   return brand_matches
 
 
-def disable_obd_multiplexing(params):
-  if not params.get_bool("ObdMultiplexingDisabled"):
-    params.put_bool("FirmwareObdQueryDone", True)
-
-    cloudlog.warning("Waiting for OBD multiplexing to be disabled")
-    params.get_bool("ObdMultiplexingDisabled", block=True)
-    cloudlog.warning("OBD multiplexing disabled")
+def set_obd_multiplexing(params: Params, 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")
 
 
 def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pandas=1, debug=False, progress=False):
@@ -212,7 +217,6 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
 
   all_car_fw = []
   brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
-  matched_brand: Optional[str] = None
 
   for brand in sorted(brand_matches, key=lambda b: len(brand_matches[b]), reverse=True):
     car_fw = get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress)
@@ -220,21 +224,14 @@ def get_fw_versions_ordered(logcan, sendcan, ecu_rx_addrs, timeout=0.1, num_pand
     # Try to match using FW returned from this brand only
     matches = match_fw_to_car_exact(build_fw_dict(car_fw))
     if len(matches) == 1:
-      matched_brand = brand
       break
 
-  disable_obd_multiplexing(Params())
-
-  # Do non-OBD queries for matched brand, or all if no match is found
-  for brand in FW_QUERY_CONFIGS.keys():
-    if brand == matched_brand or matched_brand is None:
-      all_car_fw.extend(get_fw_versions(logcan, sendcan, query_brand=brand, timeout=timeout, num_pandas=num_pandas, obd_multiplexed=False, debug=debug, progress=progress))
-
   return all_car_fw
 
 
-def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, obd_multiplexed=True, debug=False, progress=False):
+def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1, num_pandas=1, debug=False, progress=False):
   versions = VERSIONS.copy()
+  params = Params()
 
   # Each brand can define extra ECUs to query for data collection
   for brand, config in FW_QUERY_CONFIGS.items():
@@ -281,9 +278,9 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
         # Skip query if no panda available
         if r.bus > num_pandas * 4 - 1:
           continue
-        # Or if request is not designated for current multiplexing mode
-        elif r.non_obd == obd_multiplexed:
-          continue
+
+        # Toggle OBD multiplexing for each request
+        set_obd_multiplexing(params, r.obd_multiplexing)
 
         try:
           addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and

selfdrive/car/honda/values.py

@@ -186,7 +186,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
       [StdQueries.UDS_VERSION_RESPONSE],
       bus=1,
       logging=True,
-      non_obd=True,
+      obd_multiplexing=False,
     ),
   ],
   extra_ecus=[

selfdrive/controls/tests/test_startup.py

@@ -72,7 +72,6 @@ class TestStartup(unittest.TestCase):
     params.clear_all()
     params.put_bool("Passive", False)
     params.put_bool("OpenpilotEnabledToggle", True)
-    params.put_bool("ObdMultiplexingDisabled", True)
 
     # Build capnn version of FW array
     if fw_versions is not None:
@@ -109,6 +108,10 @@ class TestStartup(unittest.TestCase):
       finger = _FINGERPRINTS[car_model][0]
 
     for _ in range(1000):
+      # controlsd waits for boardd to echo back that it has changed the multiplexing mode
+      if not params.get_bool("ObdMultiplexingChanged"):
+        params.put_bool("ObdMultiplexingChanged", True)
+
       msgs = [[addr, 0, b'\x00'*length, 0] for addr, length in finger.items()]
       pm.send('can', can_list_to_can_capnp(msgs))
 

selfdrive/test/process_replay/process_replay.py

@@ -397,7 +397,6 @@ def setup_env(simulation=False, CP=None, cfg=None, controlsState=None, lr=None):
   params.put_bool("DisengageOnAccelerator", True)
   params.put_bool("WideCameraOnly", False)
   params.put_bool("DisableLogging", False)
-  params.put_bool("ObdMultiplexingDisabled", True)
 
   os.environ["NO_RADAR_SLEEP"] = "1"
   os.environ["REPLAY"] = "1"