pandad: improve startup time (#28137)

* pandad: improve startup time

* add test

* reset with gpio

* adjust threshold

* fix

* no panda

* fix

* check unknown

* increase for boardd

---------

Co-authored-by: Comma Device <device@comma.ai>

panda

@@ -1 +1 @@
-Subproject commit 98a15f2a90bf04f4eb68654163ef9601eff84280
+Subproject commit da7c57748648e786c99621271b5a88d11bcd90fa

selfdrive/boardd/pandad.py

@@ -81,13 +81,15 @@ def main() -> NoReturn:
   params = Params()
 
   while True:
+    try:
       count += 1
       cloudlog.event("pandad.flash_and_connect", count=count)
-    try:
       params.remove("PandaSignatures")
 
       # Flash all Pandas in DFU mode
-      for serial in PandaDFU.list():
+      dfu_serials = PandaDFU.list()
+      if len(dfu_serials) > 0:
+        for serial in dfu_serials:
           cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}")
           PandaDFU(serial).recover()
         time.sleep(1)
@@ -107,17 +109,6 @@ def main() -> NoReturn:
       for serial in panda_serials:
         pandas.append(flash_panda(serial))
 
-      # check health for lost heartbeat
-      for panda in pandas:
-        health = panda.health()
-        if health["heartbeat_lost"]:
-          params.put_bool("PandaHeartbeatLost", True)
-          cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
-
-        if first_run:
-          cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
-          panda.reset()
-
       # Ensure internal panda is present if expected
       internal_pandas = [panda for panda in pandas if panda.is_internal()]
       if HARDWARE.has_internal_panda() and len(internal_pandas) == 0:
@@ -133,7 +124,20 @@ def main() -> NoReturn:
       # log panda fw versions
       params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
 
-      # close all pandas
+      for panda in pandas:
+        # check health for lost heartbeat
+        health = panda.health()
+        if health["heartbeat_lost"]:
+          params.put_bool("PandaHeartbeatLost", True)
+          cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
+
+        if first_run:
+          cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
+          if panda.is_internal():
+            HARDWARE.reset_internal_panda()
+          else:
+            panda.reset(reconnect=False)
+
       for p in pandas:
         p.close()
     except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -6,7 +6,7 @@ import unittest
 from collections import defaultdict
 
 import cereal.messaging as messaging
-from cereal import car
+from cereal import car, log
 from common.params import Params
 from common.spinner import Spinner
 from common.timeout import Timeout
@@ -36,7 +36,8 @@ class TestBoardd(unittest.TestCase):
 
     with Timeout(60, "boardd didn't start"):
       sm = messaging.SubMaster(['pandaStates'])
-      while sm.rcv_frame['pandaStates'] < 1 and len(sm['pandaStates']) == 0:
+      while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
+          any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
         sm.update(1000)
 
     num_pandas = len(sm['pandaStates'])

selfdrive/boardd/tests/test_pandad.py

@@ -3,8 +3,9 @@ import time
 import unittest
 
 import cereal.messaging as messaging
-from panda import Panda
+from cereal import log
 from common.gpio import gpio_set, gpio_init
+from panda import Panda
 from selfdrive.test.helpers import phone_only
 from selfdrive.manager.process_config import managed_processes
 from system.hardware import HARDWARE
@@ -20,10 +21,10 @@ class TestPandad(unittest.TestCase):
     sm = messaging.SubMaster(['peripheralState'])
     for _ in range(timeout):
       sm.update(1000)
-      if sm.updated['peripheralState']:
+      if sm['peripheralState'].pandaType != log.PandaState.PandaType.unknown:
         break
 
-    if not sm.updated['peripheralState']:
+    if sm['peripheralState'].pandaType == log.PandaState.PandaType.unknown:
       raise Exception("boardd failed to start")
 
   @phone_only
@@ -54,6 +55,18 @@ class TestPandad(unittest.TestCase):
 
     assert any(Panda(s).is_internal() for s in Panda.list())
 
+  @phone_only
+  def test_best_case_startup_time(self):
+    # run once so we're setup
+    managed_processes['pandad'].start()
+    self._wait_for_boardd()
+    managed_processes['pandad'].stop()
+
+    # should be fast this time
+    managed_processes['pandad'].start()
+    self._wait_for_boardd(8)
+
+
   #def test_out_of_date_fw(self):
   #  pass