spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

panda: refactor can_send (#22981) 

* refactor

* merge master

* fix

* move  down

* space

* merge master

* remove struct PacketWriter, use function

* Update selfdrive/boardd/panda.cc

less violations

* Update selfdrive/boardd/panda.cc

comment

* Update selfdrive/boardd/panda.cc

add comment to counter

Co-authored-by: Igor Biletskyy <briskspirit@users.noreply.github.com>
pull/23151/head
Dean Lee 2021-12-07 12:04:25 +08:00 committed by GitHub
parent baf1a77ca2
commit 827fae8faa
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 37 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
selfdrive/boardd/panda.cc
View File

@ -363,57 +363,49 @@ static uint8_t len_to_dlc(uint8_t len) {
}
}
static void write_packet(uint8_t *dest, int *write_pos, const uint8_t *src, size_t size) {
for (int i = 0, &pos = *write_pos; i < size; ++i, ++pos) {
// Insert counter every 64 bytes (first byte of 64 bytes USB packet)
if (pos % USBPACKET_MAX_SIZE == 0) {
dest[pos] = pos / USBPACKET_MAX_SIZE;
pos++;
}
dest[pos] = src[i];
}
}
void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
std::function<void(uint8_t *, size_t)> write_func) {
if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
}
std::function<void(uint8_t *, size_t)> write_func) {
int32_t pos = 0;
uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
int msg_count = 0;
while (msg_count < can_data_list.size()) {
uint32_t pos = 0;
while (pos < USB_TX_SOFT_LIMIT) {
if (msg_count == can_data_list.size()) { break; }
auto cmsg = can_data_list[msg_count];
// check if the message is intended for this panda
uint8_t bus = cmsg.getSrc();
if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_CNT)) {
msg_count++;
continue;
}
auto can_data = cmsg.getDat();
uint8_t data_len_code = len_to_dlc(can_data.size());
assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8));
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;
memcpy(&send[pos], &header, CANPACKET_HEAD_SIZE);
memcpy(&send[pos+CANPACKET_HEAD_SIZE], can_data.begin(), can_data.size());
pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code];
msg_count++;
for (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_CNT)) {
continue;
}
auto can_data = cmsg.getDat();
uint8_t data_len_code = len_to_dlc(can_data.size());
assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8));
assert(can_data.size() == dlc_to_len[data_len_code]);
if (pos > 0) { // Helps not to spam with ZLP
// Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet)
uint8_t counter = 0;
uint8_t to_write[USB_TX_SOFT_LIMIT+128];
int ptr = 0;
for (int i = 0; i < pos; i += 63) {
to_write[ptr] = counter;
int copy_size = ((pos - i) < 63) ? (pos - i) : 63;
memcpy(&to_write[ptr+1], &(send.data()[i]) , copy_size);
ptr += copy_size + 1;
counter++;
}
write_func(to_write, ptr);
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;
write_packet(send_buf, &pos, (uint8_t *)&header, sizeof(can_header));
write_packet(send_buf, &pos, (uint8_t *)can_data.begin(), can_data.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(capnp::List<cereal::CanData>::Reader can_data_list) {

1
selfdrive/boardd/panda.h
View File

@ -69,7 +69,6 @@ class Panda {
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle = NULL;
std::mutex usb_lock;
std::vector<uint8_t> send;
std::vector<uint8_t> recv_buf;
void handle_usb_issue(int err, const char func[]);
void cleanup();