mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Simple CAN chunks (#25373)

Browse Source 
* simple chunks

* more sizeofs

* fix unit tests

* bump panda

* bump panda

* don't fail for too little data

* bump panda

* bump panda

* bump panda

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
This commit is contained in:
Robbe Derks
2022-12-01 18:12:16 +01:00
committed by GitHub
parent 92ddd3c57e
commit fee6f2efac
3 changed files with 35 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
panda

Submodule panda updated: 2ae7b9a4d5...288e14cde9

55
selfdrive/boardd/panda.cc
View File

@@ -169,22 +169,15 @@ 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) {
int32_t pos = 0;
uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
uint32_t magic = CAN_TRANSACTION_MAGIC;
memcpy(&send_buf[0], &magic, sizeof(uint32_t));
pos += sizeof(uint32_t);
for (auto cmsg : can_data_list) {
// check if the message is intended for this panda
uint8_t bus = cmsg.getSrc();
@@ -202,16 +195,19 @@ void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data
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());
memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
pos += sizeof(can_header);
memcpy(&send_buf[pos], (uint8_t *)can_data.begin(), can_data.size());
pos += can_data.size();
if (pos >= USB_TX_SOFT_LIMIT) {
write_func(send_buf, pos);
pos = 0;
pos = sizeof(uint32_t);
}
}
// send remaining packets
if (pos > 0) write_func(send_buf, pos);
if (pos > sizeof(uint32_t)) write_func(send_buf, pos);
}
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
@@ -235,20 +231,23 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec) {
recv_buf.clear();
for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) {
if (data[i] != i / USBPACKET_MAX_SIZE) {
LOGE("CAN: MALFORMED USB RECV PACKET");
handle->comms_healthy = false;
return false;
}
int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i));
recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]);
if (size < sizeof(uint32_t)) {
return true;
}
int pos = 0;
while (pos < recv_buf.size()) {
uint32_t magic;
memcpy(&magic, &data[0], sizeof(uint32_t));
if (magic != CAN_TRANSACTION_MAGIC) {
LOGE("CAN: MALFORMED CAN RECV PACKET");
handle->comms_healthy = false;
return false;
}
int pos = sizeof(uint32_t);
while (pos < size) {
can_header header;
memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE);
memcpy(&header, &data[pos], sizeof(can_header));
can_frame &canData = out_vec.emplace_back();
canData.busTime = 0;
@@ -262,9 +261,9 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &o
}
const uint8_t data_len = dlc_to_len[header.data_len_code];
canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len);
canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len);
pos += CANPACKET_HEAD_SIZE + data_len;
pos += sizeof(can_header) + data_len;
}
return true;
}

17
selfdrive/boardd/tests/test_boardd_usbprotocol.cc
View File

@@ -48,7 +48,7 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState
can.setAddress(i);
can.setSrc(random_int(0, 3) + bus_offset);
can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
total_pakets_size += CANPACKET_HEAD_SIZE + dat.size();
total_pakets_size += sizeof(can_header) + dat.size();
}
can_data_list = can_list.asReader();
@@ -58,14 +58,11 @@ PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState
void PandaTest::test_can_send() {
std::vector<uint8_t> unpacked_data;
this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
int size_left = size;
for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) {
REQUIRE(chunk[i] == counter);
uint32_t magic;
memcpy(&magic, &chunk[0], sizeof(uint32_t));
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;
}
REQUIRE(magic == CAN_TRANSACTION_MAGIC);
unpacked_data.insert(unpacked_data.end(), &chunk[sizeof(uint32_t)], &chunk[size]);
});
REQUIRE(unpacked_data.size() == total_pakets_size);
@@ -73,9 +70,9 @@ void PandaTest::test_can_send() {
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], CANPACKET_HEAD_SIZE);
memcpy(&header, &unpacked_data[pos], sizeof(can_header));
const uint8_t data_len = dlc_to_len[header.data_len_code];
pckt_len = CANPACKET_HEAD_SIZE + data_len;
pckt_len = sizeof(can_header) + data_len;
REQUIRE(header.addr == cnt);
REQUIRE(test_data.find(data_len) != test_data.end());