diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index d4af8c6c2c..f2a4068b69 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -193,11 +193,10 @@ void usb_retry_connect() { } void can_recv(PubMaster &pm) { - // create message - MessageBuilder msg; - auto event = msg.initEvent(); - panda->can_receive(event); - pm.send("can", msg); + kj::Array can_data; + panda->can_receive(can_data); + auto bytes = can_data.asBytes(); + pm.send("can", bytes.begin(), bytes.size()); } void can_send_thread() { @@ -250,7 +249,7 @@ void can_recv_thread() { uint64_t next_frame_time = nanos_since_boot() + dt; while (!do_exit && panda->connected) { - can_recv(pm); + can_recv(pm); uint64_t cur_time = nanos_since_boot(); int64_t remaining = next_frame_time - cur_time; diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 4757005fd4..50065cda8e 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -7,6 +7,7 @@ #include "common/swaglog.h" #include "common/gpio.h" #include "common/util.h" +#include "messaging.hpp" #include "panda.h" #ifdef QCOM2 @@ -324,19 +325,20 @@ void Panda::can_send(capnp::List::Reader can_data_list){ delete[] send; } -int Panda::can_receive(cereal::Event::Builder &event){ +int Panda::can_receive(kj::Array& out_buf) { uint32_t data[RECV_SIZE/4]; int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE); // Not sure if this can happen if (recv < 0) recv = 0; - + if (recv == RECV_SIZE) { LOGW("Receive buffer full"); } size_t num_msg = recv / 0x10; - auto canData = event.initCan(num_msg); + MessageBuilder msg; + auto canData = msg.initEvent().initCan(num_msg); // populate message for (int i = 0; i < num_msg; i++) { @@ -353,6 +355,6 @@ int Panda::can_receive(cereal::Event::Builder &event){ canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len)); canData[i].setSrc((data[i*4+1] >> 4) & 0xff); } - + out_buf = capnp::messageToFlatArray(msg); return recv; } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 2260c07f27..fa1b9695a6 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -78,6 +78,5 @@ class Panda { void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode); void send_heartbeat(); void can_send(capnp::List::Reader can_data_list); - int can_receive(cereal::Event::Builder &event); - + int can_receive(kj::Array& out_buf); };