diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index da1c3e905e..577b4aca8e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/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 pandas) { + for (const auto& panda : pandas) { + if (!panda->connected) return false; + } + return true; +} + +bool safety_setter_thread(std::vector 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 - panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1); + pandas[0]->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) { - return false; - }; + for (const auto& panda : pandas) { + 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) { - safety_model = safety_configs[0].getSafetyModel(); - safety_param = safety_configs[0].getSafetyParam(); - } else { - // If no safety mode is set, default to silent - safety_model = cereal::CarParams::SafetyModel::SILENT; - safety_param = 0; - } + for (uint32_t i=0; i i) { + safety_model = safety_configs[i].getSafetyModel(); + safety_param = safety_configs[i].getSafetyParam(); + } 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() { +Panda *usb_connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { - panda = std::make_unique(); + panda = std::make_unique(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) { - kj::Array 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) { +void can_send_thread(std::vector pandas, 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 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) { - can_recv(panda, pm); + while (!do_exit) { + if (!check_all_connected(pandas)){ + do_exit = true; + break; + } + + std::vector 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; isend("pandaStates", msg); } -bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) { - health_t pandaState = panda->get_state(); +bool send_panda_states(PubMaster *pm, std::vector pandas, bool spoofing_started) { + bool ignition_local = false; - if (spoofing_started) { - pandaState.ignition_line = 1; - } + // build msg + 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 - if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } + std::vector pandaStates; + for (const auto& panda : pandas){ + 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__ - bool power_save_desired = !ignition_local && !pigeon_active; - if (pandaState.power_save_enabled != power_save_desired) { - panda->set_power_saving(power_save_desired); - } + ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); - // 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); + pandaStates.push_back(pandaState); } -#endif - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy); + for (uint32_t i=0; iset_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 - auto ps = evt.initPandaStates(1); - ps[0].setUptime(pandaState.uptime); - ps[0].setIgnitionLine(pandaState.ignition_line); - ps[0].setIgnitionCan(pandaState.ignition_can); - ps[0].setControlsAllowed(pandaState.controls_allowed); - ps[0].setGasInterceptorDetected(pandaState.gas_interceptor_detected); - ps[0].setCanRxErrs(pandaState.can_rx_errs); - ps[0].setCanSendErrs(pandaState.can_send_errs); - ps[0].setCanFwdErrs(pandaState.can_fwd_errs); - ps[0].setGmlanSendErrs(pandaState.gmlan_send_errs); - ps[0].setPandaType(panda->hw_type); - ps[0].setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); - ps[0].setSafetyParam(pandaState.safety_param); - ps[0].setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); - ps[0].setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); - ps[0].setHeartbeatLost((bool)(pandaState.heartbeat_lost)); - ps[0].setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); - - // Convert faults bitset to capnp list - std::bitset fault_bits(pandaState.faults); - auto faults = ps[0].initFaults(fault_bits.count()); - - size_t i = 0; - 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(i, cereal::PandaState::FaultType(f)); - i++; + // TODO: do we still need this? + if (!panda->comms_healthy) { + evt.setValid(false); + } + + auto ps = pss[i]; + ps.setUptime(pandaState.uptime); + ps.setIgnitionLine(pandaState.ignition_line); + ps.setIgnitionCan(pandaState.ignition_can); + ps.setControlsAllowed(pandaState.controls_allowed); + ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); + ps.setCanRxErrs(pandaState.can_rx_errs); + ps.setCanSendErrs(pandaState.can_send_errs); + ps.setCanFwdErrs(pandaState.can_fwd_errs); + ps.setGmlanSendErrs(pandaState.gmlan_send_errs); + ps.setPandaType(panda->hw_type); + ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); + ps.setSafetyParam(pandaState.safety_param); + ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); + ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); + ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); + + // Convert faults bitset to capnp list + std::bitset fault_bits(pandaState.faults); + auto faults = ps.initFaults(fault_bits.count()); + + size_t j = 0; + 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 pandas, bool spoofing_started) { Params params; + Panda *peripheral_panda = pandas[0]; bool ignition_last = false; std::future 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 threads; + std::vector 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(); - Panda *peripheral_panda = panda; + std::vector serials(argv + 1, argv + argc); + if (serials.size() == 0) serials.push_back(""); + + // connect to all provided serials + for (int i=0; i threads; - threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, 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(peripheral_control_thread, peripheral_panda); + threads.emplace_back(pigeon_thread, peripheral_panda); - threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr); - threads.emplace_back(can_recv_thread, panda); + threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); + 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; } } diff --git a/selfdrive/boardd/can_list_to_can_capnp.cc b/selfdrive/boardd/can_list_to_can_capnp.cc index a4c9f3f6c8..faa0e37373 100644 --- a/selfdrive/boardd/can_list_to_can_capnp.cc +++ b/selfdrive/boardd/can_list_to_can_capnp.cc @@ -1,11 +1,5 @@ #include "cereal/messaging/messaging.h" - -typedef struct { - long address; - std::string dat; - long busTime; - long src; -} can_frame; +#include "panda.h" extern "C" { diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index c123d0c80e..5c86ae14a0 100644 --- a/selfdrive/boardd/panda.cc +++ b/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::Reader can_data_list) { - const int msg_count = can_data_list.size(); - const int buf_size = msg_count*0x10; + send.resize(4 * can_data_list.size()); - if (send.size() < buf_size) { - send.resize(buf_size); - } - - for (int i = 0; i < msg_count; i++) { + uint32_t msg_cnt = 0; + for (int i = 0; i < can_data_list.size(); 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); - memcpy(&send[i*4+2], can_data.begin(), can_data.size()); + send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4); + 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& out_buf) { +bool Panda::can_receive(std::vector& 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& 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; - auto evt = msg.initEvent(); - evt.setValid(comms_healthy); + out_vec.reserve(out_vec.size() + num_msg); - // populate message - auto canData = evt.initCan(num_msg); + // Populate messages 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); - int len = data[i*4+1]&0xF; - canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); - canData[i].setSrc((data[i*4+1] >> 4) & 0xff); + canData.busTime = data[i*4+1] >> 16; + int len = data[i*4+1] & 0xF; + canData.dat.assign((char *)&data[i*4+2], len); + 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; } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 36fd699be5..31bb9eb082 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -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 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 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::Reader can_data_list); - int can_receive(kj::Array& out_buf); + bool can_receive(std::vector& out_vec); }; diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index 83effbcc5b..242b9bac15 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -155,9 +155,6 @@ std::unordered_map 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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index aff6e1abdf..1c1d8ebfed 100755 --- a/selfdrive/controls/controlsd.py +++ b/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 pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled: - self.mismatch_counter += 1 + if any(not ps.controlsAllowed and self.enabled for ps in self.sm['pandaStates'] + if ps.safetyModel not in IGNORED_SAFETY_MODES): + self.mismatch_counter += 1 self.distance_traveled += CS.vEgo * DT_CTRL diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 9264ecc370..b1b04db41e 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/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 diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 17282fcbfd..b725ca9dda 100755 --- a/selfdrive/pandad.py +++ b/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: - panda = None - panda_dfu = None +def flash_panda(panda_serial : str) -> Panda: + panda = Panda(panda_serial) - cloudlog.info("Connecting to 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 + fw_signature = get_expected_signature(panda) 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(), - fw_signature.hex(), + panda_signature.hex()[:16], + 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: - panda = update_panda() - - # check health for lost heartbeat - health = panda.health() - if health["heartbeat_lost"]: - Params().put_bool("PandaHeartbeatLost", True) - cloudlog.event("heartbeat lost", deviceState=health) - - cloudlog.info("Resetting panda") - panda.reset() + # make sure the internal one is always first + if a.is_internal() and not b.is_internal(): + return -1 + if not a.is_internal() and b.is_internal(): + return 1 - os.chdir(os.path.join(BASEDIR, "selfdrive/boardd")) - os.execvp("./boardd", ["./boardd"]) + # sort by hardware type + if a_type != b_type: + return a_type < b_type + + # last resort: sort by serial number + 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() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 767b66c691..69098de7cf 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -4ad66f0fea4c42e79ba347a73ff839e5369c0411 \ No newline at end of file +f1f065451899f9b8e1cec379b32b970c1c16f849 \ No newline at end of file diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 71c172257b..e9e33c9da7 100755 --- a/selfdrive/thermald/thermald.py +++ b/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 \