tighten pandad timings (#37999)

* tighten can timings

* cleanup

* rm that

selfdrive/pandad/pandad.cc

@@ -1,6 +1,7 @@
 #include "selfdrive/pandad/pandad.h"
 
 #include <array>
+#include <atomic>
 #include <bitset>
 #include <cassert>
 #include <cerrno>
@@ -23,6 +24,14 @@
 
 ExitHandler do_exit;
 
+struct HwmonState {
+  std::atomic<uint32_t> voltage{0};
+  std::atomic<uint32_t> current{0};
+  std::atomic<bool> 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<bool> 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) {

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)

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;
   }