boardd: fix a panda may be connected by multiple panda instance (#22932)

* fix bugs in main

* no auto

selfdrive/boardd/boardd.cc

@@ -627,11 +627,7 @@ void pigeon_thread(Panda *panda) {
   delete pigeon;
 }
 
-int main(int argc, char* argv[]) {
+int main(int argc, char *argv[]) {
-  std::vector<std::thread> threads;
-  std::vector<Panda *> pandas;
-  Panda *peripheral_panda;
-
   LOGW("starting boardd");
 
   if (!Hardware::PC()) {
@@ -645,49 +641,42 @@ int main(int argc, char* argv[]) {
   LOGW("attempting to connect");
   PubMaster pm({"pandaStates", "peripheralState"});
 
-  // connect loop
+  std::vector<std::string> serials(argv + 1, argv + argc);
-  while (!do_exit) {
+  if (serials.size() == 0) serials.push_back("");
-    std::vector<std::string> serials(argv + 1, argv + argc);
-    if (serials.size() == 0) serials.push_back("");
-
-    // connect to all provided serials
-    for (int i=0; i<serials.size(); i++) {
-      Panda *p = usb_connect(serials[i], i);
-      if (p != NULL) {
-        pandas.push_back(p);
-      }
-    }
 
-    // send empty pandaState & peripheralState and try again
+  // connect to all provided serials
-    if (pandas.size() != serials.size()) {
+  std::vector<Panda *> pandas;
+  for (int i = 0; i < serials.size() && !do_exit; /**/) {
+    Panda *p = usb_connect(serials[i], i);
+    if (!p) {
+      // send empty pandaState & peripheralState and try again
       send_empty_panda_state(&pm);
       send_empty_peripheral_state(&pm);
       util::sleep_for(500);
-    } else {
+      continue;
-      break;
     }
-  }
 
-  if (pandas.size() == 0) {
+    pandas.push_back(p);
-    // do_exit was set while not connected to a panda
+    ++i;
-    return 0;
   }
 
-  peripheral_panda = pandas[0];
+  if (!do_exit) {
+    LOGW("connected to board");
+    Panda *peripheral_panda = pandas[0];
+    std::vector<std::thread> threads;
 
-  LOGW("connected to board");
+    threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
+    threads.emplace_back(peripheral_control_thread, peripheral_panda);
+    threads.emplace_back(pigeon_thread, peripheral_panda);
 
-  threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
+    threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
-  threads.emplace_back(peripheral_control_thread, peripheral_panda);
+    threads.emplace_back(can_recv_thread, pandas);
-  threads.emplace_back(pigeon_thread, peripheral_panda);
 
-  threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
+    for (auto &t : threads) t.join();
-  threads.emplace_back(can_recv_thread, pandas);
+  }
-
-  for (auto &t : threads) t.join();
 
   // we have exited, clean up pandas
-  for (const auto& panda : pandas){
+  for (Panda *panda : pandas) {
     delete panda;
   }
 }