diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 91de75a364..bc6dcea457 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -588,17 +588,29 @@ void can_send(cereal::Event::Reader &event) { void *can_send_thread(void *crap) { LOGD("start send thread"); - SubMaster sm({"sendcan"}); - // drain sendcan to delete any stale messages from previous runs - sm.drain(); + Context * context = Context::create(); + SubSocket * subscriber = SubSocket::create(context, "sendcan"); + assert(subscriber != NULL); + // run as fast as messages come in while (!do_exit) { - if (sm.update(1000) > 0){ - can_send(sm["sendcan"]); + Message * msg = subscriber->receive(); + + if (msg){ + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + can_send(event); + delete msg; } } + delete subscriber; + delete context; + return NULL; }