diff --git a/selfdrive/pandad/.gitignore b/selfdrive/pandad/.gitignore index f7226cdb87..71068713c7 100644 --- a/selfdrive/pandad/.gitignore +++ b/selfdrive/pandad/.gitignore @@ -1,3 +1 @@ -pandad pandad_api_impl.cpp -tests/test_pandad_usbprotocol diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index 58777cafe9..3b9daa3fff 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -1,13 +1,4 @@ -Import('env', 'envCython', 'common', 'messaging') - -libs = ['usb-1.0', common, messaging, 'pthread'] -panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) - -env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) +Import('env', 'envCython') env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) - pandad_python = envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) Export('pandad_python') - -if GetOption('extras'): - env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/pandad/main.cc b/selfdrive/pandad/main.cc deleted file mode 100644 index b63d884a45..0000000000 --- a/selfdrive/pandad/main.cc +++ /dev/null @@ -1,22 +0,0 @@ -#include - -#include "selfdrive/pandad/pandad.h" -#include "common/swaglog.h" -#include "common/util.h" -#include "system/hardware/hw.h" - -int main(int argc, char *argv[]) { - LOGW("starting pandad"); - - if (!Hardware::PC()) { - int err; - err = util::set_realtime_priority(54); - assert(err == 0); - err = util::set_core_affinity({3}); - assert(err == 0); - } - - std::vector serials(argv + 1, argv + argc); - pandad_main_thread(serials); - return 0; -} diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc deleted file mode 100644 index 93e139f0ec..0000000000 --- a/selfdrive/pandad/panda.cc +++ /dev/null @@ -1,312 +0,0 @@ -#include "selfdrive/pandad/panda.h" - -#include - -#include -#include -#include - -#include "cereal/messaging/messaging.h" -#include "common/swaglog.h" -#include "common/util.h" - -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 - } - - hw_type = get_hw_type(); - can_reset_communications(); -} - -bool Panda::connected() { - return handle->connected; -} - -bool Panda::comms_healthy() { - return handle->comms_healthy; -} - -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; -} - -void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { - handle->control_write(0xdc, (uint16_t)safety_model, safety_param); -} - -void Panda::set_alternative_experience(uint16_t alternative_experience) { - handle->control_write(0xdf, alternative_experience, 0); -} - -std::string Panda::serial_read(int port_number) { - std::string ret; - char buffer[USBPACKET_MAX_SIZE] = {}; - - while (true) { - int bytes_read = handle->control_read(0xe0, port_number, 0, (unsigned char *)buffer, USBPACKET_MAX_SIZE); - if (bytes_read <= 0) { - break; - } - ret.append(buffer, bytes_read); - } - - return ret; -} - -void Panda::set_uart_baud(int uart, int rate) { - handle->control_write(0xe4, uart, int(rate / 300)); -} - -cereal::PandaState::PandaType Panda::get_hw_type() { - unsigned char hw_query[1] = {0}; - - handle->control_read(0xc1, 0, 0, hw_query, 1); - return (cereal::PandaState::PandaType)(hw_query[0]); -} - -void Panda::set_fan_speed(uint16_t fan_speed) { - handle->control_write(0xb1, fan_speed, 0); -} - -uint16_t Panda::get_fan_speed() { - uint16_t fan_speed_rpm = 0; - handle->control_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm)); - return fan_speed_rpm; -} - -void Panda::set_ir_pwr(uint16_t ir_pwr) { - handle->control_write(0xb0, ir_pwr, 0); -} - -std::optional Panda::get_state() { - health_t health {0}; - int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); - return err >= 0 ? std::make_optional(health) : std::nullopt; -} - -std::optional Panda::get_can_state(uint16_t can_number) { - can_health_t can_health {0}; - int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health)); - return err >= 0 ? std::make_optional(can_health) : std::nullopt; -} - -void Panda::set_loopback(bool loopback) { - handle->control_write(0xe5, loopback, 0); -} - -std::optional> Panda::get_firmware_version() { - std::vector fw_sig_buf(128); - int read_1 = handle->control_read(0xd3, 0, 0, &fw_sig_buf[0], 64); - int read_2 = handle->control_read(0xd4, 0, 0, &fw_sig_buf[64], 64); - return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt; -} - -std::optional Panda::get_serial() { - char serial_buf[17] = {'\0'}; - int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); - return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; -} - -bool Panda::up_to_date() { - if (auto fw_sig = get_firmware_version()) { - for (auto fn : { "panda.bin.signed", "panda_h7.bin.signed" }) { - auto content = util::read_file(std::string("../../panda/board/obj/") + fn); - if (content.size() >= fw_sig->size() && - memcmp(content.data() + content.size() - fw_sig->size(), fw_sig->data(), fw_sig->size()) == 0) { - return true; - } - } - } - return false; -} - -void Panda::set_power_saving(bool power_saving) { - handle->control_write(0xe7, power_saving, 0); -} - -void Panda::enable_deepsleep() { - handle->control_write(0xfb, 0, 0); -} - -void Panda::send_heartbeat(bool engaged) { - handle->control_write(0xf3, engaged, 0); -} - -void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { - handle->control_write(0xde, bus, (speed * 10)); -} - -void Panda::set_can_fd_auto(uint16_t bus, bool enabled) { - handle->control_write(0xe8, bus, enabled); -} - -void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { - handle->control_write(0xf9, bus, (speed * 10)); -} - -void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) { - handle->control_write(0xfc, bus, non_iso); -} - -static uint8_t len_to_dlc(uint8_t len) { - if (len <= 8) { - return len; - } - if (len <= 24) { - return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0); - } else { - return 11 + (len / 16) + ((len % 16) ? 1 : 0); - } -} - -void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list, - std::function write_func) { - int32_t pos = 0; - uint8_t send_buf[2 * USB_TX_SOFT_LIMIT]; - - 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)) { - continue; - } - auto can_data = cmsg.getDat(); - uint8_t data_len_code = len_to_dlc(can_data.size()); - assert(can_data.size() <= 64); - assert(can_data.size() == dlc_to_len[data_len_code]); - - can_header header = {}; - header.addr = cmsg.getAddress(); - header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0; - header.data_len_code = data_len_code; - header.bus = bus - bus_offset; - header.checksum = 0; - - memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header)); - memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size()); - uint32_t msg_size = sizeof(can_header) + can_data.size(); - - // set checksum - ((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size); - - pos += msg_size; - - if (pos >= USB_TX_SOFT_LIMIT) { - write_func(send_buf, pos); - pos = 0; - } - } - - // send remaining packets - if (pos > 0) write_func(send_buf, pos); -} - -void Panda::can_send(const capnp::List::Reader &can_data_list) { - pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) { - handle->bulk_write(3, data, size, 5); - }); -} - -bool Panda::can_receive(std::vector& out_vec) { - // Check if enough space left in buffer to store RECV_SIZE data - assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); - - int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); - if (!comms_healthy()) { - return false; - } - - if (PANDAD_MAXOUT) { - static uint8_t junk[RECV_SIZE]; - handle->bulk_read(0xab, junk, RECV_SIZE - recv); - } - - bool ret = true; - if (recv > 0) { - receive_buffer_size += recv; - ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); - } - return ret; -} - -void Panda::can_reset_communications() { - handle->control_write(0xc0, 0, 0); -} - -bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec) { - int pos = 0; - - while (pos <= size - sizeof(can_header)) { - can_header header; - memcpy(&header, &data[pos], sizeof(can_header)); - - const uint8_t data_len = dlc_to_len[header.data_len_code]; - if (pos + sizeof(can_header) + data_len > size) { - // we don't have all the data for this message yet - break; - } - - if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) { - LOGE("Panda CAN checksum failed"); - size = 0; - can_reset_communications(); - return false; - } - - can_frame &canData = out_vec.emplace_back(); - canData.address = header.addr; - canData.src = header.bus + bus_offset; - if (header.rejected) { - canData.src += CAN_REJECTED_BUS_OFFSET; - } - if (header.returned) { - canData.src += CAN_RETURNED_BUS_OFFSET; - } - - canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); - - pos += sizeof(can_header) + data_len; - } - - // move the overflowing data to the beginning of the buffer for the next round - memmove(data, &data[pos], size - pos); - size -= pos; - - return true; -} - -uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) { - uint8_t checksum = 0U; - for (uint32_t i = 0U; i < len; i++) { - checksum ^= data[i]; - } - return checksum; -} diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h deleted file mode 100644 index 5cbce44f28..0000000000 --- a/selfdrive/pandad/panda.h +++ /dev/null @@ -1,99 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/gen/cpp/car.capnp.h" -#include "cereal/gen/cpp/log.capnp.h" -#include "panda/board/health.h" -#include "panda/board/can.h" -#include "selfdrive/pandad/panda_comms.h" - -#define USB_TX_SOFT_LIMIT (0x100U) -#define USBPACKET_MAX_SIZE (0x40) - -#define RECV_SIZE (0x4000U) - -#define CAN_REJECTED_BUS_OFFSET 0xC0U -#define CAN_RETURNED_BUS_OFFSET 0x80U - -#define PANDA_BUS_OFFSET 4 - -struct __attribute__((packed)) can_header { - uint8_t reserved : 1; - uint8_t bus : 3; - uint8_t data_len_code : 4; - uint8_t rejected : 1; - uint8_t returned : 1; - uint8_t extended : 1; - uint32_t addr : 29; - uint8_t checksum : 8; -}; - -struct can_frame { - long address; - std::string dat; - long src; -}; - - -class Panda { -private: - std::unique_ptr handle; - -public: - Panda(std::string serial="", uint32_t bus_offset=0); - - 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); - - // Panda functionality - cereal::PandaState::PandaType get_hw_type(); - void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); - void set_alternative_experience(uint16_t alternative_experience); - std::string serial_read(int port_number = 0); - void set_uart_baud(int uart, int rate); - void set_fan_speed(uint16_t fan_speed); - uint16_t get_fan_speed(); - void set_ir_pwr(uint16_t ir_pwr); - std::optional get_state(); - std::optional get_can_state(uint16_t can_number); - void set_loopback(bool loopback); - std::optional> get_firmware_version(); - bool up_to_date(); - std::optional get_serial(); - void set_power_saving(bool power_saving); - void enable_deepsleep(); - void send_heartbeat(bool engaged); - void set_can_speed_kbps(uint16_t bus, uint16_t speed); - void set_can_fd_auto(uint16_t bus, bool enabled); - void set_data_speed_kbps(uint16_t bus, uint16_t speed); - void set_canfd_non_iso(uint16_t bus, bool non_iso); - void can_send(const capnp::List::Reader &can_data_list); - bool can_receive(std::vector& out_vec); - void can_reset_communications(); - -protected: - // for unit tests - 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) {} - 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); - uint8_t calculate_checksum(uint8_t *data, uint32_t len); -}; diff --git a/selfdrive/pandad/panda_comms.cc b/selfdrive/pandad/panda_comms.cc deleted file mode 100644 index 8a20f397d3..0000000000 --- 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 deleted file mode 100644 index 9c452faf6d..0000000000 --- a/selfdrive/pandad/panda_comms.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -#ifndef __APPLE__ -#include -#endif - -#include - - -#define TIMEOUT 0 -#define SPI_BUF_SIZE 2048 - - -// comms base class -class PandaCommsHandle { -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); - int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); - void cleanup(); - - static std::vector list(); - -private: - int spi_fd = -1; - uint8_t tx_buf[SPI_BUF_SIZE]; - uint8_t rx_buf[SPI_BUF_SIZE]; - inline static std::recursive_mutex hw_lock; - - 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); - - spi_header header; - uint32_t xfer_count = 0; -}; -#endif diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc deleted file mode 100644 index b089503417..0000000000 --- a/selfdrive/pandad/panda_safety.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include "selfdrive/pandad/pandad.h" -#include "cereal/messaging/messaging.h" -#include "common/swaglog.h" - -void PandaSafety::configureSafetyMode(bool is_onroad) { - if (is_onroad && !safety_configured_) { - updateMultiplexingMode(); - - auto car_params = fetchCarParams(); - if (!car_params.empty()) { - LOGW("got %lu bytes CarParams", car_params.size()); - setSafetyMode(car_params); - safety_configured_ = true; - } - } else if (!is_onroad) { - initialized_ = false; - safety_configured_ = false; - log_once_ = false; - } -} - -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); - } - 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); - } - prev_obd_multiplexing_ = obd_multiplexing_requested; - params_.putBool("ObdMultiplexingChanged", true); - } -} - -std::string PandaSafety::fetchCarParams() { - if (!params_.getBool("FirmwareQueryDone")) { - return {}; - } - - if (!log_once_) { - LOGW("Finished FW query, Waiting for params to set safety model"); - log_once_ = true; - } - - if (!params_.getBool("ControlsReady")) { - return {}; - } - return params_.get("CarParams"); -} - -void PandaSafety::setSafetyMode(const std::string ¶ms_string) { - AlignedBuffer aligned_buf; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - - 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(); - } - - 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); - } -} diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc deleted file mode 100644 index faaeb15319..0000000000 --- a/selfdrive/pandad/pandad.cc +++ /dev/null @@ -1,525 +0,0 @@ -#include "selfdrive/pandad/pandad.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "cereal/gen/cpp/car.capnp.h" -#include "cereal/messaging/messaging.h" -#include "common/ratekeeper.h" -#include "common/swaglog.h" -#include "common/timing.h" -#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; - } - } - return true; -} - -Panda *connect(std::string serial="", uint32_t index=0) { - std::unique_ptr panda; - try { - panda = std::make_unique(serial, (index * PANDA_BUS_OFFSET)); - } catch (std::exception &e) { - return nullptr; - } - - // common panda config - if (getenv("BOARDD_LOOPBACK")) { - panda->set_loopback(true); - } - //panda->enable_deepsleep(); - - for (int i = 0; i < PANDA_BUS_CNT; i++) { - panda->set_can_fd_auto(i, true); - } - - if (!panda->up_to_date() && !getenv("BOARDD_SKIP_FW_CHECK")) { - throw std::runtime_error("Panda firmware out of date. Run pandad.py to update."); - } - - return panda.release(); -} - -void can_send_thread(std::vector pandas, bool fake_send) { - util::set_thread_name("pandad_can_send"); - - AlignedBuffer aligned_buf; - std::unique_ptr context(Context::create()); - std::unique_ptr subscriber(SubSocket::create(context.get(), "sendcan")); - assert(subscriber != NULL); - subscriber->setTimeout(100); - - // run as fast as messages come in - while (!do_exit && check_all_connected(pandas)) { - std::unique_ptr msg(subscriber->receive()); - if (!msg) { - continue; - } - - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get())); - cereal::Event::Reader event = cmsg.getRoot(); - - // 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()); - } - } else { - LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime()); - } - } -} - -void can_recv(std::vector &pandas, 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); - } - - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(comms_healthy); - auto canData = evt.initCan(raw_can_data.size()); - for (size_t i = 0; i < raw_can_data.size(); ++i) { - canData[i].setAddress(raw_can_data[i].address); - 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); - } -} - -void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) { - ps.setVoltage(health.voltage_pkt); - ps.setCurrent(health.current_pkt); - ps.setUptime(health.uptime_pkt); - ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); - ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); - ps.setIgnitionLine(health.ignition_line_pkt); - ps.setIgnitionCan(health.ignition_can_pkt); - ps.setControlsAllowed(health.controls_allowed_pkt); - ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); - ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); - ps.setPandaType(hw_type); - ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); - ps.setSafetyParam(health.safety_param_pkt); - ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); - ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); - ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); - ps.setAlternativeExperience(health.alternative_experience_pkt); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); - ps.setInterruptLoad(health.interrupt_load_pkt); - ps.setFanPower(health.fan_power); - ps.setFanStallCount(health.fan_stall_count); - ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt)); - ps.setSpiErrorCount(health.spi_error_count_pkt); - ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); - ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); -} - -void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) { - cs.setBusOff((bool)can_health.bus_off); - cs.setBusOffCnt(can_health.bus_off_cnt); - cs.setErrorWarning((bool)can_health.error_warning); - cs.setErrorPassive((bool)can_health.error_passive); - cs.setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error)); - cs.setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error)); - cs.setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error)); - cs.setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error)); - cs.setReceiveErrorCnt(can_health.receive_error_cnt); - cs.setTransmitErrorCnt(can_health.transmit_error_cnt); - cs.setTotalErrorCnt(can_health.total_error_cnt); - cs.setTotalTxLostCnt(can_health.total_tx_lost_cnt); - cs.setTotalRxLostCnt(can_health.total_rx_lost_cnt); - cs.setTotalTxCnt(can_health.total_tx_cnt); - cs.setTotalRxCnt(can_health.total_rx_cnt); - cs.setTotalFwdCnt(can_health.total_fwd_cnt); - cs.setCanSpeed(can_health.can_speed); - cs.setCanDataSpeed(can_health.can_data_speed); - cs.setCanfdEnabled(can_health.canfd_enabled); - cs.setBrsEnabled(can_health.brs_enabled); - cs.setCanfdNonIso(can_health.canfd_non_iso); - cs.setIrq0CallRate(can_health.irq0_call_rate); - cs.setIrq1CallRate(can_health.irq1_call_rate); - cs.setIrq2CallRate(can_health.irq2_call_rate); - 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(); - - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - auto pss = evt.initPandaStates(pandas_cnt); - - 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); - } - - for (uint32_t i = 0; i < pandas_cnt; i++) { - auto panda = pandas[i]; - const auto &health = pandaStates[i]; - - // 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); - } - - bool power_save_desired = !ignition_local; - if (health.power_save_enabled_pkt != power_save_desired) { - panda->set_power_saving(power_save_desired); - } - - // 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); - } - - if (!panda->comms_healthy()) { - evt.setValid(false); - } - - auto ps = pss[i]; - 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], pandaCanStates[i][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++; - } - } - } - - pm->send("pandaStates", msg); - return ignition_local; -} - -void send_peripheral_state(Panda *panda, PubMaster *pm) { - // build msg - MessageBuilder msg; - auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy()); - - auto ps = evt.initPeripheralState(); - ps.setPandaType(panda->hw_type); - - double read_time = millis_since_boot(); - ps.setVoltage(Hardware::get_voltage()); - ps.setCurrent(Hardware::get_current()); - read_time = millis_since_boot() - read_time; - if (read_time > 50) { - LOGW("reading hwmon took %lfms", read_time); - } - - // fall back to panda's voltage and current measurement - if (ps.getVoltage() == 0 && ps.getCurrent() == 0) { - auto health_opt = panda->get_state(); - if (health_opt) { - health_t health = *health_opt; - ps.setVoltage(health.voltage_pkt); - ps.setCurrent(health.current_pkt); - } - } - - uint16_t fan_speed_rpm = panda->get_fan_speed(); - ps.setFanSpeedRpm(fan_speed_rpm); - - 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()); - } - - { - 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); - } - } -} - -void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { - static SubMaster sm({"deviceState", "driverCameraState"}); - - static uint64_t last_driver_camera_t = 0; - static uint16_t prev_fan_speed = 999; - static int ir_pwr = 0; - static int prev_ir_pwr = 999; - - static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); - - { - sm.update(0); - if (sm.updated("deviceState") && !no_fan_control) { - // Fan speed - uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); - if (fan_speed != prev_fan_speed || sm.frame % 100 == 0) { - panda->set_fan_speed(fan_speed); - prev_fan_speed = fan_speed; - } - } - - if (sm.updated("driverCameraState")) { - auto event = sm["driverCameraState"]; - int cur_integ_lines = event.getDriverCameraState().getIntegLines(); - - cur_integ_lines = integ_lines_filter.update(cur_integ_lines); - last_driver_camera_t = event.getLogMonoTime(); - - if (cur_integ_lines <= CUTOFF_IL) { - ir_pwr = 0; - } else if (cur_integ_lines > SATURATE_IL) { - ir_pwr = 100; - } else { - ir_pwr = 100 * (cur_integ_lines - CUTOFF_IL) / (SATURATE_IL - CUTOFF_IL); - } - } - - // Disable IR on input timeout - if (nanos_since_boot() - last_driver_camera_t > 1e9) { - ir_pwr = 0; - } - - if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0) { - int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL); - panda->set_ir_pwr(ir_panda); - Hardware::set_ir_power(ir_pwr); - prev_ir_pwr = ir_pwr; - } - } -} - -void pandad_run(std::vector &pandas) { - 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); - - Params params; - RateKeeper rk("pandad", 100); - SubMaster sm({"selfdriveState"}); - PubMaster pm({"can", "pandaStates", "peripheralState"}); - PandaSafety panda_safety(pandas); - Panda *peripheral_panda = pandas[0]; - 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); - - // Process peripheral state at 20 Hz - if (rk.frame() % 5 == 0) { - process_peripheral_state(peripheral_panda, &pm, no_fan_control); - } - - // Process panda state at 10 Hz - if (rk.frame() % 10 == 0) { - 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); - panda_safety.configureSafetyMode(is_onroad); - } - - // Send out peripheralState at 2Hz - if (rk.frame() % 50 == 0) { - send_peripheral_state(peripheral_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()); - } - } - } - - rk.keepTime(); - } - - // 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); - } - } - } - - send_thread.join(); -} - -void pandad_main_thread(std::vector serials) { - if (serials.size() == 0) { - serials = Panda::list(); - - if (serials.size() == 0) { - LOGW("no pandas found, exiting"); - return; - } - } - - 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()); - - // 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; - } - - if (!do_exit) { - LOGW("connected to all pandas"); - pandad_run(pandas); - } - - for (Panda *panda : pandas) { - delete panda; - } -} diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h deleted file mode 100644 index 637807e074..0000000000 --- a/selfdrive/pandad/pandad.h +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include - -#include "common/params.h" -#include "selfdrive/pandad/panda.h" - -void pandad_main_thread(std::vector serials); - -class PandaSafety { -public: - PandaSafety(const std::vector &pandas) : pandas_(pandas) {} - void configureSafetyMode(bool is_onroad); - -private: - void updateMultiplexingMode(); - std::string fetchCarParams(); - void setSafetyMode(const std::string ¶ms_string); - - bool initialized_ = false; - bool log_once_ = false; - bool safety_configured_ = false; - bool prev_obd_multiplexing_ = false; - std::vector pandas_; - Params params_; -}; diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index 4e49813f5d..0e5bbb0994 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -3,14 +3,13 @@ import os import usb1 import time -import signal -import subprocess - +import threading from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH -from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params +from openpilot.common.realtime import config_realtime_process from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.pandad.runner import PandaRunner def get_expected_signature(panda: Panda) -> bytes: @@ -62,24 +61,17 @@ def flash_panda(panda_serial: str) -> Panda: def main() -> None: - # signal pandad to close the relay and exit - def signal_handler(signum, frame): - cloudlog.info(f"Caught signal {signum}, exiting") - nonlocal do_exit - do_exit = True - if process is not None: - process.send_signal(signal.SIGINT) - - process = None - do_exit = False - signal.signal(signal.SIGINT, signal_handler) + config_realtime_process(3, 54) + # signal pandad to close the relay and exit + evt = threading.Event() count = 0 first_run = True params = Params() no_internal_panda_count = 0 + pandas: list[Panda] = [] - while not do_exit: + while True: try: count += 1 cloudlog.event("pandad.flash_and_connect", count=count) @@ -116,9 +108,7 @@ 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)) + pandas = [flash_panda(serial) for serial in panda_serials] # Ensure internal panda is present if expected internal_pandas = [panda for panda in pandas if panda.is_internal()] @@ -151,11 +141,22 @@ def main() -> None: 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) + + # TODO: this hangs now, but nothing should've changed here + #panda.reset(reconnect=True) + + evt.clear() + first_run = False + runner = PandaRunner(panda_serials, pandas) + runner.run(evt) for p in pandas: p.close() # TODO: wrap all panda exceptions in a base panda exception + except KeyboardInterrupt: + cloudlog.info("Caught Ctrl+C, exiting") + evt.set() + break 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") @@ -166,14 +167,13 @@ def main() -> None: except Exception: cloudlog.exception("pandad.uncaught_exception") continue - - first_run = False - - # 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.wait() - + finally: + for p in pandas: + try: + p.close() + except Exception: + cloudlog.exception("Error closing panda connection") + pandas = [] if __name__ == "__main__": main() diff --git a/selfdrive/pandad/peripheral.py b/selfdrive/pandad/peripheral.py new file mode 100644 index 0000000000..8c8470bcbc --- /dev/null +++ b/selfdrive/pandad/peripheral.py @@ -0,0 +1,101 @@ +import os +import threading +import time +import cereal.messaging as messaging +from openpilot.common.swaglog import cloudlog +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.system.hardware import HARDWARE + +NO_FAN_CONTROL = os.getenv("NO_FAN_CONTROL") == "1" + +MAX_IR_PANDA_VAL = 50 +CUTOFF_IL = 400 +SATURATE_IL = 1000 + + +class HardwareReader: + def __init__(self): + self.voltage = 0 + self.current = 0 + self.running = True + self.thread = threading.Thread(target=self._read_loop, daemon=True) + self.thread.start() + + def _read_loop(self): + while self.running: + start = time.monotonic() + try: + self.voltage = HARDWARE.get_voltage() + self.current = HARDWARE.get_current() + elapsed = (time.monotonic() - start) * 1000 + if elapsed > 50: + cloudlog.warning(f"hwmon read took {elapsed:.2f}ms") + except Exception as e: + cloudlog.error(f"Hardware read error: {e}") + time.sleep(0.5) # 500ms update rate + + def get_values(self): + return self.voltage, self.current + + def stop(self): + self.running = False + self.thread.join() + + +class PeripheralManager: + def __init__(self, panda, hw_type): + self.panda = panda + self.last_camera_t = 0 + self.prev_fan = 999 + self.prev_ir_pwr = 999 + self.ir_pwr = 0 + self.filter = FirstOrderFilter(0, 30.0, 0.05) + self.hw_type = hw_type + self.hw_reader = HardwareReader() + + def process(self, sm): + if sm.updated["deviceState"] and not NO_FAN_CONTROL: + fan = sm["deviceState"].fanSpeedPercentDesired + if fan != self.prev_fan or sm.frame % 100 == 0: + self.panda.set_fan_power(fan) + self.prev_fan = fan + + if sm.updated["driverCameraState"]: + state = sm["driverCameraState"] + lines = self.filter.update(state.integLines) + self.last_camera_t = sm.logMonoTime['driverCameraState'] + if lines <= CUTOFF_IL: + self.ir_pwr = 0 + elif lines > SATURATE_IL: + self.ir_pwr = 100 + else: + self.ir_pwr = 100 * (lines - CUTOFF_IL) / (SATURATE_IL - CUTOFF_IL) + + if time.monotonic_ns() - self.last_camera_t > 1e9: + self.ir_pwr = 0 + + if self.ir_pwr != self.prev_ir_pwr or sm.frame % 100 == 0: + ir_panda = int(self.ir_pwr * MAX_IR_PANDA_VAL / 100) + self.panda.set_ir_power(ir_panda) + + HARDWARE.set_ir_power(self.ir_pwr) + self.prev_ir_pwr = self.ir_pwr + + def send_state(self, pm): + msg = messaging.new_message('peripheralState') + msg.valid = True + ps = msg.peripheralState + ps.pandaType = self.hw_type + ps.voltage, ps.current = self.hw_reader.get_values() + + if not (ps.voltage or ps.current): + h = self.panda.health() or {} + ps.voltage = h.get("voltage", 0) + ps.current = h.get("current", 0) + + ps.fanSpeedRpm = self.panda.get_fan_rpm() + + pm.send("peripheralState", msg) + + def cleanup(self): + self.hw_reader.stop() diff --git a/selfdrive/pandad/runner.py b/selfdrive/pandad/runner.py new file mode 100644 index 0000000000..d5624263cc --- /dev/null +++ b/selfdrive/pandad/runner.py @@ -0,0 +1,127 @@ +import os +import time + +from cereal import car +from cereal.messaging import SubMaster, PubMaster +import cereal.messaging as messaging +from panda import Panda +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.pandad.state_manager import PandaStateManager +from openpilot.selfdrive.pandad.peripheral import PeripheralManager +from openpilot.selfdrive.pandad.safety import PandaSafetyManager + +FAKE_SEND = os.getenv("FAKESEND") == "1" + +class PandaRunner: + def __init__(self, serials, pandas): + self.pandas = pandas + self.usb_pandas = {p.get_usb_serial() for p in pandas if not p.spi} + self.hw_types = [int.from_bytes(p.get_type(), 'big') for p in pandas] + + for panda in self.pandas: + if os.getenv("BOARDD_LOOPBACK"): + panda.set_can_loopback(True) + for i in range(3): + panda.set_canfd_auto(i, True) + + self.sm = SubMaster(["selfdriveState", "deviceState", "driverCameraState"]) + self.pm = PubMaster(["can", "pandaStates", "peripheralState"]) + self.sendcan_sock = messaging.sub_sock('sendcan', timeout=10) + self.sendcan_buffer = None + + self.state_mgr = PandaStateManager(pandas, self.hw_types) + self.periph_mgr = PeripheralManager(pandas[0], self.hw_types[0]) + self.safety_mgr = PandaSafetyManager(pandas) + + def _can_send(self): + # TODO: this needs to have a strict timeout of <10ms and handle NACKs well (buffer the data) + while (msg := messaging.recv_one_or_none(self.sendcan_sock)): + # drop msg if too old + if (time.monotonic_ns() - msg.logMonoTime) / 1e9 > 1.0: + cloudlog.warning("skipping CAN send, too old") + continue + + # Group CAN messages by panda based on bus offset + panda_msgs = [[] for _ in self.pandas] + for c in msg.sendcan: + panda_idx = c.src // 4 # Each panda handles 4 buses + if panda_idx < len(self.pandas): + # Adjust bus number for the panda (remove offset) + adjusted_bus = c.src % 4 + panda_msgs[panda_idx].append((c.address, c.dat, adjusted_bus)) + + # Send messages to each panda + for panda_idx, can_msgs in enumerate(panda_msgs): + if can_msgs: + self.pandas[panda_idx].can_send_many(can_msgs) + + def _can_recv(self): + cans = [] + for panda_idx, p in enumerate(self.pandas): + bus_offset = panda_idx * 4 # Each panda gets 4 buses + for address, dat, src in p.can_recv(): + if src >= 192: # Rejected message + base_bus = src - 192 + adjusted_src = base_bus + bus_offset + 192 + elif src >= 128: # Returned message + base_bus = src - 128 + adjusted_src = base_bus + bus_offset + 128 + else: # Normal message + adjusted_src = src + bus_offset + cans.append((address, dat, adjusted_src)) + + msg = messaging.new_message('can', len(cans) if cans else 0) + msg.valid = True + if cans: + for i, can_info in enumerate(cans): + can = msg.can[i] + can.address, can.dat, can.src = can_info + self.pm.send("can", msg) + + def run(self, evt): + rk = Ratekeeper(100, print_delay_threshold=None) + engaged = False + + try: + while not evt.is_set(): + # receive CAN messages + self._can_recv() + + # send CAN messages + self._can_send() + + # Process peripheral state at 20 Hz + if rk.frame % 5 == 0: + self.sm.update(0) + engaged = self.sm.all_checks() and self.sm["selfdriveState"].enabled + self.periph_mgr.process(self.sm) + + # Process panda state at 10 Hz + if rk.frame % 10 == 0: + ignition = self.state_mgr.process(engaged, self.pm) + if not ignition and rk.frame % 100 == 0: + if set(Panda.list(usb_only=True)) != self.usb_pandas: + cloudlog.warning("Reconnecting to new panda") + evt.set() + break + + self.safety_mgr.configure_safety_mode() + + # Send out peripheralState at 2Hz + if rk.frame % 50 == 0: + self.periph_mgr.send_state(self.pm) + + rk.keep_time() + except Exception as e: + cloudlog.error(f"Exception in main loop: {e}") + finally: + evt.set() + self.periph_mgr.cleanup() + + # Close relay on exit to prevent a fault + is_onroad = Params().get_bool("IsOnroad") + if is_onroad and not engaged: + for p in self.pandas: + p.set_safety_mode(car.CarParams.SafetyModel.noOutput) diff --git a/selfdrive/pandad/safety.py b/selfdrive/pandad/safety.py new file mode 100644 index 0000000000..99483899ff --- /dev/null +++ b/selfdrive/pandad/safety.py @@ -0,0 +1,76 @@ +from panda import Panda +from cereal import car +from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + +class PandaSafetyManager: + def __init__(self, pandas: list[Panda]): + self.pandas = pandas + self.params = Params() + self.safety_configured = False + self.initialized = False + self.prev_obd_multiplexing = False + self.log_once = False + + def configure_safety_mode(self): + is_onroad = self.params.get_bool("IsOnroad") + + if is_onroad and not self.safety_configured: + self.update_multiplexing_mode() + car_params = self.fetch_car_params() + if car_params: + cloudlog.warning(f"got {len(car_params)} bytes CarParams") + self.set_safety_mode(car_params) + self.safety_configured = True + elif not is_onroad: + self.initialized = False + self.safety_configured = False + self.log_once = False + + def update_multiplexing_mode(self): + # Initialize to ELM327 without OBD multiplexing for initial fingerprinting + if not self.initialized: + self.prev_obd_multiplexing = False + for panda in self.pandas: + panda.set_safety_mode(car.CarParams.SafetyModel.elm327, 1) + self.initialized = True + + # Switch between multiplexing modes based on OBD multiplexing request + obd_multiplexing_requested = self.params.get_bool("ObdMultiplexingEnabled") + if obd_multiplexing_requested != self.prev_obd_multiplexing: + for i, panda in enumerate(self.pandas): + safety_param = 1 if i > 0 or not obd_multiplexing_requested else 0 + panda.set_safety_mode(car.CarParams.SafetyModel.elm327, safety_param) + self.prev_obd_multiplexing = obd_multiplexing_requested + self.params.put_bool("ObdMultiplexingChanged", True) + + def fetch_car_params(self) -> bytes: + if not self.params.get_bool("FirmwareQueryDone"): + return b"" + + if not self.log_once: + cloudlog.warning("Finished FW query, waiting for params to set safety model") + self.log_once = True + + if not self.params.get_bool("ControlsReady"): + return b"" + return self.params.get("CarParams") or b"" + + def set_safety_mode(self, params_bytes: bytes): + # Parse CarParams from bytes + with car.CarParams.from_bytes(params_bytes) as car_params: + safety_configs = car_params.safetyConfigs + alternative_experience = car_params.alternativeExperience + + for i, panda in enumerate(self.pandas): + # Default to SILENT if no config for this panda + safety_model = car.CarParams.SafetyModel.silent + + safety_param = 0 + if i < len(safety_configs): + safety_model = car.CarParams.SafetyModel.schema.enumerants[safety_configs[i].safetyModel] + safety_param = safety_configs[i].safetyParam + + cloudlog.warning(f"Panda {i}: setting safety model: {safety_model}, param: {safety_param}, alternative experience: {alternative_experience}") + panda._handle.controlWrite(Panda.REQUEST_OUT, 0xdf, alternative_experience, 0, b'') + panda.set_safety_mode(safety_model, safety_param) diff --git a/selfdrive/pandad/spi.cc b/selfdrive/pandad/spi.cc deleted file mode 100644 index b6ee57801a..0000000000 --- a/selfdrive/pandad/spi.cc +++ /dev/null @@ -1,410 +0,0 @@ -#ifndef __APPLE__ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "common/util.h" -#include "common/timing.h" -#include "common/swaglog.h" -#include "panda/board/comms_definitions.h" -#include "selfdrive/pandad/panda_comms.h" - - -#define SPI_SYNC 0x5AU -#define SPI_HACK 0x79U -#define SPI_DACK 0x85U -#define SPI_NACK 0x1FU -#define SPI_CHECKSUM_START 0xABU - - -enum SpiError { - NACK = -2, - ACK_TIMEOUT = -3, -}; - -const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds -const std::string SPI_DEVICE = "/dev/spidev0.0"; - -class LockEx { -public: - LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) { - m.lock(); - flock(fd, LOCK_EX); - } - - ~LockEx() { - flock(fd, LOCK_UN); - m.unlock(); - } - -private: - int fd; - std::recursive_mutex &m; -}; - -#define SPILOG(fn, fmt, ...) do { \ - fn(fmt, ## __VA_ARGS__); \ - fn(" %d / 0x%x / %d / %d / tx: %s", \ - xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \ - util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \ - } while (0) - -PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) { - int ret; - const int uid_len = 12; - uint8_t uid[uid_len] = {0}; - - uint32_t spi_mode = SPI_MODE_0; - uint8_t spi_bits_per_word = 8; - - // 50MHz is the max of the 845. note that some older - // revs of the comma three may not support this speed - uint32_t spi_speed = 50000000; - try { - if (!util::file_exists(SPI_DEVICE)) { - throw std::runtime_error("Error connecting to panda: SPI device not found"); - } - - spi_fd = open(SPI_DEVICE.c_str(), O_RDWR); - if (spi_fd < 0) { - LOGE("failed opening SPI device %d", spi_fd); - throw std::runtime_error("Error connecting to panda: failed to open SPI device"); - } - - // SPI settings - util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode, "failed setting SPI mode"); - util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed, "failed setting SPI speed"); - util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word"); - - // get hw UID/serial - ret = control_read(0xc3, 0, 0, uid, uid_len, 100); - if (ret == uid_len) { - std::stringstream stream; - for (int i = 0; i < uid_len; i++) { - stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]); - } - hw_serial = stream.str(); - } else { - LOGD("failed to get serial %d", ret); - throw std::runtime_error("Error connecting to panda: failed to get serial"); - } - - if (!serial.empty() && (serial != hw_serial)) { - throw std::runtime_error("Error connecting to panda: serial mismatch"); - } - - } catch (...) { - cleanup(); - throw; - } - return; -} - -PandaSpiHandle::~PandaSpiHandle() { - std::lock_guard lk(hw_lock); - cleanup(); -} - -void PandaSpiHandle::cleanup() { - if (spi_fd != -1) { - close(spi_fd); - spi_fd = -1; - } -} - - - -int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) { - ControlPacket_t packet = { - .request = request, - .param1 = param1, - .param2 = param2, - .length = 0 - }; - return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0, timeout); -} - -int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) { - ControlPacket_t packet = { - .request = request, - .param1 = param1, - .param2 = param2, - .length = length - }; - return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length, timeout); -} - -int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return bulk_transfer(endpoint, data, length, NULL, 0, timeout); -} -int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { - return bulk_transfer(endpoint, NULL, 0, data, length, timeout); -} - -int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) { - const int xfer_size = SPI_BUF_SIZE - 0x40; - - int ret = 0; - uint16_t length = (tx_data != NULL) ? tx_len : rx_len; - for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) { - int d; - if (tx_data != NULL) { - int len = std::min(xfer_size, tx_len - (xfer_size * i)); - d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0, timeout); - } else { - uint16_t to_read = std::min(xfer_size, rx_len - ret); - d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout); - } - - if (d < 0) { - SPILOG(LOGE, "SPI: bulk transfer failed with %d", d); - comms_healthy = false; - return d; - } - - ret += d; - if ((rx_data != NULL) && d < xfer_size) { - break; - } - } - - return ret; -} - -std::vector PandaSpiHandle::list() { - try { - PandaSpiHandle sh(""); - return {sh.hw_serial}; - } catch (std::exception &e) { - // no panda on SPI - } - return {}; -} - -void add_checksum(uint8_t *data, int data_len) { - data[data_len] = SPI_CHECKSUM_START; - for (int i=0; i < data_len; i++) { - data[data_len] ^= data[i]; - } -} - -bool check_checksum(uint8_t *data, int data_len) { - uint8_t checksum = SPI_CHECKSUM_START; - for (uint16_t i = 0U; i < data_len; i++) { - checksum ^= data[i]; - } - return checksum == 0U; -} - - -int PandaSpiHandle::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 ret; - int nack_count = 0; - int timeout_count = 0; - bool timed_out = false; - double start_time = millis_since_boot(); - - do { - ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout); - - if (ret < 0) { - timed_out = (timeout != 0) && (timeout_count > 5); - timeout_count += ret == SpiError::ACK_TIMEOUT; - - // give other threads a chance to run - std::this_thread::yield(); - - if (ret == SpiError::NACK) { - // prevent busy waiting while the panda is NACK'ing - // due to full TX buffers - nack_count += 1; - if (nack_count > 3) { - SPILOG(LOGD, "NACK sleep %d", nack_count); - usleep(std::clamp(nack_count*10, 200, 2000)); - } - } - } - } while (ret < 0 && connected && !timed_out); - - if (ret < 0) { - SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time); - } - - return ret; -} - -int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) { - double start_millis = millis_since_boot(); - if (timeout == 0) { - timeout = SPI_ACK_TIMEOUT; - } - timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT); - - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf, - .len = length, - }; - memset(tx_buf, tx, length); - - while (true) { - int ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send ACK request"); - return ret; - } - - if (rx_buf[0] == ack) { - break; - } else if (rx_buf[0] == SPI_NACK) { - SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack); - return SpiError::NACK; - } - - // handle timeout - if (millis_since_boot() - start_millis > timeout) { - SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack); - return SpiError::ACK_TIMEOUT; - } - } - - return 0; -} - -int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) { - static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1")); - - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob) { - printf("transfer len error\n"); - t.len = rand() % SPI_BUF_SIZE; - } - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) { - printf("corrupting TX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } - } - - int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t); - - if (err_prob > 0) { - if ((static_cast(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) { - printf("corrupting RX\n"); - for (int i = 0; i < t.len; i++) { - if ((static_cast(rand()) / RAND_MAX) > 0.9) { - ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256); - } - } - } - } - - return ret; -} - -int PandaSpiHandle::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 ret; - uint16_t rx_data_len; - LockEx lock(spi_fd, hw_lock); - - // needs to be less, since we need to have space for the checksum - assert(tx_len < SPI_BUF_SIZE); - assert(max_rx_len < SPI_BUF_SIZE); - - xfer_count++; - header = { - .sync = SPI_SYNC, - .endpoint = endpoint, - .tx_len = tx_len, - .max_rx_len = max_rx_len - }; - - spi_ioc_transfer transfer = { - .tx_buf = (uint64_t)tx_buf, - .rx_buf = (uint64_t)rx_buf - }; - - // Send header - memcpy(tx_buf, &header, sizeof(header)); - add_checksum(tx_buf, sizeof(header)); - transfer.len = sizeof(header) + 1; - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send header"); - goto fail; - } - - // Wait for (N)ACK - ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1); - if (ret < 0) { - goto fail; - } - - // Send data - if (tx_data != NULL) { - memcpy(tx_buf, tx_data, tx_len); - } - add_checksum(tx_buf, tx_len); - transfer.len = tx_len + 1; - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to send data"); - goto fail; - } - - // Wait for (N)ACK - ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3); - if (ret < 0) { - goto fail; - } - - // Read data - rx_data_len = *(uint16_t *)(rx_buf+1); - if (rx_data_len >= SPI_BUF_SIZE) { - SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len); - goto fail; - } - - transfer.len = rx_data_len + 1; - transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1); - ret = lltransfer(transfer); - if (ret < 0) { - SPILOG(LOGE, "SPI: failed to read rx data"); - goto fail; - } - if (!check_checksum(rx_buf, rx_data_len + 4)) { - SPILOG(LOGE, "SPI: bad checksum"); - goto fail; - } - - if (rx_data != NULL) { - memcpy(rx_data, rx_buf + 3, rx_data_len); - } - - return rx_data_len; - -fail: - // ensure slave is in a consistent state - // and ready for the next transfer - int nack_cnt = 0; - while (nack_cnt < 3) { - if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) { - nack_cnt += 1; - } else { - nack_cnt = 0; - } - } - - if (ret >= 0) ret = -1; - return ret; -} -#endif diff --git a/selfdrive/pandad/state_manager.py b/selfdrive/pandad/state_manager.py new file mode 100644 index 0000000000..4a914bf94f --- /dev/null +++ b/selfdrive/pandad/state_manager.py @@ -0,0 +1,126 @@ +import os +from cereal import car, log +import cereal.messaging as messaging +from panda.python import PANDA_BUS_CNT + +SPOOFING_STARTED = os.getenv("STARTED") == "1" + +LEC_ERROR_CODE = { + "No error": 0, "Stuff error": 1, "Form error": 2, "AckError": 3, + "Bit1Error": 4, "Bit0Error": 5, "CRCError": 6, "NoChange": 7, +} + +class PandaStateManager: + def __init__(self, pandas, hw_types): + self.pandas = pandas + self.hw_types = hw_types + + self.is_comma_three_red = ( + len(pandas) == 2 and + self.hw_types[0] == log.PandaState.PandaType.dos and + self.hw_types[1] == log.PandaState.PandaType.redPanda + ) + + def process(self, engaged, pm) -> bool: + msg = messaging.new_message('pandaStates', len(self.pandas)) + msg.valid = True + panda_states = msg.pandaStates + ignition = False + + for i, panda in enumerate(self.pandas): + health = panda.health() or {} + + if SPOOFING_STARTED: + health['ignition_line'] = 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 self.is_comma_three_red and i == 0: + health['ignition_line'] = 0 + + ignition |= bool(health['ignition_line']) or bool(health['ignition_can']) + ps = panda_states[i] + self._fill_state(ps, self.hw_types[i], health) + + # Fill can state + for j in range(PANDA_BUS_CNT): + can_health = panda.can_health(j) + can_state = ps.init(f'canState{j}') + self._fill_can_state(can_state, can_health) + + # Set faults + fault_bits = int(health.get('faults', 0)) + faults_list = [f for f in range(log.PandaState.FaultType.relayMalfunction, + log.PandaState.FaultType.heartbeatLoopWatchdog + 1) + if fault_bits & (1 << f)] + faults = ps.init('faults', len(faults_list)) + for idx, fault in enumerate(faults_list): + faults[idx] = fault + + for panda, ps in zip(self.pandas, panda_states, strict=False): + if ps.safetyModel == car.CarParams.SafetyModel.silent or ( + not ignition and ps.safetyModel != car.CarParams.SafetyModel.noOutput): + panda.set_safety_mode(car.CarParams.SafetyModel.noOutput) + + if ps.powerSaveEnabled != (not ignition): + panda.set_power_save(not ignition) + + panda.send_heartbeat(engaged) + + pm.send("pandaStates", msg) + return ignition + + def _fill_state(self, ps, hw_type, health): + ps.voltage = health['voltage'] + ps.current = health['current'] + ps.uptime = health['uptime'] + ps.safetyTxBlocked = health['safety_tx_blocked'] + ps.safetyRxInvalid = health['safety_rx_invalid'] + ps.ignitionLine = bool(health['ignition_line']) + ps.ignitionCan = bool(health['ignition_can']) + ps.controlsAllowed = bool(health['controls_allowed']) + ps.txBufferOverflow = health['tx_buffer_overflow'] + ps.rxBufferOverflow = health['rx_buffer_overflow'] + ps.pandaType = hw_type + ps.safetyModel = health['safety_mode'] + ps.safetyParam = health['safety_param'] + ps.faultStatus = health['fault_status'] + ps.powerSaveEnabled = bool(health['power_save_enabled']) + ps.heartbeatLost = bool(health['heartbeat_lost']) + ps.alternativeExperience = health['alternative_experience'] + ps.harnessStatus = health['car_harness_status'] + ps.interruptLoad = health['interrupt_load'] + ps.fanPower = health['fan_power'] + ps.fanStallCount = health['fan_stall_count'] + ps.safetyRxChecksInvalid = bool(health['safety_rx_checks_invalid']) + ps.spiErrorCount = health['spi_error_count'] + ps.sbu1Voltage = health['sbu1_voltage_mV'] / 1000.0 + ps.sbu2Voltage = health['sbu2_voltage_mV'] / 1000.0 + + def _fill_can_state(self, cs, can_health): + cs.busOff = bool(can_health['bus_off']) + cs.busOffCnt = can_health['bus_off_cnt'] + cs.errorWarning = bool(can_health['error_warning']) + cs.errorPassive = bool(can_health['error_passive']) + cs.lastError = LEC_ERROR_CODE[can_health['last_error']] + cs.lastStoredError = LEC_ERROR_CODE[can_health['last_stored_error']] + cs.lastDataError = LEC_ERROR_CODE[can_health['last_data_error']] + cs.lastDataStoredError = LEC_ERROR_CODE[can_health['last_data_stored_error']] + cs.receiveErrorCnt = can_health['receive_error_cnt'] + cs.transmitErrorCnt = can_health['transmit_error_cnt'] + cs.totalErrorCnt = can_health['total_error_cnt'] + cs.totalTxLostCnt = can_health['total_tx_lost_cnt'] + cs.totalRxLostCnt = can_health['total_rx_lost_cnt'] + cs.totalTxCnt = can_health['total_tx_cnt'] + cs.totalRxCnt = can_health['total_rx_cnt'] + cs.totalFwdCnt = can_health['total_fwd_cnt'] + cs.canSpeed = can_health['can_speed'] + cs.canDataSpeed = can_health['can_data_speed'] + cs.canfdEnabled = bool(can_health['canfd_enabled']) + cs.brsEnabled = bool(can_health['brs_enabled']) + cs.canfdNonIso = bool(can_health['canfd_non_iso']) + cs.irq0CallRate = can_health['irq0_call_rate'] + cs.irq1CallRate = can_health['irq1_call_rate'] + cs.irq2CallRate = can_health['irq2_call_rate'] + cs.canCoreResetCnt = can_health['can_core_reset_count'] diff --git a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc b/selfdrive/pandad/tests/test_pandad_usbprotocol.cc deleted file mode 100644 index 11f7184efd..0000000000 --- a/selfdrive/pandad/tests/test_pandad_usbprotocol.cc +++ /dev/null @@ -1,135 +0,0 @@ -#define CATCH_CONFIG_MAIN -#define CATCH_CONFIG_ENABLE_BENCHMARKING - -#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); - void test_can_send(); - void test_can_recv(uint32_t chunk_size = 0); - void test_chunked_can_recv(); - - std::map test_data; - int can_list_size = 0; - int total_pakets_size = 0; - MessageBuilder msg; - 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_) { - this->hw_type = hw_type; - int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8); - // prepare test data - for (int i = 0; i < data_limit; ++i) { - std::random_device rd; - std::independent_bits_engine rbe(rd()); - - int data_len = dlc_to_len[i]; - std::string bytes(data_len, '\0'); - std::generate(bytes.begin(), bytes.end(), std::ref(rbe)); - test_data[data_len] = bytes; - } - - // generate can messages for this panda - auto can_list = msg.initEvent().initSendcan(can_list_size); - for (uint8_t i = 0; i < can_list_size; ++i) { - auto can = can_list[i]; - 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.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size())); - total_pakets_size += sizeof(can_header) + dat.size(); - } - - can_data_list = can_list.asReader(); - INFO("test " << can_list_size << " packets, total size " << total_pakets_size); -} - -void PandaTest::test_can_send() { - std::vector unpacked_data; - this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) { - unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]); - }); - REQUIRE(unpacked_data.size() == total_pakets_size); - - int cnt = 0; - INFO("test can message integrity"); - for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) { - can_header header; - memcpy(&header, &unpacked_data[pos], sizeof(can_header)); - const uint8_t data_len = dlc_to_len[header.data_len_code]; - pckt_len = sizeof(can_header) + data_len; - - REQUIRE(header.addr == cnt); - REQUIRE(test_data.find(data_len) != test_data.end()); - const std::string &dat = test_data[data_len]; - REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0); - ++cnt; - } - REQUIRE(cnt == can_list_size); -} - -void PandaTest::test_can_recv(uint32_t rx_chunk_size) { - std::vector frames; - this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) { - if (rx_chunk_size == 0) { - REQUIRE(this->unpack_can_buffer(data, size, frames)); - } else { - this->receive_buffer_size = 0; - uint32_t pos = 0; - - while (pos < size) { - uint32_t chunk_size = std::min(rx_chunk_size, size - pos); - memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size); - this->receive_buffer_size += chunk_size; - pos += chunk_size; - - REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames)); - } - } - }); - - REQUIRE(frames.size() == can_list_size); - for (int i = 0; i < frames.size(); ++i) { - REQUIRE(frames[i].address == i); - REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end()); - const std::string &dat = test_data[frames[i].dat.size()]; - REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0); - } -} - -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); - - SECTION("can_send") { - test.test_can_send(); - } - SECTION("can_receive") { - test.test_can_recv(); - } - SECTION("chunked_can_receive") { - test.test_can_recv(0x40); - } -} - -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); - - SECTION("can_send") { - test.test_can_send(); - } - SECTION("can_receive") { - test.test_can_recv(); - } - SECTION("chunked_can_receive") { - test.test_can_recv(0x40); - } -} diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 0149653c84..3a6a9d3190 100644 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -33,7 +33,7 @@ CPU usage budget TEST_DURATION = 25 LOG_OFFSET = 8 -MAX_TOTAL_CPU = 280. # total for all 8 cores +MAX_TOTAL_CPU = 310. # total for all 8 cores PROCS = { # Baseline CPU usage by process "selfdrive.controls.controlsd": 16.0, @@ -63,7 +63,7 @@ PROCS = { "./logcatd": 1.0, "system.micd": 5.0, "system.timed": 0, - "selfdrive.pandad.pandad": 0, + "selfdrive.pandad.pandad": 22.0, "system.statsd": 1.0, "system.loggerd.uploader": 15.0, "system.loggerd.deleter": 1.0, @@ -71,12 +71,10 @@ PROCS = { PROCS.update({ "tici": { - "./pandad": 5.0, "./ubloxd": 1.0, "system.ubloxd.pigeond": 6.0, }, "tizi": { - "./pandad": 19.0, "system.qcomgpsd.qcomgpsd": 1.0, } }.get(HARDWARE.get_device_type(), {})) diff --git a/system/hardware/base.h b/system/hardware/base.h index df9700a017..cbe51a5028 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -25,7 +25,6 @@ public: static void reboot() {} static void poweroff() {} static void set_brightness(int percent) {} - static void set_ir_power(int percentage) {} static void set_display_power(bool on) {} static bool get_ssh_enabled() { return false; } diff --git a/system/hardware/base.py b/system/hardware/base.py index b457ea4e17..27df1dac07 100644 --- a/system/hardware/base.py +++ b/system/hardware/base.py @@ -225,3 +225,12 @@ class HardwareBase(ABC): def get_modem_data_usage(self): return -1, -1 + + def get_voltage(self) -> float: + return 0. + + def get_current(self) -> float: + return 0. + + def set_ir_power(self, percent: int): + pass diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index ed8a7e7d17..55b8f83c14 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -66,19 +66,6 @@ public: std::ofstream("/sys/class/backlight/panel0-backlight/bl_power") << (on ? "0" : "4") << "\n"; } - static void set_ir_power(int percent) { - auto device = get_device_type(); - if (device == cereal::InitData::DeviceType::TICI || - device == cereal::InitData::DeviceType::TIZI) { - return; - } - - int value = util::map_val(std::clamp(percent, 0, 100), 0, 100, 0, 300); - std::ofstream("/sys/class/leds/led:switch_2/brightness") << 0 << "\n"; - std::ofstream("/sys/class/leds/led:torch_2/brightness") << value << "\n"; - std::ofstream("/sys/class/leds/led:switch_2/brightness") << value << "\n"; - } - static std::map get_init_logs() { std::map ret = { {"/BUILD", util::read_file("/BUILD")}, diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 35f9916c31..f10ae20643 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -116,6 +116,24 @@ class Tici(HardwareBase): def get_serial(self): return self.get_cmdline()['androidboot.serialno'] + def get_voltage(self): + with open("/sys/class/hwmon/hwmon1/in1_input") as f: + return int(f.read()) + + def get_current(self): + with open("/sys/class/hwmon/hwmon1/curr1_input") as f: + return int(f.read()) + + def set_ir_power(self, percent: int): + if self.get_device_type() in ("tici", "tizi"): + return + + value = int((percent / 100) * 300) + with open("/sys/class/leds/led:torch_2/brightness", "w") as f: + f.write(f"{value}\n") + with open("/sys/class/leds/led:switch_2/brightness", "w") as f: + f.write(f"{value}\n") + def get_network_type(self): try: primary_connection = self.nm.Get(NM, 'PrimaryConnection', dbus_interface=DBUS_PROPS, timeout=TIMEOUT) diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 5758512f2b..e3bd21e029 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -84,7 +84,6 @@ procs = [ PythonProcess("raylib_ui", "selfdrive.ui.ui", always_run, enabled=False, watchdog_max_dt=(5 if not PC else None)), PythonProcess("soundd", "selfdrive.ui.soundd", only_onroad), PythonProcess("locationd", "selfdrive.locationd.locationd", only_onroad), - NativeProcess("_pandad", "selfdrive/pandad", ["./pandad"], always_run, enabled=False), PythonProcess("calibrationd", "selfdrive.locationd.calibrationd", only_onroad), PythonProcess("torqued", "selfdrive.locationd.torqued", only_onroad), PythonProcess("controlsd", "selfdrive.controls.controlsd", and_(not_joystick, iscar)),