From 861ceb2feeb8e9f303b96c0e7a9ef19c5423938d Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 23 Aug 2023 01:31:04 -0700
Subject: [PATCH] FW query: query one sub-address in parallel (#28675)

* put one subaddr in parallel_addrs

* p

* remove print

* update test refs

* test

* revert test changes

* do same in present ECU query, lower threshold to catch (~0.82 to ~0.72 right within bounds)

* type

* clean up

* update test ref
---
 selfdrive/car/fw_versions.py               | 12 ++++++++----
 selfdrive/car/tests/test_fw_fingerprint.py |  6 +++---
 2 files changed, 11 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 45c4967cb8..9b4b5d6a33 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -185,11 +185,13 @@ def get_present_ecus(logcan, sendcan, num_pandas=1) -> Set[EcuAddrBusType]:
         if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
           a = (addr, sub_addr, r.bus)
           # Build set of queries
-          if sub_addr is None:
+          # Add first sub-address to parallel queries since there will be no conflicts
+          sub_addr_in_parallel_addrs = any(p[1] is not None for p in parallel_queries[r.obd_multiplexing])
+          if sub_addr is None or not sub_addr_in_parallel_addrs:
             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[r.obd_multiplexing]:
+            if [a] not in queries[r.obd_multiplexing] and a not in parallel_queries[r.obd_multiplexing]:
               queries[r.obd_multiplexing].append([a])
 
           # Build set of expected responses to filter
@@ -282,11 +284,13 @@ def get_fw_versions(logcan, sendcan, query_brand=None, extra=None, timeout=0.1,
         if a not in ecu_types:
           ecu_types[a] = ecu_type
 
-        if sub_addr is None:
+        # Add first sub-address to parallel queries since there will be no conflicts
+        sub_addr_in_parallel_addrs = any(p[2] is not None for p in parallel_addrs)
+        if sub_addr is None or not sub_addr_in_parallel_addrs:
           if a not in parallel_addrs:
             parallel_addrs.append(a)
         else:
-          if [a] not in addrs:
+          if [a] not in addrs and a not in parallel_addrs:
             addrs.append([a])
 
   addrs.insert(0, parallel_addrs)
diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py
index 883a72897c..47b194bcf8 100755
--- a/selfdrive/car/tests/test_fw_fingerprint.py
+++ b/selfdrive/car/tests/test_fw_fingerprint.py
@@ -206,7 +206,7 @@ class TestFwFingerprintTiming(unittest.TestCase):
   def test_startup_timing(self):
     # Tests worse-case VIN query time and typical present ECU query time
     vin_ref_time = 1.0
-    present_ecu_ref_time = 0.8
+    present_ecu_ref_time = 0.7
 
     fake_socket = FakeSocket()
     present_ecu_time = 0.0
@@ -225,7 +225,7 @@ class TestFwFingerprintTiming(unittest.TestCase):
     print(f'get_vin, query time={vin_time / self.N} seconds')
 
   def test_fw_query_timing(self):
-    total_ref_time = 6.1
+    total_ref_time = 5.9
     brand_ref_times = {
       1: {
         'body': 0.1,
@@ -237,7 +237,7 @@ class TestFwFingerprintTiming(unittest.TestCase):
         'nissan': 0.4,
         'subaru': 0.2,
         'tesla': 0.2,
-        'toyota': 1.6,
+        'toyota': 1.4,
         'volkswagen': 0.2,
       },
       2: {