Multipanda support (#22402)

* squash all PR commits for easy rebase

* merged all panda rx can into one message

* fix buffers in can_send

* more cleanup and minor fixes

* fix even more stuff

* fix non-allocated send buffer

* make connecting more robust

* fix bus offset

* fix controls_mismatch?

* simplify mismatch check

* C++ style struct

* fix connect loop

* update ref

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: 585c16cd2a

selfdrive/boardd/boardd.cc

@@ -31,6 +31,25 @@
 #include "selfdrive/boardd/panda.h"
 #include "selfdrive/boardd/pigeon.h"
 
+// -- Multi-panda conventions --
+// Ordering:
+// - The internal panda will always be the first panda
+// - Consecutive pandas will be sorted based on panda type, and then serial number
+// Connecting:
+// - If a panda connection is dropped, boardd wil reconnect to all pandas
+// - If a panda is added, we will only reconnect when we are offroad
+// CAN buses:
+// - Each panda will have it's block of 4 buses. E.g.: the second panda will use
+//   bus numbers 4, 5, 6 and 7
+// - The internal panda will always be used for accessing the OBD2 port,
+//   and thus firmware queries
+// Safety:
+// - SafetyConfig is a list, which is mapped to the connected pandas
+// - If there are more pandas connected than there are SafetyConfigs,
+//   the excess pandas will remain in "silent" ot "noOutput" mode
+// Ignition:
+// - If any of the ignition sources in any panda is high, ignition is high
+
 #define MAX_IR_POWER 0.5f
 #define MIN_IR_POWER 0.0f
 #define CUTOFF_IL 200
@@ -49,18 +68,30 @@ std::string get_time_str(const struct tm &time) {
   return s;
 }
 
-bool safety_setter_thread(Panda *panda) {
+bool check_all_connected(std::vector<Panda *> pandas) {
+  for (const auto& panda : pandas) {
+    if (!panda->connected) return false;
+  }
+  return true;
+}
+
+bool safety_setter_thread(std::vector<Panda *> pandas) {
   LOGD("Starting safety setter thread");
-  // diagnostic only is the default, needed for VIN query
+
-  panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
+  // there should be at least one panda connected
+  if (pandas.size() == 0) {
+    return false;
+  }
+
+  pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
 
   Params p = Params();
 
   // switch to SILENT when CarVin param is read
   while (true) {
-    if (do_exit || !panda->connected || !ignition) {
+    if (do_exit || !check_all_connected(pandas) || !ignition) {
       return false;
-    };
+    }
 
     std::string value_vin = p.get("CarVin");
     if (value_vin.size() > 0) {
@@ -72,15 +103,16 @@ bool safety_setter_thread(Panda *panda) {
     util::sleep_for(20);
   }
 
-  // VIN query done, stop listening to OBDII
+  pandas[0]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
-  panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1);
 
   std::string params;
   LOGW("waiting for params to set safety model");
   while (true) {
-    if (do_exit || !panda->connected || !ignition) {
+    for (const auto& panda : pandas) {
-      return false;
+      if (do_exit || !panda->connected || !ignition) {
-    };
+        return false;
+      }
+    }
 
     if (p.getBool("ControlsReady")) {
       params = p.get("CarParams");
@@ -97,27 +129,31 @@ bool safety_setter_thread(Panda *panda) {
   int safety_param;
 
   auto safety_configs = car_params.getSafetyConfigs();
-  if (safety_configs.size() > 0) {
+  for (uint32_t i=0; i<pandas.size(); i++) {
-    safety_model = safety_configs[0].getSafetyModel();
+    auto panda = pandas[i];
-    safety_param = safety_configs[0].getSafetyParam();
+
-  } else {
+    if (safety_configs.size() > i) {
-    // If no safety mode is set, default to silent
+      safety_model = safety_configs[i].getSafetyModel();
-    safety_model = cereal::CarParams::SafetyModel::SILENT;
+      safety_param = safety_configs[i].getSafetyParam();
-    safety_param = 0;
+    } else {
-  }
+      // If no safety mode is specified, default to silent
+      safety_model = cereal::CarParams::SafetyModel::SILENT;
+      safety_param = 0;
+    }
+
+    LOGW("panda %d: setting safety model: %d with param %d", i, (int)safety_model, safety_param);
 
-  panda->set_unsafe_mode(0);  // see safety_declarations.h for allowed values
+    panda->set_unsafe_mode(0);  // see safety_declarations.h for allowed values
+    panda->set_safety_model(safety_model, safety_param);
+  }
 
-  LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param);
-  panda->set_safety_model(safety_model, safety_param);
   return true;
 }
 
-
+Panda *usb_connect(std::string serial="", uint32_t index=0) {
-Panda *usb_connect() {
   std::unique_ptr<Panda> panda;
   try {
-    panda = std::make_unique<Panda>();
+    panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
   } catch (std::exception &e) {
     return nullptr;
   }
@@ -128,27 +164,6 @@ Panda *usb_connect() {
     panda->set_loopback(true);
   }
 
-  if (auto fw_sig = panda->get_firmware_version(); fw_sig) {
-    params.put("PandaFirmware", (const char *)fw_sig->data(), fw_sig->size());
-
-    // Convert to hex for offroad
-    char fw_sig_hex_buf[16] = {0};
-    const uint8_t *fw_sig_buf = fw_sig->data();
-    for (size_t i = 0; i < 8; i++) {
-      fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] >> 4);
-      fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX((uint8_t)fw_sig_buf[i] & 0xF);
-    }
-
-    params.put("PandaFirmwareHex", fw_sig_hex_buf, 16);
-    LOGW("fw signature: %.*s", 16, fw_sig_hex_buf);
-  } else { return nullptr; }
-
-  // get panda serial
-  if (auto serial = panda->get_serial(); serial) {
-    params.put("PandaDongleId", serial->c_str(), serial->length());
-    LOGW("panda serial: %s", serial->c_str());
-  } else { return nullptr; }
-
   // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
 #ifndef __x86_64__
   static std::once_flag connected_once;
@@ -171,14 +186,7 @@ Panda *usb_connect() {
   return panda.release();
 }
 
-void can_recv(Panda *panda, PubMaster &pm) {
+void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
-  kj::Array<capnp::word> can_data;
-  panda->can_receive(can_data);
-  auto bytes = can_data.asBytes();
-  pm.send("can", bytes.begin(), bytes.size());
-}
-
-void can_send_thread(Panda *panda, bool fake_send) {
   LOGD("start send thread");
 
   AlignedBuffer aligned_buf;
@@ -188,7 +196,12 @@ void can_send_thread(Panda *panda, bool fake_send) {
   subscriber->setTimeout(100);
 
   // run as fast as messages come in
-  while (!do_exit && panda->connected) {
+  while (!do_exit) {
+    if (!check_all_connected(pandas)) {
+      do_exit = true;
+      break;
+    }
+
     Message * msg = subscriber->receive();
 
     if (!msg) {
@@ -204,7 +217,9 @@ void can_send_thread(Panda *panda, bool fake_send) {
     //Dont send if older than 1 second
     if (nanos_since_boot() - event.getLogMonoTime() < 1e9) {
       if (!fake_send) {
-        panda->can_send(event.getSendcan());
+        for (const auto& panda : pandas) {
+          panda->can_send(event.getSendcan());
+        }
       }
     }
 
@@ -215,7 +230,7 @@ void can_send_thread(Panda *panda, bool fake_send) {
   delete context;
 }
 
-void can_recv_thread(Panda *panda) {
+void can_recv_thread(std::vector<Panda *> pandas) {
   LOGD("start recv thread");
 
   // can = 8006
@@ -225,8 +240,29 @@ void can_recv_thread(Panda *panda) {
   const uint64_t dt = 10000000ULL;
   uint64_t next_frame_time = nanos_since_boot() + dt;
 
-  while (!do_exit && panda->connected) {
+  while (!do_exit) {
-    can_recv(panda, pm);
+    if (!check_all_connected(pandas)){
+      do_exit = true;
+      break;
+    }
+
+    std::vector<can_frame> raw_can_data;
+    bool comms_healthy = true;
+    for (const auto& panda : pandas) {
+      comms_healthy &= panda->can_receive(raw_can_data);
+    }
+
+    MessageBuilder msg;
+    auto evt = msg.initEvent();
+    evt.setValid(comms_healthy);
+    auto canData = evt.initCan(raw_can_data.size());
+    for (uint i = 0; i<raw_can_data.size(); i++) {
+      canData[i].setAddress(raw_can_data[i].address);
+      canData[i].setBusTime(raw_can_data[i].busTime);
+      canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
+      canData[i].setSrc(raw_can_data[i].src);
+    }
+    pm.send("can", msg);
 
     uint64_t cur_time = nanos_since_boot();
     int64_t remaining = next_frame_time - cur_time;
@@ -257,70 +293,86 @@ void send_empty_panda_state(PubMaster *pm) {
   pm->send("pandaStates", msg);
 }
 
-bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) {
+bool send_panda_states(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
-  health_t pandaState = panda->get_state();
+  bool ignition_local = false;
 
-  if (spoofing_started) {
+  // build msg
-    pandaState.ignition_line = 1;
+  MessageBuilder msg;
-  }
+  auto evt = msg.initEvent();
+  auto pss = evt.initPandaStates(pandas.size());
 
-  // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
+  std::vector<health_t> pandaStates;
-  if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+  for (const auto& panda : pandas){
-    panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    health_t pandaState = panda->get_state();
-  }
 
-  bool ignition_local = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
+    if (spoofing_started) {
+      pandaState.ignition_line = 1;
+    }
 
-#ifndef __x86_64__
+    ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
-  bool power_save_desired = !ignition_local && !pigeon_active;
-  if (pandaState.power_save_enabled != power_save_desired) {
-    panda->set_power_saving(power_save_desired);
-  }
 
-  // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
+    pandaStates.push_back(pandaState);
-  if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
-    panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
   }
-#endif
 
-  // build msg
+  for (uint32_t i=0; i<pandas.size(); i++) {
-  MessageBuilder msg;
+    auto panda = pandas[i];
-  auto evt = msg.initEvent();
+    auto pandaState = pandaStates[i];
-  evt.setValid(panda->comms_healthy);
+
+    // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
+    if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    }
+
+  #ifndef __x86_64__
+    bool power_save_desired = !ignition_local && !pigeon_active;
+    if (pandaState.power_save_enabled != power_save_desired) {
+      panda->set_power_saving(power_save_desired);
+    }
+
+    // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
+    if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    }
+  #endif
 
-  // TODO: this has to be adapted to merge in multipanda support
+    // TODO: do we still need this?
-  auto ps = evt.initPandaStates(1);
+    if (!panda->comms_healthy) {
-  ps[0].setUptime(pandaState.uptime);
+      evt.setValid(false);
-  ps[0].setIgnitionLine(pandaState.ignition_line);
+    }
-  ps[0].setIgnitionCan(pandaState.ignition_can);
+
-  ps[0].setControlsAllowed(pandaState.controls_allowed);
+    auto ps = pss[i];
-  ps[0].setGasInterceptorDetected(pandaState.gas_interceptor_detected);
+    ps.setUptime(pandaState.uptime);
-  ps[0].setCanRxErrs(pandaState.can_rx_errs);
+    ps.setIgnitionLine(pandaState.ignition_line);
-  ps[0].setCanSendErrs(pandaState.can_send_errs);
+    ps.setIgnitionCan(pandaState.ignition_can);
-  ps[0].setCanFwdErrs(pandaState.can_fwd_errs);
+    ps.setControlsAllowed(pandaState.controls_allowed);
-  ps[0].setGmlanSendErrs(pandaState.gmlan_send_errs);
+    ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
-  ps[0].setPandaType(panda->hw_type);
+    ps.setCanRxErrs(pandaState.can_rx_errs);
-  ps[0].setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
+    ps.setCanSendErrs(pandaState.can_send_errs);
-  ps[0].setSafetyParam(pandaState.safety_param);
+    ps.setCanFwdErrs(pandaState.can_fwd_errs);
-  ps[0].setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
+    ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
-  ps[0].setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
+    ps.setPandaType(panda->hw_type);
-  ps[0].setHeartbeatLost((bool)(pandaState.heartbeat_lost));
+    ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
-  ps[0].setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
+    ps.setSafetyParam(pandaState.safety_param);
-
+    ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
-  // Convert faults bitset to capnp list
+    ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
-  std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
+    ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
-  auto faults = ps[0].initFaults(fault_bits.count());
+    ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
-
+
-  size_t i = 0;
+    // Convert faults bitset to capnp list
-  for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
+    std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
-      f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+    auto faults = ps.initFaults(fault_bits.count());
-    if (fault_bits.test(f)) {
+
-      faults.set(i, cereal::PandaState::FaultType(f));
+    size_t j = 0;
-      i++;
+    for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
+        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+      if (fault_bits.test(f)) {
+        faults.set(j, cereal::PandaState::FaultType(f));
+        j++;
+      }
     }
   }
-  pm->send("pandaStates", msg);
 
+  pm->send("pandaStates", msg);
   return ignition_local;
 }
 
@@ -355,23 +407,36 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
   pm->send("peripheralState", msg);
 }
 
-void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, bool spoofing_started) {
+void panda_state_thread(PubMaster *pm, std::vector<Panda *> pandas, bool spoofing_started) {
   Params params;
+  Panda *peripheral_panda = pandas[0];
   bool ignition_last = false;
   std::future<bool> safety_future;
 
   LOGD("start panda state thread");
 
   // run at 2hz
-  while (!do_exit && panda->connected) {
+  while (!do_exit) {
+    if(!check_all_connected(pandas)) {
+      do_exit = true;
+      break;
+    }
+
     send_peripheral_state(pm, peripheral_panda);
-    ignition = send_panda_state(pm, panda, spoofing_started);
+    ignition = send_panda_states(pm, pandas, spoofing_started);
+
+    // check if we have new pandas and are offroad
+    if (!ignition && (pandas.size() != Panda::list().size())) {
+      LOGW("Reconnecting to changed amount of pandas!");
+      do_exit = true;
+      break;
+    }
 
     // clear VIN, CarParams, and set new safety on car start
     if (ignition && !ignition_last) {
       params.clearAll(CLEAR_ON_IGNITION_ON);
       if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
-        safety_future = std::async(std::launch::async, safety_setter_thread, panda);
+        safety_future = std::async(std::launch::async, safety_setter_thread, pandas);
       } else {
         LOGW("Safety setter thread already running");
       }
@@ -381,7 +446,9 @@ void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, b
 
     ignition_last = ignition;
 
-    panda->send_heartbeat();
+    for (const auto &panda : pandas) {
+      panda->send_heartbeat();
+    }
     util::sleep_for(500);
   }
 }
@@ -561,7 +628,11 @@ void pigeon_thread(Panda *panda) {
   delete pigeon;
 }
 
-int main() {
+int main(int argc, char* argv[]) {
+  std::vector<std::thread> threads;
+  std::vector<Panda *> pandas;
+  Panda *peripheral_panda;
+
   LOGW("starting boardd");
 
   if (!Hardware::PC()) {
@@ -575,31 +646,44 @@ int main() {
   LOGW("attempting to connect");
   PubMaster pm({"pandaStates", "peripheralState"});
 
+  // connect loop
   while (!do_exit) {
-    Panda *panda = usb_connect();
+    std::vector<std::string> serials(argv + 1, argv + argc);
-    Panda *peripheral_panda = panda;
+    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
+    // send empty pandaState & peripheralState and try again
-    if (panda == nullptr || peripheral_panda == nullptr) {
+    if (pandas.size() != serials.size()) {
       send_empty_panda_state(&pm);
       send_empty_peripheral_state(&pm);
       util::sleep_for(500);
-      continue;
+    } else {
+      break;
     }
+  }
+
+  peripheral_panda = pandas[0];
 
-    LOGW("connected to board");
+  LOGW("connected to board");
 
-    std::vector<std::thread> threads;
+  threads.emplace_back(panda_state_thread, &pm, pandas, getenv("STARTED") != nullptr);
-    threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, getenv("STARTED") != nullptr);
+  threads.emplace_back(peripheral_control_thread, peripheral_panda);
-    threads.emplace_back(peripheral_control_thread, peripheral_panda);
+  threads.emplace_back(pigeon_thread, peripheral_panda);
-    threads.emplace_back(pigeon_thread, peripheral_panda);
 
-    threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
+  threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
-    threads.emplace_back(can_recv_thread, panda);
+  threads.emplace_back(can_recv_thread, pandas);
 
-    for (auto &t : threads) t.join();
+  for (auto &t : threads) t.join();
 
+  // we have exited, clean up pandas
+  for (const auto& panda : pandas){
     delete panda;
-    panda = nullptr;
   }
 }

selfdrive/boardd/can_list_to_can_capnp.cc

@@ -1,11 +1,5 @@
 #include "cereal/messaging/messaging.h"
-
+#include "panda.h"
-typedef struct {
-	long address;
-	std::string dat;
-	long busTime;
-	long src;
-} can_frame;
 
 extern "C" {
 

selfdrive/boardd/panda.cc

@@ -30,7 +30,7 @@ static int init_usb_ctx(libusb_context **context) {
 }
 
 
-Panda::Panda(std::string serial) {
+Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
   // init libusb
   ssize_t num_devices;
   libusb_device **dev_list = NULL;
@@ -344,30 +344,35 @@ void Panda::send_heartbeat() {
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
-  const int msg_count = can_data_list.size();
+  send.resize(4 * can_data_list.size());
-  const int buf_size = msg_count*0x10;
 
-  if (send.size() < buf_size) {
+  uint32_t msg_cnt = 0;
-    send.resize(buf_size);
+  for (int i = 0; i < can_data_list.size(); i++) {
-  }
-
-  for (int i = 0; i < msg_count; i++) {
     auto cmsg = can_data_list[i];
+
+    // check if the message is intended for this panda
+    uint8_t bus = cmsg.getSrc();
+    if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
+      continue;
+    }
+
     if (cmsg.getAddress() >= 0x800) { // extended
-      send[i*4] = (cmsg.getAddress() << 3) | 5;
+      send[msg_cnt*4] = (cmsg.getAddress() << 3) | 5;
     } else { // normal
-      send[i*4] = (cmsg.getAddress() << 21) | 1;
+      send[msg_cnt*4] = (cmsg.getAddress() << 21) | 1;
     }
     auto can_data = cmsg.getDat();
     assert(can_data.size() <= 8);
-    send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
+    send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4);
-    memcpy(&send[i*4+2], can_data.begin(), can_data.size());
+    memcpy(&send[msg_cnt*4+2], can_data.begin(), can_data.size());
+
+    msg_cnt++;
   }
 
-  usb_bulk_write(3, (unsigned char*)send.data(), buf_size, 5);
+  usb_bulk_write(3, (unsigned char*)send.data(), msg_cnt * 0x10, 5);
 }
 
-int Panda::can_receive(kj::Array<capnp::word>& out_buf) {
+bool Panda::can_receive(std::vector<can_frame>& out_vec) {
   uint32_t data[RECV_SIZE/4];
   int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
 
@@ -378,27 +383,34 @@ int Panda::can_receive(kj::Array<capnp::word>& out_buf) {
     LOGW("Receive buffer full");
   }
 
+  if (!comms_healthy) {
+    return false;
+  }
+
+  // Append to the end of the out_vec, such that we can pass it to multiple pandas
+  // We already insert space for all the messages here for speed
   size_t num_msg = recv / 0x10;
-  MessageBuilder msg;
+  out_vec.reserve(out_vec.size() + num_msg);
-  auto evt = msg.initEvent();
-  evt.setValid(comms_healthy);
 
-  // populate message
+  // Populate messages
-  auto canData = evt.initCan(num_msg);
   for (int i = 0; i < num_msg; i++) {
+    can_frame canData;
     if (data[i*4] & 4) {
       // extended
-      canData[i].setAddress(data[i*4] >> 3);
+      canData.address = data[i*4] >> 3;
       //printf("got extended: %x\n", data[i*4] >> 3);
     } else {
       // normal
-      canData[i].setAddress(data[i*4] >> 21);
+      canData.address = data[i*4] >> 21;
     }
-    canData[i].setBusTime(data[i*4+1] >> 16);
+    canData.busTime = data[i*4+1] >> 16;
-    int len = data[i*4+1]&0xF;
+    int len = data[i*4+1] & 0xF;
-    canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
+    canData.dat.assign((char *)&data[i*4+2], len);
-    canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
+    canData.src = ((data[i*4+1] >> 4) & 0xff) + bus_offset;
+
+    // add to vector
+    out_vec.push_back(canData);
   }
-  out_buf = capnp::messageToFlatArray(msg);
+
-  return recv;
+  return true;
 }

selfdrive/boardd/panda.h

@@ -3,6 +3,7 @@
 #include <atomic>
 #include <cstdint>
 #include <ctime>
+#include <list>
 #include <mutex>
 #include <optional>
 #include <vector>
@@ -15,6 +16,7 @@
 // double the FIFO size
 #define RECV_SIZE (0x1000)
 #define TIMEOUT 0
+#define PANDA_BUS_CNT 4
 
 // copied from panda/board/main.c
 struct __attribute__((packed)) health_t {
@@ -39,6 +41,12 @@ struct __attribute__((packed)) health_t {
   uint8_t heartbeat_lost;
 };
 
+struct can_frame {
+	long address;
+	std::string dat;
+	long busTime;
+	long src;
+};
 
 class Panda {
  private:
@@ -50,7 +58,7 @@ class Panda {
   void cleanup();
 
  public:
-  Panda(std::string serial="");
+  Panda(std::string serial="", uint32_t bus_offset=0);
   ~Panda();
 
   std::string usb_serial;
@@ -58,6 +66,7 @@ class Panda {
   std::atomic<bool> comms_healthy = true;
   cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
   bool has_rtc = false;
+  const uint32_t bus_offset;
 
   // Static functions
   static std::vector<std::string> list();
@@ -85,5 +94,5 @@ class Panda {
   void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
   void send_heartbeat();
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
-  int can_receive(kj::Array<capnp::word>& out_buf);
+  bool can_receive(std::vector<can_frame>& out_vec);
 };

selfdrive/common/params.cc

@@ -155,9 +155,6 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"NavSettingTime24h", PERSISTENT},
     {"OpenpilotEnabledToggle", PERSISTENT},
-    {"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
-    {"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
-    {"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"Passive", PERSISTENT},
     {"PrimeRedirected", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -51,6 +51,8 @@ EventName = car.CarEvent.EventName
 ButtonEvent = car.CarState.ButtonEvent
 SafetyModel = car.CarParams.SafetyModel
 
+IGNORED_SAFETY_MODES = [SafetyModel.silent, SafetyModel.noOutput]
+
 
 class Controls:
   def __init__(self, sm=None, pm=None, can_sock=None):
@@ -247,14 +249,17 @@ class Controls:
       self.events.add(EventName.canError)
 
     for i, pandaState in enumerate(self.sm['pandaStates']):
-      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent
+      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput
       if i < len(self.CP.safetyConfigs):
         safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
       else:
-        safety_mismatch = pandaState.safetyModel != SafetyModel.silent
+        safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES
       if safety_mismatch or self.mismatch_counter >= 200:
         self.events.add(EventName.controlsMismatch)
 
+      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
+        self.events.add(EventName.relayMalfunction)
+
     if not self.sm['liveParameters'].valid:
       self.events.add(EventName.vehicleModelInvalid)
 
@@ -370,9 +375,9 @@ class Controls:
       self.mismatch_counter = 0
 
     # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
-    for pandaState in self.sm['pandaStates']:
+    if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates']
-      if pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled:
+           if ps.safetyModel not in IGNORED_SAFETY_MODES):
-        self.mismatch_counter += 1
+      self.mismatch_counter += 1
 
     self.distance_traveled += CS.vEgo * DT_CTRL
 

selfdrive/controls/lib/alerts_offroad.json

@@ -21,10 +21,6 @@
     "severity": 1,
     "_comment": "Append the command and error to the text."
   },
-  "Offroad_PandaFirmwareMismatch": {
-    "text": "Unexpected panda firmware version. System won't start. Reboot your device to reflash panda.",
-    "severity": 1
-  },
   "Offroad_InvalidTime": {
     "text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
     "severity": 1

selfdrive/pandad.py

@@ -1,61 +1,40 @@
 #!/usr/bin/env python3
 # simple boardd wrapper that updates the panda first
 import os
+import usb1
 import time
+import subprocess
+from typing import List
+from functools import cmp_to_key
 
-from panda import BASEDIR as PANDA_BASEDIR, Panda, PandaDFU
+from panda import DEFAULT_FW_FN, DEFAULT_H7_FW_FN, MCU_TYPE_H7, Panda, PandaDFU
 from common.basedir import BASEDIR
 from common.params import Params
 from selfdrive.swaglog import cloudlog
 
-PANDA_FW_FN = os.path.join(PANDA_BASEDIR, "board", "obj", "panda.bin.signed")
 
+def get_expected_signature(panda : Panda) -> bytes:
+  fn = DEFAULT_H7_FW_FN if (panda.get_mcu_type() == MCU_TYPE_H7) else DEFAULT_FW_FN
 
-def get_expected_signature() -> bytes:
   try:
-    return Panda.get_signature_from_firmware(PANDA_FW_FN)
+    return Panda.get_signature_from_firmware(fn)
   except Exception:
     cloudlog.exception("Error computing expected signature")
     return b""
 
 
-def update_panda() -> Panda:
+def flash_panda(panda_serial : str) -> Panda:
-  panda = None
+  panda = Panda(panda_serial)
-  panda_dfu = None
 
-  cloudlog.info("Connecting to panda")
+  fw_signature = get_expected_signature(panda)
-
-  while True:
-    # break on normal mode Panda
-    panda_list = Panda.list()
-    if len(panda_list) > 0:
-      cloudlog.info("Panda found, connecting")
-      panda = Panda(panda_list[0])
-      break
-
-    # flash on DFU mode Panda
-    panda_dfu = PandaDFU.list()
-    if len(panda_dfu) > 0:
-      cloudlog.info("Panda in DFU mode found, flashing recovery")
-      panda_dfu = PandaDFU(panda_dfu[0])
-      panda_dfu.recover()
-
-    time.sleep(1)
-
-  fw_signature = get_expected_signature()
-
-  try:
-    serial = panda.get_serial()[0].decode("utf-8")
-  except Exception:
-    serial = None
 
   panda_version = "bootstub" if panda.bootstub else panda.get_version()
   panda_signature = b"" if panda.bootstub else panda.get_signature()
   cloudlog.warning("Panda %s connected, version: %s, signature %s, expected %s" % (
-    serial,
+    panda_serial,
     panda_version,
-    panda_signature.hex(),
+    panda_signature.hex()[:16],
-    fw_signature.hex(),
+    fw_signature.hex()[:16],
   ))
 
   if panda.bootstub or panda_signature != fw_signature:
@@ -80,22 +59,68 @@ def update_panda() -> Panda:
 
   return panda
 
+def panda_sort_cmp(a : Panda, b : Panda):
+  a_type = a.get_type()
+  b_type = b.get_type()
 
-def main() -> None:
+  # make sure the internal one is always first
-  panda = update_panda()
+  if a.is_internal() and not b.is_internal():
-
+    return -1
-  # check health for lost heartbeat
+  if not a.is_internal() and b.is_internal():
-  health = panda.health()
+    return 1
-  if health["heartbeat_lost"]:
-    Params().put_bool("PandaHeartbeatLost", True)
-    cloudlog.event("heartbeat lost", deviceState=health)
 
-  cloudlog.info("Resetting panda")
+  # sort by hardware type
-  panda.reset()
+  if a_type != b_type:
+    return a_type < b_type
   
-  os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
+  # last resort: sort by serial number
-  os.execvp("./boardd", ["./boardd"])
+  return a.get_usb_serial() < b.get_usb_serial()
 
+def main() -> None:
+  while True:
+    try:
+      # Flash all Pandas in DFU mode
+      for p in PandaDFU.list():
+        cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}")
+        PandaDFU(p).recover()
+      time.sleep(1)
+
+      panda_serials = Panda.list()
+      if len(panda_serials) == 0:
+        continue
+
+      cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
+
+      # Flash pandas
+      pandas = []
+      for serial in panda_serials:
+        pandas.append(flash_panda(serial))
+
+      # check health for lost heartbeat
+      for panda in pandas:
+        health = panda.health()
+        if health["heartbeat_lost"]:
+          Params().put_bool("PandaHeartbeatLost", True)
+          cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
+
+        cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
+        panda.reset()
+
+      # sort pandas to have deterministic order
+      pandas.sort(key=cmp_to_key(panda_sort_cmp))
+      panda_serials = list(map(lambda p: p.get_usb_serial(), pandas))
+
+      # close all pandas
+      for p in pandas:
+        p.close()
+    except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
+      # a panda was disconnected while setting everything up. let's try again
+      cloudlog.exception("Panda USB exception while setting up")
+      continue
+
+    # run boardd with all connected serials as arguments
+    os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
+    subprocess.run(["./boardd", *panda_serials], check=True)
 
 if __name__ == "__main__":
   main()

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-4ad66f0fea4c42e79ba347a73ff839e5369c0411
+f1f065451899f9b8e1cec379b32b970c1c16f849

selfdrive/thermald/thermald.py

@@ -20,13 +20,10 @@ from selfdrive.controls.lib.alertmanager import set_offroad_alert
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.hardware import EON, TICI, PC, HARDWARE
 from selfdrive.loggerd.config import get_available_percent
-from selfdrive.pandad import get_expected_signature
 from selfdrive.swaglog import cloudlog
 from selfdrive.thermald.power_monitoring import PowerMonitoring
 from selfdrive.version import tested_branch, terms_version, training_version
 
-FW_SIGNATURE = get_expected_signature()
-
 ThermalStatus = log.DeviceState.ThermalStatus
 NetworkType = log.DeviceState.NetworkType
 NetworkStrength = log.DeviceState.NetworkStrength
@@ -364,10 +361,6 @@ def thermald_thread():
     startup_conditions["not_uninstalling"] = not params.get_bool("DoUninstall")
     startup_conditions["accepted_terms"] = params.get("HasAcceptedTerms") == terms_version
 
-    panda_signature = params.get("PandaFirmware")
-    startup_conditions["fw_version_match"] = (panda_signature is None) or (panda_signature == FW_SIGNATURE)   # don't show alert is no panda is connected (None)
-    set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"]))
-
     # with 2% left, we killall, otherwise the phone will take a long time to boot
     startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 2
     startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \