mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-07-15 14:22:11 +08:00
pandad: remove multi-panda + USB support (#37217)
* pandad: remove multi-panda support * lil more * mac * skip mac
This commit is contained in:
Vendored
+1
-9
@@ -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/"]]),
|
||||
])
|
||||
},
|
||||
|
||||
@@ -1,3 +1,3 @@
|
||||
pandad
|
||||
pandad_api_impl.cpp
|
||||
tests/test_pandad_usbprotocol
|
||||
tests/test_pandad_canprotocol
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -16,7 +16,7 @@ int main(int argc, char *argv[]) {
|
||||
assert(err == 0);
|
||||
}
|
||||
|
||||
std::vector<std::string> serials(argv + 1, argv + argc);
|
||||
pandad_main_thread(serials);
|
||||
std::string serial = (argc > 1) ? argv[1] : "";
|
||||
pandad_main_thread(serial);
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -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<PandaUsbHandle>(serial);
|
||||
LOGW("connected to %s over USB", serial.c_str());
|
||||
} catch (std::exception &e) {
|
||||
#ifndef __APPLE__
|
||||
handle = std::make_unique<PandaSpiHandle>(serial);
|
||||
LOGW("connected to %s over SPI", serial.c_str());
|
||||
#else
|
||||
throw e;
|
||||
#endif
|
||||
}
|
||||
Panda::Panda(std::string serial) {
|
||||
handle = std::make_unique<PandaSpiHandle>(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<std::string> Panda::list(bool usb_only) {
|
||||
std::vector<std::string> 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<std::string> 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<cereal::CanData>::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<cereal::CanData>::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<can_fra
|
||||
|
||||
can_frame &canData = out_vec.emplace_back();
|
||||
canData.address = header.addr;
|
||||
canData.src = header.bus + bus_offset;
|
||||
canData.src = header.bus;
|
||||
if (header.rejected) {
|
||||
canData.src += CAN_REJECTED_BUS_OFFSET;
|
||||
}
|
||||
|
||||
@@ -45,20 +45,19 @@ struct can_frame {
|
||||
|
||||
class Panda {
|
||||
private:
|
||||
std::unique_ptr<PandaCommsHandle> handle;
|
||||
std::unique_ptr<PandaSpiHandle> 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<std::string> list(bool usb_only=false);
|
||||
static std::vector<std::string> 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<cereal::CanData>::Reader &can_data_list,
|
||||
std::function<void(uint8_t *, size_t)> write_func);
|
||||
bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
|
||||
|
||||
@@ -1,227 +0,0 @@
|
||||
#include "selfdrive/pandad/panda.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <stdexcept>
|
||||
#include <memory>
|
||||
|
||||
#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<std::string> PandaUsbHandle::list() {
|
||||
static std::unique_ptr<libusb_context, decltype(&libusb_exit)> context(init_usb_ctx(), libusb_exit);
|
||||
// init libusb
|
||||
ssize_t num_devices;
|
||||
libusb_device **dev_list = NULL;
|
||||
std::vector<std::string> 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;
|
||||
}
|
||||
@@ -6,67 +6,20 @@
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#ifndef __APPLE__
|
||||
#include <linux/spi/spidev.h>
|
||||
#endif
|
||||
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
|
||||
#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<bool> connected = true;
|
||||
std::atomic<bool> comms_healthy = true;
|
||||
static std::vector<std::string> 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<std::string> 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
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
+108
-206
@@ -1,6 +1,5 @@
|
||||
#include "selfdrive/pandad/pandad.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <bitset>
|
||||
#include <cassert>
|
||||
@@ -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<Panda *> &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> panda;
|
||||
try {
|
||||
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_OFFSET));
|
||||
panda = std::make_unique<Panda>(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<Panda *> 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<Panda *> 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<Message> msg(subscriber->receive());
|
||||
if (!msg) {
|
||||
continue;
|
||||
@@ -99,25 +77,20 @@ void can_send_thread(std::vector<Panda *> 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<Panda *> &pandas, PubMaster *pm) {
|
||||
void can_recv(Panda *panda, PubMaster *pm) {
|
||||
static std::vector<can_frame> 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<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool is_onroad, bool spoofing_started) {
|
||||
bool ignition_local = false;
|
||||
const uint32_t pandas_cnt = pandas.size();
|
||||
|
||||
std::optional<bool> 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<health_t> pandaStates;
|
||||
pandaStates.reserve(pandas_cnt);
|
||||
|
||||
std::vector<std::array<can_health_t, PANDA_CAN_CNT>> 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_t, PANDA_CAN_CNT> 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_t, PANDA_CAN_CNT> 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<sizeof(health.faults_pkt) * 8> 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<sizeof(health.faults_pkt) * 8> 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<Panda *> &pandas, PubMaster *pm, bool engaged, bool is_onroad, bool spoofing_started) {
|
||||
std::vector<std::string> 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<Panda *> &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<Panda *> &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<Panda *> &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<std::string> 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<Panda *> 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;
|
||||
}
|
||||
|
||||
@@ -1,16 +1,15 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "common/params.h"
|
||||
#include "selfdrive/pandad/panda.h"
|
||||
|
||||
void pandad_main_thread(std::vector<std::string> serials);
|
||||
void pandad_main_thread(std::string serial);
|
||||
|
||||
class PandaSafety {
|
||||
public:
|
||||
PandaSafety(const std::vector<Panda *> &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<Panda *> pandas_;
|
||||
Panda *panda_;
|
||||
Params params_;
|
||||
};
|
||||
|
||||
+20
-31
@@ -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()
|
||||
|
||||
|
||||
|
||||
@@ -1,4 +1,3 @@
|
||||
#ifndef __APPLE__
|
||||
#include <sys/file.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <linux/spi/spidev.h>
|
||||
@@ -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
|
||||
|
||||
+7
-7
@@ -1,13 +1,15 @@
|
||||
#define CATCH_CONFIG_MAIN
|
||||
#define CATCH_CONFIG_ENABLE_BENCHMARKING
|
||||
|
||||
#include <climits>
|
||||
|
||||
#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<cereal::CanData>::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();
|
||||
@@ -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()})
|
||||
|
||||
@@ -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')}
|
||||
|
||||
Reference in New Issue
Block a user