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

panda.cc: struct for CAN header, counter complexity, cleanup for readability (#22956)

Browse Source 
* try struct

* can_send refactor

* cleanups

* Resize vector only when it is needed

* ...

* more cleanup
This commit is contained in:
Igor Biletskyy
2021-11-17 16:00:18 -08:00
committed by GitHub
parent 7d18e26ff1
commit 47d0d717eb
2 changed files with 45 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/boardd/panda.cc
+31 -23
View File
@@ -364,12 +364,14 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
}
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
send.resize(72 * can_data_list.size()); // TODO: need to include 1 byte for each usb 64bytes frame
if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
}
int msg_count = 0;
while (msg_count < can_data_list.size()) {
uint32_t pos = 0;
while (pos < 256) {
while (pos < USB_TX_SOFT_LIMIT) {
if (msg_count == can_data_list.size()) { break; }
auto cmsg = can_data_list[msg_count];
@@ -384,26 +386,31 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8);
assert(can_data.size() == dlc_to_len[data_len_code]);
*(uint32_t*)&send[pos+1] = (cmsg.getAddress() << 3);
if (cmsg.getAddress() >= 0x800) {
*(uint32_t*)&send[pos+1] |= (1 << 2);
}
send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1);
memcpy(&send[pos+5], can_data.begin(), can_data.size());
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++;
}
if (pos > 0) { // Helps not to spam with ZLP
// insert counter
// Counter needs to be inserted every 64 bytes (first byte of 64 bytes USB packet)
uint8_t counter = 0;
for (int i = 0; i < pos; i += 64) {
send.insert(send.begin() + i, counter);
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++;
pos++;
}
usb_bulk_write(3, (uint8_t*)send.data(), pos, 5);
usb_bulk_write(3, to_write, ptr, 5);
}
}
}
@@ -415,7 +422,6 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
// Not sure if this can happen
if (recv < 0) recv = 0;
// TODO: Might change from full to overloaded? if > some threshold that is lower than RECV_SIZE, let's say 80-90%
if (recv == RECV_SIZE) {
LOGW("Receive buffer full");
}
@@ -426,17 +432,18 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE));
static uint8_t tail[72];
static uint8_t tail[CANPACKET_MAX_SIZE];
uint8_t tail_size = 0;
uint8_t counter = 0;
for (int i = 0; i < recv; i += 64) {
// Check for counter every 64 bytes (length of USB packet)
if (counter != data[i]) {
LOGE("CAN: MALFORMED USB RECV PACKET");
break;
}
counter++;
uint8_t chunk_len = ((recv - i) > 64) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
uint8_t chunk[72];
uint8_t chunk[CANPACKET_MAX_SIZE];
memcpy(chunk, tail, tail_size);
memcpy(&chunk[tail_size], &data[i+1], chunk_len);
chunk_len += tail_size;
@@ -447,21 +454,22 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
if (pckt_len <= (chunk_len - pos)) {
can_frame canData;
can_header header;
memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
canData.busTime = 0;
canData.address = (*(uint32_t*)&chunk[pos+1]) >> 3;
canData.src = ((chunk[pos] >> 1) & 0x7) + bus_offset;
canData.address = header.addr;
canData.src = header.bus + bus_offset;
bool rejected = chunk[pos+1] & 0x1;
bool returned = (chunk[pos+1] >> 1) & 0x1;
if (rejected) { canData.src += CANPACKET_REJECTED; }
if (returned) { canData.src += CANPACKET_RETURNED; }
canData.dat.assign((char*)&chunk[pos+5], data_len);
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);
pos += pckt_len;
// add to vector
out_vec.push_back(canData);
} else {
// Keep partial CAN packet until next USB packet
tail_size = (chunk_len - pos);
memcpy(tail, &chunk[pos], tail_size);
break;
selfdrive/boardd/panda.h
+14 -3
View File
@@ -13,11 +13,12 @@
#include "cereal/gen/cpp/car.capnp.h"
#include "cereal/gen/cpp/log.capnp.h"
// double the FIFO size
#define RECV_SIZE (0x4000)
#define TIMEOUT 0
#define PANDA_BUS_CNT 4
#define CANPACKET_HEAD_SIZE (0x5U)
#define RECV_SIZE (0x4000U)
#define USB_TX_SOFT_LIMIT (0x100U)
#define CANPACKET_HEAD_SIZE 5U
#define CANPACKET_MAX_SIZE 72U
#define CANPACKET_REJECTED (0xC0U)
#define CANPACKET_RETURNED (0x80U)
@@ -44,6 +45,16 @@ struct __attribute__((packed)) health_t {
uint8_t heartbeat_lost;
};
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;
};
struct can_frame {
long address;
std::string dat;