mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-22 14:32:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

panda: refactor can_recv (#22977)

Browse Source 
* refactor

* use macro

* rebase master

* fix incorrect chunk_len

* cleanup

* type fix

* lines

* while

Co-authored-by: Igor Biletksyy <bs@privacy.im>
This commit is contained in:
Dean Lee
2021-12-03 08:28:38 +08:00
committed by GitHub
parent 0ee6f42a8e
commit 599c07e027
3 changed files with 29 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/boardd/panda.cc
+26 -46
View File
@@ -425,64 +425,44 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
bool Panda::can_receive(std::vector<can_frame>& out_vec) {
uint8_t data[RECV_SIZE];
int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
// Not sure if this can happen
if (recv < 0) recv = 0;
if (recv == RECV_SIZE) {
LOGW("Receive buffer full");
}
if (!comms_healthy) {
return false;
}
return unpack_can_buffer(data, recv, out_vec);
if (recv == RECV_SIZE) {
LOGW("Panda receive buffer full");
}
return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec);
}
bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec) {
static uint8_t tail[CANPACKET_MAX_SIZE];
uint8_t tail_size = 0;
uint8_t counter = 0;
recv_buf.clear();
for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) {
// Check for counter every 64 bytes (length of USB packet)
if (counter != data[i]) {
if (data[i] != i / USBPACKET_MAX_SIZE) {
LOGE("CAN: MALFORMED USB RECV PACKET");
break;
comms_healthy = false;
return false;
}
counter++;
uint8_t chunk_len = ((size - i) > USBPACKET_MAX_SIZE) ? 63 : (size - i - 1); // as 1 is always reserved for counter
uint8_t chunk[USBPACKET_MAX_SIZE + CANPACKET_MAX_SIZE];
memcpy(chunk, tail, tail_size);
memcpy(&chunk[tail_size], &data[i+1], chunk_len);
chunk_len += tail_size;
tail_size = 0;
uint8_t pos = 0;
while (pos < chunk_len) {
uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)];
uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
if (pckt_len <= (chunk_len - pos)) {
can_header header;
memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i));
recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]);
}
can_frame &canData = out_vec.emplace_back();
canData.busTime = 0;
canData.address = header.addr;
canData.src = header.bus + bus_offset;
int pos = 0;
while (pos < recv_buf.size()) {
can_header header;
memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE);
if (header.rejected) { canData.src += CANPACKET_REJECTED; }
if (header.returned) { canData.src += CANPACKET_RETURNED; }
canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
can_frame &canData = out_vec.emplace_back();
canData.busTime = 0;
canData.address = header.addr;
canData.src = header.bus + bus_offset;
if (header.rejected) { canData.src += CANPACKET_REJECTED; }
if (header.returned) { canData.src += CANPACKET_RETURNED; }
pos += pckt_len;
} else {
// Keep partial CAN packet until next USB packet
tail_size = (chunk_len - pos);
assert(tail_size <= CANPACKET_MAX_SIZE);
memcpy(tail, &chunk[pos], tail_size);
break;
}
}
const uint8_t data_len = dlc_to_len[header.data_len_code];
canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len);
pos += CANPACKET_HEAD_SIZE + data_len;
}
return true;
}
selfdrive/boardd/panda.h
+2 -1
View File
@@ -18,7 +18,7 @@
#define PANDA_BUS_CNT 4
#define RECV_SIZE (0x4000U)
#define USB_TX_SOFT_LIMIT (0x100U)
#define USBPACKET_MAX_SIZE (0x40U)
#define USBPACKET_MAX_SIZE (0x40)
#define CANPACKET_HEAD_SIZE 5U
#define CANPACKET_MAX_SIZE 72U
#define CANPACKET_REJECTED (0xC0U)
@@ -70,6 +70,7 @@ class Panda {
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();
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
+1 -1
View File
@@ -64,7 +64,7 @@ void PandaTest::test_can_send() {
for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) {
REQUIRE(chunk[i] == counter);
const int len = std::min(USBPACKET_MAX_SIZE, (uint32_t)size_left);
const int len = std::min(USBPACKET_MAX_SIZE, size_left);
unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]);
size_left -= len;
}