Don't send empty pandaState from panda_state_thread (#22393)

selfdrive/boardd/boardd.cc

@@ -162,20 +162,6 @@ Panda *usb_connect() {
   return panda.release();
 }
 
-// must be called before threads or with mutex
-static Panda *usb_retry_connect() {
-  LOGW("attempting to connect");
-  while (!do_exit) {
-    Panda *panda = usb_connect();
-    if (panda) {
-      LOGW("connected to board");
-      return panda;
-    }
-    util::sleep_for(100);
-  };
-  return nullptr;
-}
-
 void can_recv(Panda *panda, PubMaster &pm) {
   kj::Array<capnp::word> can_data;
   panda->can_receive(can_data);
@@ -248,24 +234,19 @@ void can_recv_thread(Panda *panda) {
   }
 }
 
-void panda_state_thread(Panda *&panda, bool spoofing_started) {
-  LOGD("start panda state thread");
-  PubMaster pm({"pandaState"});
+void send_empty_panda_state(PubMaster *pm) {
+  MessageBuilder msg;
+  auto pandaState  = msg.initEvent().initPandaState();
+  pandaState.setPandaType(cereal::PandaState::PandaType::UNKNOWN);
+  pm->send("pandaState", msg);
+}
 
+void panda_state_thread(PubMaster *pm, Panda *panda, bool spoofing_started) {
+  LOGD("start panda state thread");
   uint32_t no_ignition_cnt = 0;
   bool ignition_last = false;
   Params params = Params();
 
-  // Broadcast empty pandaState message when panda is not yet connected
-  while (!do_exit && !panda) {
-    MessageBuilder msg;
-    auto pandaState  = msg.initEvent().initPandaState();
-
-    pandaState.setPandaType(cereal::PandaState::PandaType::UNKNOWN);
-    pm.send("pandaState", msg);
-    util::sleep_for(500);
-  }
-
   // run at 2hz
   while (!do_exit && panda->connected) {
     health_t pandaState = panda->get_state();
@@ -386,7 +367,7 @@ void panda_state_thread(Panda *&panda, bool spoofing_started) {
         i++;
       }
     }
-    pm.send("pandaState", msg);
+    pm->send("pandaState", msg);
     panda->send_heartbeat();
     util::sleep_for(500);
   }
@@ -556,20 +537,28 @@ int main() {
   err = set_core_affinity(Hardware::TICI() ? 4 : 3);
   LOG("set affinity returns %d", err);
 
+  LOGW("attempting to connect");
+  PubMaster pm({"pandaState"});
+
   while (!do_exit) {
-    Panda *panda = nullptr;
-    std::vector<std::thread> threads;
-    threads.emplace_back(panda_state_thread, std::ref(panda), getenv("STARTED") != nullptr);
-
-    // connect to the board
-    panda = usb_retry_connect();
-    if (panda != nullptr) {
-      threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
-      threads.emplace_back(can_recv_thread, panda);
-      threads.emplace_back(hardware_control_thread, panda);
-      threads.emplace_back(pigeon_thread, panda);
+    Panda *panda = usb_connect();
+
+    // Send empty pandaState and try again
+    if (panda == nullptr) {
+      send_empty_panda_state(&pm);
+      util::sleep_for(500);
+      continue;
     }
 
+    LOGW("connected to board");
+
+    std::vector<std::thread> threads;
+    threads.emplace_back(panda_state_thread, &pm, panda, getenv("STARTED") != nullptr);
+    threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
+    threads.emplace_back(can_recv_thread, panda);
+    threads.emplace_back(hardware_control_thread, panda);
+    threads.emplace_back(pigeon_thread, panda);
+
     for (auto &t : threads) t.join();
 
     delete panda;