Toyota: use FW query for DSU detection (#20615)

* Toyota: use FW query for DSU detection

* make sure we have at least some fw responses

* bool

* add test to test_startup

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/car/__init__.py

@@ -119,17 +119,5 @@ def create_gas_command(packer, gas_amount, idx):
   return packer.make_can_msg("GAS_COMMAND", 0, values)
 
 
-def is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu):
-  # check if a stock ecu is disconnected by looking for specific CAN msgs in the fingerprint
-  # return True if the reference car fingerprint contains the ecu fingerprint msg and
-  # fingerprint does not contains messages normally sent by a given ecu
-  ecu_in_car = False
-  for car_finger in fingerprint_list[car]:
-    if any(msg in car_finger for msg in ecu_fingerprint[ecu]):
-      ecu_in_car = True
-
-  return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu])
-
-
 def make_can_msg(addr, dat, bus):
   return [addr, 0, dat, bus]

selfdrive/car/toyota/interface.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.config import Conversions as CV
-from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, NO_DSU_CAR, FINGERPRINTS, CarControllerParams
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
+from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, CarControllerParams
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.swaglog import cloudlog
 from selfdrive.car.interfaces import CarInterfaceBase
 
@@ -323,13 +323,12 @@ class CarInterface(CarInterfaceBase):
     ret.enableCamera = True
     # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it
     smartDsu = 0x2FF in fingerprint[0]
-    # TODO: use FW query for the enableDsu flag
     # In TSS2 cars the camera does long control
-    ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in NO_DSU_CAR
+    found_ecus = [fw.ecu for fw in car_fw]
+    ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR)
     ret.enableGasInterceptor = 0x201 in fingerprint[0]
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
     ret.openpilotLongitudinalControl = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR)
-    cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
     cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu)
     cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
 

selfdrive/car/toyota/values.py

@@ -72,11 +72,6 @@ STATIC_MSGS = [
   (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'),
 ]
 
-ECU_FINGERPRINT = {
-  Ecu.fwdCamera: [0x2e4],   # steer torque cmd
-  Ecu.dsu: [0x283],   # accel cmd
-}
-
 
 FINGERPRINTS = {
   CAR.RAV4: [{

selfdrive/controls/tests/test_startup.py

@@ -17,14 +17,16 @@ from selfdrive.test.helpers import with_processes
 EventName = car.CarEvent.EventName
 Ecu = car.CarParams.Ecu
 
-COROLLA_TSS2_FW_VERSIONS = [
-  (Ecu.engine, 0x700, None, b'\x01896630ZG5000\x00\x00\x00\x00'),
-  (Ecu.eps, 0x7a1, None, b'\x018965B1255000\x00\x00\x00\x00'),
-  (Ecu.esp, 0x7b0, None, b'\x01F152602280\x00\x00\x00\x00\x00\x00'),
-  (Ecu.fwdRadar, 0x750, 0xf, b'\x018821F3301100\x00\x00\x00\x00'),
-  (Ecu.fwdCamera, 0x750, 0x6d, b'\x028646F12010D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00'),
+COROLLA_FW_VERSIONS = [
+    (Ecu.engine, 0x7e0, None, b'\x0230ZC2000\x00\x00\x00\x00\x00\x00\x00\x0050212000\x00\x00\x00\x00\x00\x00\x00\x00'),
+    (Ecu.esp, 0x7b0, None, b'F152602190\x00\x00\x00\x00\x00\x00'),
+    (Ecu.eps, 0x7a1, None, b'8965B02181\x00\x00\x00\x00\x00\x00'),
+    (Ecu.fwdRadar, 0x750, 0xf, b'8821F4702100\x00\x00\x00\x00'),
+    (Ecu.fwdCamera, 0x750, 0x6d, b'8646F0201101\x00\x00\x00\x00'),
+    (Ecu.dsu, 0x791, None, b'881510201100\x00\x00\x00\x00'),
 ]
-COROLLA_TSS2_FW_VERSIONS_FUZZY = COROLLA_TSS2_FW_VERSIONS[:-1] + [(Ecu.fwdCamera, 0x750, 0x6d, b'xxxxxx')]
+COROLLA_FW_VERSIONS_FUZZY = COROLLA_FW_VERSIONS[:-1] + [(Ecu.dsu, 0x791, None, b'xxxxxx')]
+COROLLA_FW_VERSIONS_NO_DSU = COROLLA_FW_VERSIONS[:-1]
 
 
 class TestStartup(unittest.TestCase):
@@ -37,7 +39,11 @@ class TestStartup(unittest.TestCase):
     (EventName.startupMaster, HYUNDAI.SONATA, True, None),
 
     # offically supported car, FW query
-    (EventName.startupMaster, TOYOTA.COROLLA_TSS2, False, COROLLA_TSS2_FW_VERSIONS),
+    (EventName.startupMaster, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS),
+
+    # DSU unplugged
+    (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_NO_DSU),
+    (EventName.communityFeatureDisallowed, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_NO_DSU),
 
     # community supported car
     (EventName.startupMaster, HYUNDAI.KIA_STINGER, True, None),
@@ -52,8 +58,8 @@ class TestStartup(unittest.TestCase):
     (EventName.startupNoCar, None, False, None),
 
     # fuzzy match
-    (EventName.startupFuzzyFingerprint, TOYOTA.COROLLA_TSS2, True, COROLLA_TSS2_FW_VERSIONS_FUZZY),
-    (EventName.communityFeatureDisallowed, TOYOTA.COROLLA_TSS2, False, COROLLA_TSS2_FW_VERSIONS_FUZZY),
+    (EventName.startupFuzzyFingerprint, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_FUZZY),
+    (EventName.communityFeatureDisallowed, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_FUZZY),
   ])
   @with_processes(['controlsd'])
   def test_startup_alert(self, expected_event, car_model, toggle_enabled, fw_versions):