boardd: panda_state_thread run closer to 2 Hz (#23529)

* boardd: panda_state_thread run closer to 2Hz

* make explicit
old-commit-hash: b75e410779

selfdrive/boardd/boardd.cc

@@ -420,6 +420,8 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
 
   // run at 2hz
   while (!do_exit && check_all_connected(pandas)) {
+    uint64_t start_time = nanos_since_boot();
+
     // send out peripheralState
     send_peripheral_state(pm, peripheral_panda);
     ignition = send_panda_states(pm, pandas, spoofing_started);
@@ -452,7 +454,9 @@ void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofin
     for (const auto &panda : pandas) {
       panda->send_heartbeat(engaged);
     }
-    util::sleep_for(500);
+
+    uint64_t dt = nanos_since_boot() - start_time;
+    util::sleep_for(500 - dt / 1000000ULL);
   }
 }
 

selfdrive/common/util.h

@@ -91,7 +91,9 @@ bool create_directories(const std::string &dir, mode_t mode);
 std::string check_output(const std::string& command);
 
 inline void sleep_for(const int milliseconds) {
-  std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
+  if (milliseconds > 0) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
+  }
 }
 
 }  // namespace util