From da62722d07346a7f9fd39d688af00776769273eb Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Sun, 10 May 2026 18:04:27 -0700 Subject: [PATCH] tighten pandad timings (#37999) * tighten can timings * cleanup * rm that --- selfdrive/pandad/pandad.cc | 55 +++++++++++++++---- .../pandad/tests/test_pandad_loopback.py | 16 ++++-- system/hardware/tici/hardware.h | 9 ++- 3 files changed, 61 insertions(+), 19 deletions(-) diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index f0f13d32e..7c91fe361 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -1,6 +1,7 @@ #include "selfdrive/pandad/pandad.h" #include +#include #include #include #include @@ -23,6 +24,14 @@ ExitHandler do_exit; +struct HwmonState { + std::atomic voltage{0}; + std::atomic current{0}; + std::atomic initialized{false}; +}; + +HwmonState hwmon_state; + bool check_connected(Panda *panda) { if (!panda->connected()) { do_exit = true; @@ -105,6 +114,26 @@ void can_recv(Panda *panda, PubMaster *pm) { } } +void hwmon_thread() { + util::set_thread_name("pandad_hwmon"); + + while (!do_exit) { + double read_time = millis_since_boot(); + uint32_t voltage = Hardware::get_voltage(); + uint32_t current = Hardware::get_current(); + read_time = millis_since_boot() - read_time; + if (read_time > 50) { + LOGW("reading hwmon took %lfms", read_time); + } + + hwmon_state.voltage.store(voltage); + hwmon_state.current.store(current); + hwmon_state.initialized.store(true); + + util::sleep_for(500); + } +} + void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) { ps.setVoltage(health.voltage_pkt); ps.setCurrent(health.current_pkt); @@ -235,6 +264,10 @@ std::optional send_panda_states(PubMaster *pm, Panda *panda, bool is_onroa } void send_peripheral_state(Panda *panda, PubMaster *pm) { + if (!hwmon_state.initialized.load()) { + return; + } + // build msg MessageBuilder msg; auto evt = msg.initEvent(); @@ -243,13 +276,8 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) { auto ps = evt.initPeripheralState(); ps.setPandaType(panda->hw_type); - double read_time = millis_since_boot(); - ps.setVoltage(Hardware::get_voltage()); - ps.setCurrent(Hardware::get_current()); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } + ps.setVoltage(hwmon_state.voltage.load()); + ps.setCurrent(hwmon_state.current.load()); // fall back to panda's voltage and current measurement if (ps.getVoltage() == 0 && ps.getCurrent() == 0) { @@ -371,18 +399,18 @@ void pandad_run(Panda *panda) { const bool spoofing_started = getenv("STARTED") != nullptr; const bool fake_send = getenv("FAKESEND") != nullptr; - // Start the CAN send thread + // Start helper threads for event-driven sendcan and slow non-Panda reads. std::thread send_thread(can_send_thread, panda, fake_send); + std::thread hardware_thread(hwmon_thread); - Params params; RateKeeper rk("pandad", 100); - SubMaster sm({"selfdriveState"}); + SubMaster sm({"selfdriveState", "deviceState"}); PubMaster pm({"can", "pandaStates", "peripheralState"}); PandaSafety panda_safety(panda); bool engaged = false; bool is_onroad = false; - // Main loop: receive CAN data and process states + // Main loop: receive CAN first, then process lower priority panda and peripheral state. while (!do_exit && check_connected(panda)) { can_recv(panda, &pm); @@ -395,7 +423,9 @@ void pandad_run(Panda *panda) { if (rk.frame() % 10 == 0) { sm.update(0); engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled(); - is_onroad = params.getBool("IsOnroad"); + if (sm.updated("deviceState")) { + is_onroad = sm["deviceState"].getDeviceState().getStarted(); + } process_panda_state(panda, &pm, engaged, is_onroad, spoofing_started); panda_safety.configureSafetyMode(is_onroad); } @@ -427,6 +457,7 @@ void pandad_run(Panda *panda) { } send_thread.join(); + hardware_thread.join(); } void pandad_main_thread(std::string serial) { diff --git a/selfdrive/pandad/tests/test_pandad_loopback.py b/selfdrive/pandad/tests/test_pandad_loopback.py index fd4a99be6..0c0e57b37 100644 --- a/selfdrive/pandad/tests/test_pandad_loopback.py +++ b/selfdrive/pandad/tests/test_pandad_loopback.py @@ -16,17 +16,24 @@ from openpilot.selfdrive.pandad import can_list_to_can_capnp from openpilot.selfdrive.test.helpers import with_processes +def publish_device_state(pm, started): + msg = messaging.new_message('deviceState') + msg.deviceState.started = started + pm.send('deviceState', msg) + @retry(attempts=3) def setup_pandad(): params = Params() params.clear_all() - params.put_bool("IsOnroad", False) + pm = messaging.PubMaster(['deviceState']) sm = messaging.SubMaster(['pandaStates']) + publish_device_state(pm, False) with Timeout(90, "pandad didn't start"): while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \ any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): - sm.update(1000) + publish_device_state(pm, False) + sm.update(100) # pandad safety setting relies on these params cp = car.CarParams.new_message() @@ -35,14 +42,15 @@ def setup_pandad(): safety_config.safetyModel = car.CarParams.SafetyModel.allOutput cp.safetyConfigs = [safety_config] - params.put_bool("IsOnroad", True) params.put_bool("FirmwareQueryDone", True) params.put_bool("ControlsReady", True) params.put("CarParams", cp.to_bytes()) + publish_device_state(pm, True) with Timeout(90, "pandad didn't set safety mode"): while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']): - sm.update(1000) + publish_device_state(pm, True) + sm.update(100) def send_random_can_messages(sendcan, count): sent_msgs = defaultdict(set) diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index d59b45efc..a06ce43b3 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -13,8 +13,11 @@ class HardwareTici : public HardwareNone { public: static std::string get_name() { - std::string model = util::read_file("/sys/firmware/devicetree/base/model"); - return util::strip(model.substr(std::string("comma ").size())); + static const std::string name = []() { + std::string model = util::read_file("/sys/firmware/devicetree/base/model"); + return util::strip(model.substr(std::string("comma ").size())); + }(); + return name; } static cereal::InitData::DeviceType get_device_type() { @@ -23,7 +26,7 @@ public: {"tizi", cereal::InitData::DeviceType::TIZI}, {"mici", cereal::InitData::DeviceType::MICI} }; - auto it = device_map.find(get_name()); + static const auto it = device_map.find(get_name()); assert(it != device_map.end()); return it->second; }