diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 8439616c33..e6ef4a4072 100644 --- a/selfdrive/pandad/pandad.cc +++ b/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 &pandas, PubMaster *pm, bool spoofing_started) { - static SubMaster sm({"selfdriveState"}); - +void process_panda_state(std::vector &pandas, PubMaster *pm, bool engaged, bool spoofing_started) { std::vector connected_serials; for (Panda *p : pandas) { connected_serials.push_back(p->hw_serial()); @@ -361,8 +359,6 @@ void process_panda_state(std::vector &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 &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 &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 &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(); } diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index 12accdbf5e..fd6668feba 100755 --- a/selfdrive/pandad/pandad.py +++ b/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()