diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 6248da7d4b..6f293d1c0d 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -472,6 +472,11 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin bool is_onroad_last = false; std::future safety_future; + std::vector connected_serials; + for (Panda *p : pandas) { + connected_serials.push_back(p->hw_serial()); + } + LOGD("start panda state thread"); // run at 2hz @@ -499,10 +504,15 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin LOGE("Reconnecting, communication to pandas not healthy"); do_exit = true; - // TODO: list is slow, takes 16ms - } else if (pandas.size() != Panda::list().size()) { - LOGW("Reconnecting to changed amount of pandas!"); - do_exit = true; + } else { + // check for new pandas + for (std::string &s : Panda::list(true)) { + if (!std::count(connected_serials.begin(), connected_serials.end(), s)) { + LOGW("Reconnecting to new panda: %s", s.c_str()); + do_exit = true; + break; + } + } } if (do_exit) { diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 1e7b510eca..72c3ea36ca 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -44,13 +44,15 @@ std::string Panda::hw_serial() { return handle->hw_serial; } -std::vector Panda::list() { +std::vector Panda::list(bool usb_only) { std::vector serials = PandaUsbHandle::list(); #ifndef __APPLE__ - for (auto s : PandaSpiHandle::list()) { - if (std::find(serials.begin(), serials.end(), s) == serials.end()) { - serials.push_back(s); + if (!usb_only) { + for (auto s : PandaSpiHandle::list()) { + if (std::find(serials.begin(), serials.end(), s) == serials.end()) { + serials.push_back(s); + } } } #endif diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 69df2e2b66..ea93229725 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -57,7 +57,7 @@ public: std::string hw_serial(); // Static functions - static std::vector list(); + static std::vector list(bool usb_only=false); // Panda functionality cereal::PandaState::PandaType get_hw_type();