mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-10-01 03:43:46 +08:00
cabana: add independent panda lib (#35920)
* cabana: add separate panda lib * cleanup
This commit is contained in:
Adeeb Shihadeh
@@ -15,7 +15,8 @@ else:
|
||||
qt_libs = ['qt_util'] + base_libs
|
||||
|
||||
cabana_env = qt_env.Clone()
|
||||
cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'panda', 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'usb-1.0'] + qt_libs
|
||||
|
||||
cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'usb-1.0'] + qt_libs
|
||||
opendbc_path = '-DOPENDBC_FILE_PATH=\'"%s"\'' % (cabana_env.Dir("../../opendbc/dbc").abspath)
|
||||
cabana_env['CXXFLAGS'] += [opendbc_path]
|
||||
|
||||
@@ -29,7 +30,7 @@ cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/socketcans
|
||||
'streams/routes.cc', 'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc',
|
||||
'utils/export.cc', 'utils/util.cc',
|
||||
'chart/chartswidget.cc', 'chart/chart.cc', 'chart/signalselector.cc', 'chart/tiplabel.cc', 'chart/sparkline.cc',
|
||||
'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc',
|
||||
'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc', 'panda.cc',
|
||||
'cameraview.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc', 'tools/findsignal.cc', 'tools/routeinfo.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)
|
||||
cabana_env.Program('cabana', ['cabana.cc', cabana_lib, assets], LIBS=cabana_libs, FRAMEWORKS=base_frameworks)
|
||||
|
||||
@@ -40,4 +41,4 @@ output_json_file = 'tools/cabana/dbc/car_fingerprint_to_dbc.json'
|
||||
generate_dbc = cabana_env.Command('#' + output_json_file,
|
||||
['dbc/generate_dbc_json.py'],
|
||||
"python3 tools/cabana/dbc/generate_dbc_json.py --out " + output_json_file)
|
||||
cabana_env.Depends(generate_dbc, ["#common", "#selfdrive/pandad", '#opendbc_repo', "#cereal", "#msgq_repo"])
|
||||
cabana_env.Depends(generate_dbc, ["#common", '#opendbc_repo', "#cereal", "#msgq_repo"])
|
||||
|
||||
@@ -0,0 +1,346 @@
|
||||
#include "tools/cabana/panda.h"
|
||||
|
||||
#include <unistd.h>
|
||||
#include <cassert>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include "cereal/messaging/messaging.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/util.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;
|
||||
}
|
||||
|
||||
Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
|
||||
if (!init_usb_connection(serial)) {
|
||||
throw std::runtime_error("Error connecting to panda");
|
||||
}
|
||||
|
||||
LOGW("connected to %s over USB", serial.c_str());
|
||||
hw_type = get_hw_type();
|
||||
can_reset_communications();
|
||||
}
|
||||
|
||||
Panda::~Panda() {
|
||||
cleanup_usb();
|
||||
}
|
||||
|
||||
bool Panda::connected() {
|
||||
return connected_flag;
|
||||
}
|
||||
|
||||
bool Panda::comms_healthy() {
|
||||
return comms_healthy_flag;
|
||||
}
|
||||
|
||||
std::string Panda::hw_serial() {
|
||||
return hw_serial_str;
|
||||
}
|
||||
|
||||
std::vector<std::string> Panda::list(bool usb_only) {
|
||||
static std::unique_ptr<libusb_context, decltype(&libusb_exit)> context(init_usb_ctx(), libusb_exit);
|
||||
|
||||
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 Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) {
|
||||
control_write(0xdc, (uint16_t)safety_model, safety_param);
|
||||
}
|
||||
|
||||
|
||||
cereal::PandaState::PandaType Panda::get_hw_type() {
|
||||
unsigned char hw_query[1] = {0};
|
||||
|
||||
control_read(0xc1, 0, 0, hw_query, 1);
|
||||
return (cereal::PandaState::PandaType)(hw_query[0]);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void Panda::send_heartbeat(bool engaged) {
|
||||
control_write(0xf3, engaged, 0);
|
||||
}
|
||||
|
||||
void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
|
||||
control_write(0xde, bus, (speed * 10));
|
||||
}
|
||||
|
||||
|
||||
void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) {
|
||||
control_write(0xf9, bus, (speed * 10));
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool Panda::can_receive(std::vector<can_frame>& 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 = bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE);
|
||||
if (!comms_healthy()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
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() {
|
||||
control_write(0xc0, 0, 0);
|
||||
}
|
||||
|
||||
bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &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;
|
||||
}
|
||||
|
||||
// USB implementation methods
|
||||
bool Panda::init_usb_connection(const std::string& serial) {
|
||||
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_str = std::string((char *)desc_serial, ret);
|
||||
if (serial.empty() || serial == hw_serial_str) {
|
||||
break;
|
||||
}
|
||||
libusb_close(dev_handle);
|
||||
dev_handle = NULL;
|
||||
}
|
||||
}
|
||||
if (dev_handle == NULL) goto fail;
|
||||
libusb_free_device_list(dev_list, 1);
|
||||
|
||||
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 true;
|
||||
|
||||
fail:
|
||||
if (dev_list != NULL) {
|
||||
libusb_free_device_list(dev_list, 1);
|
||||
}
|
||||
cleanup_usb();
|
||||
return false;
|
||||
}
|
||||
|
||||
void Panda::cleanup_usb() {
|
||||
if (dev_handle) {
|
||||
libusb_release_interface(dev_handle, 0);
|
||||
libusb_close(dev_handle);
|
||||
dev_handle = nullptr;
|
||||
}
|
||||
|
||||
if (ctx) {
|
||||
libusb_exit(ctx);
|
||||
ctx = nullptr;
|
||||
}
|
||||
connected_flag = false;
|
||||
}
|
||||
|
||||
void Panda::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_flag = false;
|
||||
}
|
||||
}
|
||||
|
||||
int Panda::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_flag) {
|
||||
return LIBUSB_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
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_flag);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
int Panda::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_flag) {
|
||||
return LIBUSB_ERROR_NO_DEVICE;
|
||||
}
|
||||
|
||||
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_flag);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
int Panda::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||
int err;
|
||||
int transferred = 0;
|
||||
|
||||
if (!connected_flag) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
do {
|
||||
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_flag);
|
||||
|
||||
return transferred;
|
||||
}
|
||||
|
||||
int Panda::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
|
||||
int err;
|
||||
int transferred = 0;
|
||||
|
||||
if (!connected_flag) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
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_flag = false;
|
||||
LOGE_100("overflow got 0x%x", transferred);
|
||||
} else if (err != 0) {
|
||||
handle_usb_issue(err, __func__);
|
||||
}
|
||||
} while (err != 0 && connected_flag);
|
||||
|
||||
return transferred;
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
#include <ctime>
|
||||
#include <functional>
|
||||
#include <list>
|
||||
#include <memory>
|
||||
#include <optional>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
#include "cereal/gen/cpp/car.capnp.h"
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "panda/board/health.h"
|
||||
#include "panda/board/can.h"
|
||||
|
||||
#define USB_TX_SOFT_LIMIT (0x100U)
|
||||
#define USBPACKET_MAX_SIZE (0x40)
|
||||
#define RECV_SIZE (0x4000U)
|
||||
#define TIMEOUT 0
|
||||
|
||||
#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 {
|
||||
public:
|
||||
Panda(std::string serial="", uint32_t bus_offset=0);
|
||||
~Panda();
|
||||
|
||||
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);
|
||||
|
||||
// Panda functionality
|
||||
cereal::PandaState::PandaType get_hw_type();
|
||||
void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U);
|
||||
void send_heartbeat(bool engaged);
|
||||
void set_can_speed_kbps(uint16_t bus, uint16_t speed);
|
||||
void set_data_speed_kbps(uint16_t bus, uint16_t speed);
|
||||
bool can_receive(std::vector<can_frame>& out_vec);
|
||||
void can_reset_communications();
|
||||
|
||||
private:
|
||||
// USB connection members
|
||||
libusb_context *ctx = nullptr;
|
||||
libusb_device_handle *dev_handle = nullptr;
|
||||
std::string hw_serial_str;
|
||||
std::atomic<bool> connected_flag = true;
|
||||
std::atomic<bool> comms_healthy_flag = true;
|
||||
|
||||
// CAN buffer members
|
||||
uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
|
||||
uint32_t receive_buffer_size = 0;
|
||||
|
||||
// Internal methods
|
||||
bool init_usb_connection(const std::string& serial);
|
||||
void cleanup_usb();
|
||||
void handle_usb_issue(int err, const char func[]);
|
||||
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);
|
||||
bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
|
||||
uint8_t calculate_checksum(uint8_t *data, uint32_t len);
|
||||
};
|
||||
@@ -7,7 +7,7 @@
|
||||
#include <QFormLayout>
|
||||
|
||||
#include "tools/cabana/streams/livestream.h"
|
||||
#include "selfdrive/pandad/panda.h"
|
||||
#include "tools/cabana/panda.h"
|
||||
|
||||
const uint32_t speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U};
|
||||
const uint32_t data_speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U, 2000U, 5000U};
|
||||
|
||||
Reference in New Issue
Block a user