diff --git a/Jenkinsfile b/Jenkinsfile index c095eda8a..6f81755d4 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -216,12 +216,6 @@ node { step("test manager", "pytest system/manager/test/test_manager.py"), ]) }, - 'loopback': { - deviceStage("loopback", "tizi-loopback", ["UNSAFE=1"], [ - step("build openpilot", "cd system/manager && ./build.py"), - step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"), - ]) - }, 'camerad OX03C10': { deviceStage("OX03C10", "tizi-ox03c10", ["UNSAFE=1"], [ step("build", "cd system/manager && ./build.py"), @@ -251,11 +245,9 @@ node { 'tizi': { deviceStage("tizi", "tizi", ["UNSAFE=1"], [ step("build openpilot", "cd system/manager && ./build.py"), - step("test pandad loopback", "SINGLE_PANDA=1 pytest selfdrive/pandad/tests/test_pandad_loopback.py"), + step("test pandad loopback", "pytest selfdrive/pandad/tests/test_pandad_loopback.py"), step("test pandad spi", "pytest selfdrive/pandad/tests/test_pandad_spi.py"), step("test amp", "pytest system/hardware/tici/tests/test_amplifier.py"), - // TODO: enable once new AGNOS is available - // step("test esim", "pytest system/hardware/tici/tests/test_esim.py"), step("test qcomgpsd", "pytest system/qcomgpsd/tests/test_qcomgpsd.py", [diffPaths: ["system/qcomgpsd/"]]), ]) }, diff --git a/selfdrive/pandad/.gitignore b/selfdrive/pandad/.gitignore index f7226cdb8..cb292405c 100644 --- a/selfdrive/pandad/.gitignore +++ b/selfdrive/pandad/.gitignore @@ -1,3 +1,3 @@ pandad pandad_api_impl.cpp -tests/test_pandad_usbprotocol +tests/test_pandad_canprotocol diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index 5e0b782c1..fd59db985 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -1,9 +1,10 @@ -Import('env', 'common', 'messaging') +Import('env', 'arch', 'common', 'messaging') -libs = ['usb-1.0', common, messaging, 'pthread'] -panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) +if arch != "Darwin": + libs = [common, messaging, 'pthread'] + panda = env.Library('panda', ['panda.cc', 'spi.cc']) -env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) + env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) -if GetOption('extras'): - env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs) + if GetOption('extras'): + env.Program('tests/test_pandad_canprotocol', ['tests/test_pandad_canprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/pandad/main.cc b/selfdrive/pandad/main.cc index b63d884a4..ef30d6037 100644 --- a/selfdrive/pandad/main.cc +++ b/selfdrive/pandad/main.cc @@ -16,7 +16,7 @@ int main(int argc, char *argv[]) { assert(err == 0); } - std::vector serials(argv + 1, argv + argc); - pandad_main_thread(serials); + std::string serial = (argc > 1) ? argv[1] : ""; + pandad_main_thread(serial); return 0; } diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc index 93e139f0e..edc2228c0 100644 --- a/selfdrive/pandad/panda.cc +++ b/selfdrive/pandad/panda.cc @@ -12,19 +12,9 @@ const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr; -Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // try USB first, then SPI - try { - handle = std::make_unique(serial); - LOGW("connected to %s over USB", serial.c_str()); - } catch (std::exception &e) { -#ifndef __APPLE__ - handle = std::make_unique(serial); - LOGW("connected to %s over SPI", serial.c_str()); -#else - throw e; -#endif - } +Panda::Panda(std::string serial) { + handle = std::make_unique(serial); + LOGW("connected to %s over SPI", serial.c_str()); hw_type = get_hw_type(); can_reset_communications(); @@ -42,20 +32,8 @@ std::string Panda::hw_serial() { return handle->hw_serial; } -std::vector Panda::list(bool usb_only) { - std::vector serials = PandaUsbHandle::list(); - -#ifndef __APPLE__ - if (!usb_only) { - for (const auto &s : PandaSpiHandle::list()) { - if (std::find(serials.begin(), serials.end(), s) == serials.end()) { - serials.push_back(s); - } - } - } -#endif - - return serials; +std::vector Panda::list() { + return PandaSpiHandle::list(); } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { @@ -195,7 +173,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data for (const auto &cmsg : can_data_list) { // check if the message is intended for this panda uint8_t bus = cmsg.getSrc(); - if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_OFFSET)) { + if (bus >= PANDA_BUS_OFFSET) { continue; } auto can_data = cmsg.getDat(); @@ -207,7 +185,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data header.addr = cmsg.getAddress(); header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; header.data_len_code = data_len_code; - header.bus = bus - bus_offset; + header.bus = bus; header.checksum = 0; memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header)); @@ -283,7 +261,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector handle; + std::unique_ptr handle; public: - Panda(std::string serial="", uint32_t bus_offset=0); + Panda(std::string serial); cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; - const uint32_t bus_offset; bool connected(); bool comms_healthy(); std::string hw_serial(); // Static functions - static std::vector list(bool usb_only=false); + static std::vector list(); // Panda functionality cereal::PandaState::PandaType get_hw_type(); @@ -91,7 +90,7 @@ protected: uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64]; uint32_t receive_buffer_size = 0; - Panda(uint32_t bus_offset) : bus_offset(bus_offset) {} + Panda() {} void pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func); bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); diff --git a/selfdrive/pandad/panda_comms.cc b/selfdrive/pandad/panda_comms.cc deleted file mode 100644 index 8a20f397d..000000000 --- a/selfdrive/pandad/panda_comms.cc +++ /dev/null @@ -1,227 +0,0 @@ -#include "selfdrive/pandad/panda.h" - -#include -#include -#include - -#include "common/swaglog.h" - -static libusb_context *init_usb_ctx() { - libusb_context *context = nullptr; - int err = libusb_init(&context); - if (err != 0) { - LOGE("libusb initialization error"); - return nullptr; - } - -#if LIBUSB_API_VERSION >= 0x01000106 - libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); -#else - libusb_set_debug(context, 3); -#endif - return context; -} - -PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - int err = 0; - ctx = init_usb_ctx(); - if (!ctx) { goto fail; } - - // connect by serial - num_devices = libusb_get_device_list(ctx, &dev_list); - if (num_devices < 0) { goto fail; } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device_descriptor desc; - libusb_get_device_descriptor(dev_list[i], &desc); - if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { - int ret = libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL || ret < 0) { goto fail; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - if (ret < 0) { goto fail; } - - hw_serial = std::string((char *)desc_serial, ret); - if (serial.empty() || serial == hw_serial) { - break; - } - libusb_close(dev_handle); - dev_handle = NULL; - } - } - if (dev_handle == NULL) goto fail; - libusb_free_device_list(dev_list, 1); - dev_list = nullptr; - - if (libusb_kernel_driver_active(dev_handle, 0) == 1) { - libusb_detach_kernel_driver(dev_handle, 0); - } - - err = libusb_set_configuration(dev_handle, 1); - if (err != 0) { goto fail; } - - err = libusb_claim_interface(dev_handle, 0); - if (err != 0) { goto fail; } - - return; - -fail: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - cleanup(); - throw std::runtime_error("Error connecting to panda"); -} - -PandaUsbHandle::~PandaUsbHandle() { - std::lock_guard lk(hw_lock); - cleanup(); - connected = false; -} - -void PandaUsbHandle::cleanup() { - if (dev_handle) { - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - } - - if (ctx) { - libusb_exit(ctx); - } -} - -std::vector PandaUsbHandle::list() { - static std::unique_ptr context(init_usb_ctx(), libusb_exit); - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - std::vector serials; - if (!context) { return serials; } - - num_devices = libusb_get_device_list(context.get(), &dev_list); - if (num_devices < 0) { - LOGE("libusb can't get device list"); - goto finish; - } - for (size_t i = 0; i < num_devices; ++i) { - libusb_device *device = dev_list[i]; - libusb_device_descriptor desc; - libusb_get_device_descriptor(device, &desc); - if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { - libusb_device_handle *handle = NULL; - int ret = libusb_open(device, &handle); - if (ret < 0) { goto finish; } - - unsigned char desc_serial[26] = { 0 }; - ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); - libusb_close(handle); - if (ret < 0) { goto finish; } - - serials.push_back(std::string((char *)desc_serial, ret)); - } - } - -finish: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - return serials; -} - -void PandaUsbHandle::handle_usb_issue(int err, const char func[]) { - LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); - if (err == LIBUSB_ERROR_NO_DEVICE) { - LOGE("lost connection"); - connected = false; - } - // TODO: check other errors, is simply retrying okay? -} - -int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(hw_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { - int err; - const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(hw_lock); - do { - err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); - if (err < 0) handle_usb_issue(err, __func__); - } while (err < 0 && connected); - - return err; -} - -int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(hw_lock); - do { - // Try sending can messages. If the receive buffer on the panda is full it will NAK - // and libusb will try again. After 5ms, it will time out. We will drop the messages. - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - LOGW("Transmit buffer full"); - break; - } else if (err != 0 || length != transferred) { - handle_usb_issue(err, __func__); - } - } while (err != 0 && connected); - - return transferred; -} - -int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - int err; - int transferred = 0; - - if (!connected) { - return 0; - } - - std::lock_guard lk(hw_lock); - - do { - err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); - - if (err == LIBUSB_ERROR_TIMEOUT) { - break; // timeout is okay to exit, recv still happened - } else if (err == LIBUSB_ERROR_OVERFLOW) { - comms_healthy = false; - LOGE_100("overflow got 0x%x", transferred); - } else if (err != 0) { - handle_usb_issue(err, __func__); - } - - } while (err != 0 && connected); - - return transferred; -} diff --git a/selfdrive/pandad/panda_comms.h b/selfdrive/pandad/panda_comms.h index 9c452faf6..cdfb5019b 100644 --- a/selfdrive/pandad/panda_comms.h +++ b/selfdrive/pandad/panda_comms.h @@ -6,67 +6,20 @@ #include #include -#ifndef __APPLE__ -#include -#endif - -#include - #define TIMEOUT 0 #define SPI_BUF_SIZE 2048 -// comms base class -class PandaCommsHandle { +class PandaSpiHandle { public: - PandaCommsHandle(std::string serial) {} - virtual ~PandaCommsHandle() {} - virtual void cleanup() = 0; - std::string hw_serial; std::atomic connected = true; std::atomic comms_healthy = true; - static std::vector list(); - // HW communication - virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0; - virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; - virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; - virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; -}; - -class PandaUsbHandle : public PandaCommsHandle { -public: - PandaUsbHandle(std::string serial); - ~PandaUsbHandle(); - int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); - int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); - int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - void cleanup(); - - static std::vector list(); - -private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::recursive_mutex hw_lock; - void handle_usb_issue(int err, const char func[]); -}; - -#ifndef __APPLE__ -struct __attribute__((packed)) spi_header { - uint8_t sync; - uint8_t endpoint; - uint16_t tx_len; - uint16_t max_rx_len; -}; - -class PandaSpiHandle : public PandaCommsHandle { -public: PandaSpiHandle(std::string serial); ~PandaSpiHandle(); + int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); @@ -81,13 +34,19 @@ private: uint8_t rx_buf[SPI_BUF_SIZE]; inline static std::recursive_mutex hw_lock; + struct __attribute__((packed)) spi_header { + uint8_t sync; + uint8_t endpoint; + uint16_t tx_len; + uint16_t max_rx_len; + }; + int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length); int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout); int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout); - int lltransfer(spi_ioc_transfer &t); + int lltransfer(struct spi_ioc_transfer &t); spi_header header; uint32_t xfer_count = 0; }; -#endif diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc index b08950341..32d129bc2 100644 --- a/selfdrive/pandad/panda_safety.cc +++ b/selfdrive/pandad/panda_safety.cc @@ -23,19 +23,15 @@ void PandaSafety::updateMultiplexingMode() { // Initialize to ELM327 without OBD multiplexing for initial fingerprinting if (!initialized_) { prev_obd_multiplexing_ = false; - for (int i = 0; i < pandas_.size(); ++i) { - pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); - } + panda_->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); initialized_ = true; } // Switch between multiplexing modes based on the OBD multiplexing request bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled"); if (obd_multiplexing_requested != prev_obd_multiplexing_) { - for (int i = 0; i < pandas_.size(); ++i) { - const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; - pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); - } + const uint16_t safety_param = obd_multiplexing_requested ? 0U : 1U; + panda_->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); prev_obd_multiplexing_ = obd_multiplexing_requested; params_.putBool("ObdMultiplexingChanged", true); } @@ -65,17 +61,10 @@ void PandaSafety::setSafetyMode(const std::string ¶ms_string) { auto safety_configs = car_params.getSafetyConfigs(); uint16_t alternative_experience = car_params.getAlternativeExperience(); - for (int i = 0; i < pandas_.size(); ++i) { - // Default to SILENT safety model if not specified - cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT; - uint16_t safety_param = 0U; - if (i < safety_configs.size()) { - safety_model = safety_configs[i].getSafetyModel(); - safety_param = safety_configs[i].getSafetyParam(); - } + cereal::CarParams::SafetyModel safety_model = safety_configs[0].getSafetyModel(); + uint16_t safety_param = safety_configs[0].getSafetyParam(); - LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); - pandas_[i]->set_alternative_experience(alternative_experience); - pandas_[i]->set_safety_model(safety_model, safety_param); - } + LOGW("setting safety model: %d, param: %d, alternative experience: %d", (int)safety_model, safety_param, alternative_experience); + panda_->set_alternative_experience(alternative_experience); + panda_->set_safety_model(safety_model, safety_param); } diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 2fd4a4def..d048dbd0c 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -1,6 +1,5 @@ #include "selfdrive/pandad/pandad.h" -#include #include #include #include @@ -18,45 +17,24 @@ #include "common/util.h" #include "system/hardware/hw.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, pandad will reconnect to all pandas -// - If a panda is added, we will only reconnect when we are offroad -// CAN buses: -// - Each panda will have its 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" or "noOutput" mode -// Ignition: -// - If any of the ignition sources in any panda is high, ignition is high - #define MAX_IR_PANDA_VAL 50 #define CUTOFF_IL 400 #define SATURATE_IL 1000 ExitHandler do_exit; -bool check_all_connected(const std::vector &pandas) { - for (const auto& panda : pandas) { - if (!panda->connected()) { - do_exit = true; - return false; - } +bool check_connected(Panda *panda) { + if (!panda->connected()) { + do_exit = true; + return false; } return true; } -Panda *connect(std::string serial="", uint32_t index=0) { +Panda *connect(std::string serial) { std::unique_ptr panda; try { - panda = std::make_unique(serial, (index * PANDA_BUS_OFFSET)); + panda = std::make_unique(serial); } catch (std::exception &e) { return nullptr; } @@ -78,7 +56,7 @@ Panda *connect(std::string serial="", uint32_t index=0) { return panda.release(); } -void can_send_thread(std::vector pandas, bool fake_send) { +void can_send_thread(Panda *panda, bool fake_send) { util::set_thread_name("pandad_can_send"); AlignedBuffer aligned_buf; @@ -88,7 +66,7 @@ void can_send_thread(std::vector pandas, bool fake_send) { subscriber->setTimeout(100); // run as fast as messages come in - while (!do_exit && check_all_connected(pandas)) { + while (!do_exit && check_connected(panda)) { std::unique_ptr msg(subscriber->receive()); if (!msg) { continue; @@ -99,25 +77,20 @@ void can_send_thread(std::vector pandas, bool fake_send) { // Don't send if older than 1 second if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) { - for (const auto& panda : pandas) { - LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); - panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); - } + LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str()); + panda->can_send(event.getSendcan()); + LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str()); } else { LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime()); } } } -void can_recv(std::vector &pandas, PubMaster *pm) { +void can_recv(Panda *panda, PubMaster *pm) { static std::vector raw_can_data; { - bool comms_healthy = true; raw_can_data.clear(); - for (const auto& panda : pandas) { - comms_healthy &= panda->can_receive(raw_can_data); - } + bool comms_healthy = panda->can_receive(raw_can_data); MessageBuilder msg; auto evt = msg.initEvent(); @@ -187,102 +160,72 @@ void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const cs.setCanCoreResetCnt(can_health.can_core_reset_cnt); } -std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool is_onroad, bool spoofing_started) { - bool ignition_local = false; - const uint32_t pandas_cnt = pandas.size(); - +std::optional send_panda_states(PubMaster *pm, Panda *panda, bool is_onroad, bool spoofing_started) { // build msg MessageBuilder msg; auto evt = msg.initEvent(); - auto pss = evt.initPandaStates(pandas_cnt); + auto pss = evt.initPandaStates(1); - std::vector pandaStates; - pandaStates.reserve(pandas_cnt); - - std::vector> pandaCanStates; - pandaCanStates.reserve(pandas_cnt); - - const bool red_panda_comma_three = (pandas.size() == 2) && - (pandas[0]->hw_type == cereal::PandaState::PandaType::DOS) && - (pandas[1]->hw_type == cereal::PandaState::PandaType::RED_PANDA); - - for (const auto& panda : pandas){ - auto health_opt = panda->get_state(); - if (!health_opt) { - return std::nullopt; - } - - health_t health = *health_opt; - - std::array can_health{}; - for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) { - auto can_health_opt = panda->get_can_state(i); - if (!can_health_opt) { - return std::nullopt; - } - can_health[i] = *can_health_opt; - } - pandaCanStates.push_back(can_health); - - if (spoofing_started) { - health.ignition_line_pkt = 1; - } - - // on comma three setups with a red panda, the dos can - // get false positive ignitions due to the harness box - // without a harness connector, so ignore it - if (red_panda_comma_three && (panda->hw_type == cereal::PandaState::PandaType::DOS)) { - health.ignition_line_pkt = 0; - } - - ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); - - pandaStates.push_back(health); + auto health_opt = panda->get_state(); + if (!health_opt) { + return std::nullopt; } - for (uint32_t i = 0; i < pandas_cnt; i++) { - auto panda = pandas[i]; - const auto &health = pandaStates[i]; + health_t health = *health_opt; - // 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 (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + std::array can_health{}; + for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) { + auto can_health_opt = panda->get_can_state(i); + if (!can_health_opt) { + return std::nullopt; } + can_health[i] = *can_health_opt; + } - bool power_save_desired = !ignition_local; - if (health.power_save_enabled_pkt != power_save_desired) { - panda->set_power_saving(power_save_desired); - } + if (spoofing_started) { + health.ignition_line_pkt = 1; + } - // set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect - bool should_close_relay = !ignition_local || !is_onroad; - if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { - panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } + bool ignition_local = ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); - if (!panda->comms_healthy()) { - evt.setValid(false); - } + // 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 (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } - auto ps = pss[i]; - fill_panda_state(ps, panda->hw_type, health); + bool power_save_desired = !ignition_local; + if (health.power_save_enabled_pkt != power_save_desired) { + panda->set_power_saving(power_save_desired); + } - auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; - for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) { - fill_panda_can_state(cs[j], pandaCanStates[i][j]); - } + // set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect + bool should_close_relay = !ignition_local || !is_onroad; + if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); + } - // Convert faults bitset to capnp list - std::bitset fault_bits(health.faults_pkt); - auto faults = ps.initFaults(fault_bits.count()); + if (!panda->comms_healthy()) { + evt.setValid(false); + } - size_t j = 0; - for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION); - f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) { - if (fault_bits.test(f)) { - faults.set(j, cereal::PandaState::FaultType(f)); - j++; - } + auto ps = pss[0]; + fill_panda_state(ps, panda->hw_type, health); + + auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; + for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) { + fill_panda_can_state(cs[j], can_health[j]); + } + + // Convert faults bitset to capnp list + std::bitset fault_bits(health.faults_pkt); + 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::HEARTBEAT_LOOP_WATCHDOG); f++) { + if (fault_bits.test(f)) { + faults.set(j, cereal::PandaState::FaultType(f)); + j++; } } @@ -323,46 +266,22 @@ void send_peripheral_state(Panda *panda, PubMaster *pm) { pm->send("peripheralState", msg); } -void process_panda_state(std::vector &pandas, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) { - std::vector connected_serials; - for (Panda *p : pandas) { - connected_serials.push_back(p->hw_serial()); +void process_panda_state(Panda *panda, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) { + auto ignition_opt = send_panda_states(pm, panda, is_onroad, spoofing_started); + if (!ignition_opt) { + LOGE("Failed to get ignition_opt"); + return; } - { - auto ignition_opt = send_panda_states(pm, pandas, is_onroad, spoofing_started); - if (!ignition_opt) { - LOGE("Failed to get ignition_opt"); - return; - } - - // check if we should have pandad reconnect - if (!ignition_opt.value()) { - bool comms_healthy = true; - for (const auto &panda : pandas) { - comms_healthy &= panda->comms_healthy(); - } - - if (!comms_healthy) { - LOGE("Reconnecting, communication to pandas not healthy"); - 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; - } - } - } - } - - for (const auto &panda : pandas) { - panda->send_heartbeat(engaged); + // check if we should have pandad reconnect + if (!ignition_opt.value()) { + if (!panda->comms_healthy()) { + LOGE("Reconnecting, communication to panda not healthy"); + do_exit = true; } } + + panda->send_heartbeat(engaged); } void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { @@ -429,30 +348,29 @@ void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) } } -void pandad_run(std::vector &pandas) { +void pandad_run(Panda *panda) { const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr; const bool spoofing_started = getenv("STARTED") != nullptr; const bool fake_send = getenv("FAKESEND") != nullptr; // Start the CAN send thread - std::thread send_thread(can_send_thread, pandas, fake_send); + std::thread send_thread(can_send_thread, panda, fake_send); Params params; RateKeeper rk("pandad", 100); SubMaster sm({"selfdriveState"}); PubMaster pm({"can", "pandaStates", "peripheralState"}); - PandaSafety panda_safety(pandas); - Panda *peripheral_panda = pandas[0]; + PandaSafety panda_safety(panda); bool engaged = false; bool is_onroad = false; // Main loop: receive CAN data and process states - while (!do_exit && check_all_connected(pandas)) { - can_recv(pandas, &pm); + while (!do_exit && check_connected(panda)) { + can_recv(panda, &pm); // Process peripheral state at 20 Hz if (rk.frame() % 5 == 0) { - process_peripheral_state(peripheral_panda, &pm, no_fan_control); + process_peripheral_state(panda, &pm, no_fan_control); } // Process panda state at 10 Hz @@ -460,25 +378,23 @@ void pandad_run(std::vector &pandas) { sm.update(0); engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled(); is_onroad = params.getBool("IsOnroad"); - process_panda_state(pandas, &pm, engaged, is_onroad, spoofing_started); + process_panda_state(panda, &pm, engaged, is_onroad, spoofing_started); panda_safety.configureSafetyMode(is_onroad); } // Send out peripheralState at 2Hz if (rk.frame() % 50 == 0) { - send_peripheral_state(peripheral_panda, &pm); + send_peripheral_state(panda, &pm); } - // Forward logs from pandas to cloudlog if available - for (auto *panda : pandas) { - std::string log = panda->serial_read(); - if (!log.empty()) { - if (log.find("Register 0x") != std::string::npos) { - // Log register divergent faults as errors - LOGE("%s", log.c_str()); - } else { - LOGD("%s", log.c_str()); - } + // Forward logs from panda to cloudlog if available + std::string log = panda->serial_read(); + if (!log.empty()) { + if (log.find("Register 0x") != std::string::npos) { + // Log register divergent faults as errors + LOGE("%s", log.c_str()); + } else { + LOGD("%s", log.c_str()); } } @@ -487,52 +403,38 @@ void pandad_run(std::vector &pandas) { // Close relay on exit to prevent a fault if (is_onroad && !engaged) { - for (auto &p : pandas) { - if (p->connected()) { - p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); - } + if (panda->connected()) { + panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } } send_thread.join(); } -void pandad_main_thread(std::vector serials) { - if (serials.size() == 0) { - serials = Panda::list(); +void pandad_main_thread(std::string serial) { + if (serial.empty()) { + auto serials = Panda::list(); - if (serials.size() == 0) { + if (serials.empty()) { LOGW("no pandas found, exiting"); return; } + serial = serials[0]; } - std::string serials_str; - for (int i = 0; i < serials.size(); i++) { - serials_str += serials[i]; - if (i < serials.size() - 1) serials_str += ", "; - } - LOGW("connecting to pandas: %s", serials_str.c_str()); + LOGW("connecting to panda: %s", serial.c_str()); - // connect to all provided serials - std::vector pandas; - for (int i = 0; i < serials.size() && !do_exit; /**/) { - Panda *p = connect(serials[i], i); - if (!p) { - util::sleep_for(100); - continue; - } - - pandas.push_back(p); - ++i; + Panda *panda = nullptr; + while (!do_exit) { + panda = connect(serial); + if (panda) break; + util::sleep_for(100); } if (!do_exit) { - LOGW("connected to all pandas"); - pandad_run(pandas); + LOGW("connected to panda"); + pandad_run(panda); } - for (Panda *panda : pandas) { - delete panda; - } + delete panda; } diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h index 637807e07..aa10d1ae4 100644 --- a/selfdrive/pandad/pandad.h +++ b/selfdrive/pandad/pandad.h @@ -1,16 +1,15 @@ #pragma once #include -#include #include "common/params.h" #include "selfdrive/pandad/panda.h" -void pandad_main_thread(std::vector serials); +void pandad_main_thread(std::string serial); class PandaSafety { public: - PandaSafety(const std::vector &pandas) : pandas_(pandas) {} + PandaSafety(Panda *panda) : panda_(panda) {} void configureSafetyMode(bool is_onroad); private: @@ -22,6 +21,6 @@ private: bool log_once_ = false; bool safety_configured_ = false; bool prev_obd_multiplexing_ = false; - std::vector pandas_; + Panda *panda_; Params params_; }; diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index d5c58ddd6..df2b4f7ee 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -110,46 +110,35 @@ def main() -> None: cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}") - # Flash pandas - pandas: list[Panda] = [] - for serial in panda_serials: - pandas.append(flash_panda(serial)) + # Flash the first panda + panda_serial = panda_serials[0] + panda = flash_panda(panda_serial) # Ensure internal panda is present if expected - internal_pandas = [panda for panda in pandas if panda.is_internal()] - if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: + if HARDWARE.has_internal_panda() and not panda.is_internal(): cloudlog.error("Internal panda is missing, trying again") no_internal_panda_count += 1 continue no_internal_panda_count = 0 - # sort pandas to have deterministic order - # * the internal one is always first - # * then sort by hardware type - # * as a last resort, sort by serial number - pandas.sort(key=lambda x: (not x.is_internal(), x.get_type(), x.get_usb_serial())) - panda_serials = [p.get_usb_serial() for p in pandas] + # log panda fw version + params.put("PandaSignatures", panda.get_signature()) - # log panda fw versions - params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) + # check health for lost heartbeat + health = panda.health() + if health["heartbeat_lost"]: + params.put_bool("PandaHeartbeatLost", True) + cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) + if health["som_reset_triggered"]: + params.put_bool("PandaSomResetTriggered", True) + cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial()) - for panda in pandas: - # check health for lost heartbeat - health = panda.health() - if health["heartbeat_lost"]: - params.put_bool("PandaHeartbeatLost", True) - cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) - if health["som_reset_triggered"]: - params.put_bool("PandaSomResetTriggered", True) - cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial()) + if first_run: + # reset panda to ensure we're in a good state + cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") + panda.reset(reconnect=True) - if first_run: - # reset panda to ensure we're in a good state - cloudlog.info(f"Resetting panda {panda.get_usb_serial()}") - panda.reset(reconnect=True) - - for p in pandas: - p.close() + panda.close() # TODO: wrap all panda exceptions in a base panda exception except (usb1.USBErrorNoDevice, usb1.USBErrorPipe): # a panda was disconnected while setting everything up. let's try again @@ -166,7 +155,7 @@ def main() -> None: # run pandad with all connected serials as arguments os.environ['MANAGER_DAEMON'] = 'pandad' - process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "selfdrive/pandad")) + process = subprocess.Popen(["./pandad", panda_serial], cwd=os.path.join(BASEDIR, "selfdrive/pandad")) process.wait() diff --git a/selfdrive/pandad/spi.cc b/selfdrive/pandad/spi.cc index b6ee57801..25682e6b1 100644 --- a/selfdrive/pandad/spi.cc +++ b/selfdrive/pandad/spi.cc @@ -1,4 +1,3 @@ -#ifndef __APPLE__ #include #include #include @@ -55,7 +54,7 @@ private: util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \ } while (0) -PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { +PandaSpiHandle::PandaSpiHandle(std::string serial) { int ret; const int uid_len = 12; uint8_t uid[uid_len] = {0}; @@ -407,4 +406,3 @@ fail: if (ret >= 0) ret = -1; return ret; } -#endif diff --git a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc b/selfdrive/pandad/tests/test_pandad_canprotocol.cc similarity index 88% rename from selfdrive/pandad/tests/test_pandad_usbprotocol.cc rename to selfdrive/pandad/tests/test_pandad_canprotocol.cc index 11f7184ef..9b53392f6 100644 --- a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc +++ b/selfdrive/pandad/tests/test_pandad_canprotocol.cc @@ -1,13 +1,15 @@ #define CATCH_CONFIG_MAIN #define CATCH_CONFIG_ENABLE_BENCHMARKING +#include + #include "catch2/catch.hpp" #include "cereal/messaging/messaging.h" #include "common/util.h" #include "selfdrive/pandad/panda.h" struct PandaTest : public Panda { - PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type); + PandaTest(int can_list_size, cereal::PandaState::PandaType hw_type); void test_can_send(); void test_can_recv(uint32_t chunk_size = 0); void test_chunked_can_recv(); @@ -19,7 +21,7 @@ struct PandaTest : public Panda { capnp::List::Reader can_data_list; }; -PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) { +PandaTest::PandaTest(int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda() { this->hw_type = hw_type; int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8); // prepare test data @@ -40,7 +42,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState uint32_t id = util::random_int(0, std::size(dlc_to_len) - 1); const std::string &dat = test_data[dlc_to_len[id]]; can.setAddress(i); - can.setSrc(util::random_int(0, 2) + bus_offset); + can.setSrc(util::random_int(0, 2)); can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); total_pakets_size += sizeof(can_header) + dat.size(); } @@ -103,9 +105,8 @@ void PandaTest::test_can_recv(uint32_t rx_chunk_size) { } TEST_CASE("send/recv CAN 2.0 packets") { - auto bus_offset = GENERATE(0, 4); auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); - PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS); + PandaTest test(can_list_size, cereal::PandaState::PandaType::DOS); SECTION("can_send") { test.test_can_send(); @@ -119,9 +120,8 @@ TEST_CASE("send/recv CAN 2.0 packets") { } TEST_CASE("send/recv CAN FD packets") { - auto bus_offset = GENERATE(0, 4); auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200); - PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA); + PandaTest test(can_list_size, cereal::PandaState::PandaType::RED_PANDA); SECTION("can_send") { test.test_can_send(); diff --git a/selfdrive/pandad/tests/test_pandad_loopback.py b/selfdrive/pandad/tests/test_pandad_loopback.py index eff70d254..fd4a99be6 100644 --- a/selfdrive/pandad/tests/test_pandad_loopback.py +++ b/selfdrive/pandad/tests/test_pandad_loopback.py @@ -13,12 +13,11 @@ from openpilot.common.utils import retry from openpilot.common.params import Params from openpilot.common.timeout import Timeout from openpilot.selfdrive.pandad import can_list_to_can_capnp -from openpilot.system.hardware import TICI from openpilot.selfdrive.test.helpers import with_processes @retry(attempts=3) -def setup_pandad(num_pandas): +def setup_pandad(): params = Params() params.clear_all() params.put_bool("IsOnroad", False) @@ -29,16 +28,12 @@ def setup_pandad(num_pandas): any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']): sm.update(1000) - found_pandas = len(sm['pandaStates']) - assert num_pandas == found_pandas, "connected pandas ({found_pandas}) doesn't match expected panda count ({num_pandas}). \ - connect another panda for multipanda tests." - # pandad safety setting relies on these params cp = car.CarParams.new_message() safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.allOutput - cp.safetyConfigs = [safety_config]*num_pandas + cp.safetyConfigs = [safety_config] params.put_bool("IsOnroad", True) params.put_bool("FirmwareQueryDone", True) @@ -49,12 +44,12 @@ def setup_pandad(num_pandas): while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']): sm.update(1000) -def send_random_can_messages(sendcan, count, num_pandas=1): +def send_random_can_messages(sendcan, count): sent_msgs = defaultdict(set) for _ in range(count): to_send = [] for __ in range(random.randrange(20)): - bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3]) + bus = random.choice(range(3)) addr = random.randrange(1, 1<<29) dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9))) if (addr, dat) in sent_msgs[bus]: @@ -74,8 +69,7 @@ class TestBoarddLoopback: @with_processes(['pandad']) def test_loopback(self): - num_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1 - setup_pandad(num_pandas) + setup_pandad() sendcan = messaging.pub_sock('sendcan') can = messaging.sub_sock('can', conflate=False, timeout=100) @@ -86,7 +80,7 @@ class TestBoarddLoopback: for i in range(n): print(f"pandad loopback {i}/{n}") - sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100), num_pandas) + sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100)) sent_loopback = copy.deepcopy(sent_msgs) sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()}) diff --git a/selfdrive/pandad/tests/test_pandad_spi.py b/selfdrive/pandad/tests/test_pandad_spi.py index da4b18199..69dfb67e9 100644 --- a/selfdrive/pandad/tests/test_pandad_spi.py +++ b/selfdrive/pandad/tests/test_pandad_spi.py @@ -22,7 +22,7 @@ class TestBoarddSpi: @with_processes(['pandad']) def test_spi_corruption(self, subtests): - setup_pandad(1) + setup_pandad() sendcan = messaging.pub_sock('sendcan') socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')}