pandad: reset safety mode on exit

selfdrive/pandad/pandad.cc

@@ -323,9 +323,7 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) {
   pm->send("peripheralState", msg);
 }
 
-void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoofing_started) {
-  static SubMaster sm({"selfdriveState"});
-
+void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engaged, bool spoofing_started) {
   std::vector<std::string> connected_serials;
   for (Panda *p : pandas) {
     connected_serials.push_back(p->hw_serial());
@@ -361,8 +359,6 @@ void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool spoof
       }
     }
 
-    sm.update(0);
-    const bool engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
     for (const auto &panda : pandas) {
       panda->send_heartbeat(engaged);
     }
@@ -428,9 +424,11 @@ void pandad_run(std::vector<Panda *> &pandas) {
   std::thread send_thread(can_send_thread, pandas, fake_send);
 
   RateKeeper rk("pandad", 100);
+  SubMaster sm({"selfdriveState"});
   PubMaster pm({"can", "pandaStates", "peripheralState"});
   PandaSafety panda_safety(pandas);
   Panda *peripheral_panda = pandas[0];
+  bool engaged = false;
 
   // Main loop: receive CAN data and process states
   while (!do_exit && check_all_connected(pandas)) {
@@ -443,7 +441,9 @@ void pandad_run(std::vector<Panda *> &pandas) {
 
     // Process panda state at 10 Hz
     if (rk.frame() % 10 == 0) {
-      process_panda_state(pandas, &pm, spoofing_started);
+      sm.update(0);
+      engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
+      process_panda_state(pandas, &pm, engaged, spoofing_started);
       panda_safety.configureSafetyMode();
     }
 
@@ -455,6 +455,16 @@ void pandad_run(std::vector<Panda *> &pandas) {
     rk.keepTime();
   }
 
+  // Close relay on exit to prevent a fault
+  const bool is_onroad = Params().getBool("IsOnroad");
+  if (is_onroad && !engaged) {
+    for (auto &p : pandas) {
+      if (p->connected()) {
+        p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+      }
+    }
+  }
+
   send_thread.join();
 }
 

selfdrive/pandad/pandad.py

@@ -3,8 +3,8 @@
 import os
 import usb1
 import time
+import signal
 import subprocess
-from typing import NoReturn
 
 from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
 from openpilot.common.basedir import BASEDIR
@@ -61,13 +61,25 @@ def flash_panda(panda_serial: str) -> Panda:
   return panda
 
 
-def main() -> NoReturn:
+def main() -> None:
+  # signal pandad to close the relay and exit
+  def signal_handler(signum, frame):
+    cloudlog.info(f"Caught signal {signum}, exiting")
+    nonlocal do_exit
+    do_exit = True
+    if process is not None:
+      process.send_signal(signal.SIGINT)
+
+  process = None
+  do_exit = False
+  signal.signal(signal.SIGINT, signal_handler)
+
   count = 0
   first_run = True
   params = Params()
   no_internal_panda_count = 0
 
-  while True:
+  while not do_exit:
     try:
       count += 1
       cloudlog.event("pandad.flash_and_connect", count=count)
@@ -159,8 +171,9 @@ def main() -> NoReturn:
 
     # run pandad with all connected serials as arguments
     os.environ['MANAGER_DAEMON'] = 'pandad'
-    os.chdir(os.path.join(BASEDIR, "selfdrive/pandad"))
-    subprocess.run(["./pandad", *panda_serials], check=True)
+    process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "selfdrive/pandad"))
+    process.wait()
+
 
 if __name__ == "__main__":
   main()