sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

New usb protocol for panda (#22752)

Browse Source 
* first try

* move dlc_to_len

* panda switch to mailbox branch

* add bitstruct to pipfile

* bump panda and remove bitstruct

* few cleanups and fixes

* sanity check data.size == dlc_to_len

* check problem with test

* Revert "check problem with test"

This reverts commit b4855b7c12188d36d5d510e729344dab2cf21be9.

* clean

* revert Pipfile.lock

* that was an issue???

* fix crash

* ...

* include dlc_to_len from panda

* alph

* define macros for returned and rejected

* cleanup

* bump panda to current master
pull/22895/head
Igor Biletskyy 4 years ago committed by GitHub
parent 534bf697ee
commit e293ccf016
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 101 additions and 49 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      panda
  2. 118
      selfdrive/boardd/panda.cc
  3. 8
      selfdrive/boardd/panda.h

2
panda

@ -1 +1 @@
Subproject commit 73bbf5856769a0b16c22a185635dafbe6a4aa152
Subproject commit ae26b75d7bac3d57b060f9fdc48b591fa23e0a25

118
selfdrive/boardd/panda.cc
Unescape View File

@ -7,6 +7,7 @@
#include <vector>
#include "cereal/messaging/messaging.h"
#include "panda/board/dlc_to_len.h"
#include "selfdrive/common/gpio.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/util.h"
@ -343,42 +344,70 @@ void Panda::send_heartbeat() {
usb_write(0xf3, 1, 0);
}
uint8_t Panda::len_to_dlc(uint8_t len) {
if (len <= 8) {
return len;
}
if (len <= 24) {
return 8 + ((len - 8) / 4) + (len % 4) ? 1 : 0;
} else {
return 11 + (len / 16) + (len % 16) ? 1 : 0;
}
}
void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
send.resize(4 * can_data_list.size());
send.resize(72 * can_data_list.size()); // TODO: need to include 1 byte for each usb 64bytes frame
uint32_t msg_cnt = 0;
for (int i = 0; i < can_data_list.size(); i++) {
auto cmsg = can_data_list[i];
int msg_count = 0;
while (msg_count < can_data_list.size()) {
uint32_t pos = 0;
while (pos < 256) {
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]);
if (cmsg.getAddress() >= 0x800) { // extended
send[msg_cnt*4] = (cmsg.getAddress() << 3) | 5;
} else { // normal
send[msg_cnt*4] = (cmsg.getAddress() << 21) | 1;
*(uint32_t*)&send[pos+1] = (cmsg.getAddress() << 3);
if (cmsg.getAddress() >= 0x800) {
*(uint32_t*)&send[pos+1] |= (1 << 2);
}
auto can_data = cmsg.getDat();
assert(can_data.size() <= 8);
send[msg_cnt*4+1] = can_data.size() | ((bus - bus_offset) << 4);
memcpy(&send[msg_cnt*4+2], can_data.begin(), can_data.size());
send[pos] = data_len_code << 4 | ((bus - bus_offset) << 1);
memcpy(&send[pos+5], can_data.begin(), can_data.size());
msg_cnt++;
pos += CANPACKET_HEAD_SIZE + dlc_to_len[data_len_code];
msg_count++;
}
usb_bulk_write(3, (unsigned char*)send.data(), msg_cnt * 0x10, 5);
if (pos > 0) { // Helps not to spam with ZLP
// insert counter
uint8_t counter = 0;
for (int i = 0; i < pos; i += 64) {
send.insert(send.begin() + i, counter);
counter++;
pos++;
}
usb_bulk_write(3, (uint8_t*)send.data(), pos, 5);
}
}
}
bool Panda::can_receive(std::vector<can_frame>& out_vec) {
uint32_t data[RECV_SIZE/4];
int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
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;
// 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");
}
@ -387,30 +416,49 @@ bool Panda::can_receive(std::vector<can_frame>& out_vec) {
return false;
}
// Append to the end of the out_vec, such that we can pass it to multiple pandas
// We already insert space for all the messages here for speed
size_t num_msg = recv / 0x10;
out_vec.reserve(out_vec.size() + num_msg);
out_vec.reserve(out_vec.size() + (recv / CANPACKET_HEAD_SIZE));
// Populate messages
for (int i = 0; i < num_msg; i++) {
can_frame canData;
if (data[i*4] & 4) {
// extended
canData.address = data[i*4] >> 3;
//printf("got extended: %x\n", data[i*4] >> 3);
} else {
// normal
canData.address = data[i*4] >> 21;
static uint8_t tail[72];
uint8_t tail_size = 0;
uint8_t counter = 0;
for (int i = 0; i < recv; i += 64) {
if (counter != data[i]) {
LOGE("CAN: MALFORMED USB RECV PACKET");
break;
}
canData.busTime = data[i*4+1] >> 16;
int len = data[i*4+1] & 0xF;
canData.dat.assign((char *)&data[i*4+2], len);
canData.src = ((data[i*4+1] >> 4) & 0xff) + bus_offset;
counter++;
uint8_t chunk_len = ((recv - i) > 64) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
uint8_t chunk[72];
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_frame canData;
canData.busTime = 0;
canData.address = (*(uint32_t*)&chunk[pos+1]) >> 3;
canData.src = (chunk[pos] >> 1) & 0x7;
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);
pos += pckt_len;
// add to vector
out_vec.push_back(canData);
} else {
tail_size = (chunk_len - pos);
memcpy(tail, &chunk[pos], tail_size);
break;
}
}
}
return true;
}

8
selfdrive/boardd/panda.h
Unescape View File

@ -14,9 +14,12 @@
#include "cereal/gen/cpp/log.capnp.h"
// double the FIFO size
#define RECV_SIZE (0x1000)
#define RECV_SIZE (0x4000)
#define TIMEOUT 0
#define PANDA_BUS_CNT 4
#define CANPACKET_HEAD_SIZE (0x5U)
#define CANPACKET_REJECTED (0xC0U)
#define CANPACKET_RETURNED (0x80U)
// copied from panda/board/main.c
struct __attribute__((packed)) health_t {
@ -53,7 +56,7 @@ class Panda {
libusb_context *ctx = NULL;
libusb_device_handle *dev_handle = NULL;
std::mutex usb_lock;
std::vector<uint32_t> send;
std::vector<uint8_t> send;
void handle_usb_issue(int err, const char func[]);
void cleanup();
@ -93,6 +96,7 @@ class Panda {
void set_power_saving(bool power_saving);
void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
void send_heartbeat();
uint8_t len_to_dlc(uint8_t len);
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
bool can_receive(std::vector<can_frame>& out_vec);
};

Write Preview
Loading…
Cancel
Save