cleanup

Author: deanlee

selfdrive/pandad/panda/runner.py

@@ -21,6 +21,7 @@ def __init__(self, serials, pandas):
     self.pandas = pandas
     self.serials = set(serials)
     self.hw_types = [int.from_bytes(p.get_type(), 'big') for p in pandas]
+
     for panda in self.pandas:
       if os.getenv("BOARDD_LOOPBACK"):
         panda.set_can_loopback(True)
@@ -38,17 +39,21 @@ def __init__(self, serials, pandas):
   def _can_send(self, evt):
     sock = messaging.sub_sock('sendcan', timeout=100)
     while not evt.is_set():
-      data = sock.receive()
-      if data:
-        msg = messaging.log_from_bytes(data)
-        cans = [(c.address, c.dat, c.src) for c in msg.sendcan]
-        age = (time.monotonic_ns() - msg.logMonoTime) / 1e9
-        if age < 1 and not FAKE_SEND:
-          with self.lock:
-            for p in self.pandas:
-              p.can_send_many(cans)
-        elif age >= 1:
-          cloudlog.error(f"Dropping stale sendcan message, age: {age:.2f}s")
+      try:
+        data = sock.receive()
+        if data:
+          msg = messaging.log_from_bytes(data)
+          cans = [(c.address, c.dat, c.src) for c in msg.sendcan]
+          age = (time.monotonic_ns() - msg.logMonoTime) / 1e9
+          if age < 1 and not FAKE_SEND:
+            with self.lock:
+              for p in self.pandas:
+                p.can_send_many(cans)
+          elif age >= 1:
+            cloudlog.error(f"Dropping stale sendcan message, age: {age:.2f}s")
+
+      except Exception as e:
+        cloudlog.error(f"Exception in sendcan thread: {e}")
 
   def _can_recv(self):
     with self.lock:
@@ -63,8 +68,8 @@ def _can_recv(self):
       self.pm.send("can", msg)
 
   def run(self, evt):
-    thread = threading.Thread(target=self._can_send, args=(evt,))
-    thread.start()
+    send_thread = threading.Thread(target=self._can_send, args=(evt,))
+    send_thread.start()
 
     rk = Ratekeeper(100, print_delay_threshold=None)
     engaged = False
@@ -88,6 +93,7 @@ def run(self, evt):
             if current_serials != self.serials:
               cloudlog.warning("Reconnecting to new panda")
               evt.set()
+              break
 
           self.safety_mgr.configure_safety_mode()
 
@@ -101,7 +107,7 @@ def run(self, evt):
     finally:
       evt.set()
       self.periph_mgr.cleanup()
-      thread.join()
+      send_thread.join()
 
       # Close relay on exit to prevent a fault
       is_onroad = Params().get_bool("IsOnroad")

selfdrive/pandad/panda/state_manager.py

@@ -1,7 +1,7 @@
 import os
 from cereal import car, log
 import cereal.messaging as messaging
-from openpilot.common.params import Params
+from panda.python import PANDA_BUS_CNT
 
 SPOOFING_STARTED = os.getenv("STARTED") == "1"
 
@@ -13,7 +13,6 @@
 class PandaStateManager:
   def __init__(self, pandas, hw_types, lock):
     self.pandas = pandas
-    self.params = Params()
     self.lock = lock
     self.hw_types = hw_types
 
@@ -26,47 +25,48 @@ def __init__(self, pandas, hw_types, lock):
   def process(self, engaged, pm) -> bool:
     msg = messaging.new_message('pandaStates', len(self.pandas))
     msg.valid = True
-    pss = msg.pandaStates
+    panda_states = msg.pandaStates
     ignition = False
 
-    for i, p in enumerate(self.pandas):
+    for i, panda in enumerate(self.pandas):
       with self.lock:
-        health = p.health() or {}
+        health = panda.health() or {}
 
       if SPOOFING_STARTED:
         health['ignition_line'] = 1
       elif self.is_comma_three_red and self.hw_types[i] == log.PandaState.PandaType.dos:
         health['ignition_line'] = 0
 
       ignition |= bool(health['ignition_line']) or bool(health['ignition_can'])
-      ps = pss[i]
+      ps = panda_states[i]
       self._fill_state(ps, self.hw_types[i], health)
 
-      for j, cs in enumerate((ps.init('canState0'), ps.init('canState1'), ps.init('canState2'))):
+      # Fill can state
+      for j in range(PANDA_BUS_CNT):
         with self.lock:
-          can_health = p.can_health(j)
-        self._fill_can_state(cs, can_health)
-
-      fault_bits = int(health['faults'])
-      fault_count = bin(fault_bits).count('1')
-      faults = ps.init('faults', fault_count)
-      idx = 0
-      for f in range(log.PandaState.FaultType.relayMalfunction,
-                     log.PandaState.FaultType.heartbeatLoopWatchdog + 1):
-        if fault_bits & (1 << f):
-          faults[idx] = f
-          idx += 1
+          can_health = panda.can_health(j)
+        can_state = ps.init(f'canState{j}')
+        self._fill_can_state(can_state, can_health)
+
+      # Set faults
+      fault_bits = int(health.get('faults', 0))
+      faults_list = [f for f in range(log.PandaState.FaultType.relayMalfunction,
+                                      log.PandaState.FaultType.heartbeatLoopWatchdog + 1)
+                    if fault_bits & (1 << f)]
+      faults = ps.init('faults', len(faults_list))
+      for idx, fault in enumerate(faults_list):
+        faults[idx] = fault
 
     with self.lock:
-      for p, ps in zip(self.pandas, pss):
+      for panda, ps in zip(self.pandas, panda_states):
         if ps.safetyModel == car.CarParams.SafetyModel.silent or (
             not ignition and ps.safetyModel != car.CarParams.SafetyModel.noOutput):
-          p.set_safety_mode(car.CarParams.SafetyModel.noOutput)
+          panda.set_safety_mode(car.CarParams.SafetyModel.noOutput)
 
         if ps.powerSaveEnabled != (not ignition):
-          p.set_power_save(not ignition)
+          panda.set_power_save(not ignition)
 
-        p.send_heartbeat(engaged)
+        panda.send_heartbeat(engaged)
 
     pm.send("pandaStates", msg)
     return ignition