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

Bypass alignment related copy whenever possible (#1443)

Browse Source 
remove blank lines

add file messagehelp.h

remove blank line

simplify

after review
old-commit-hash: b225016628
commatwo_master
Dean Lee 5 years ago committed by GitHub
parent 5da84ba511
commit f2932aadfd
10 changed files with 95 additions and 121 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      release/files_common
  2. 29
      selfdrive/boardd/boardd.cc
  3. 14
      selfdrive/camerad/cameras/camera_frame_stream.cc
  4. 37
      selfdrive/camerad/main.cc
  5. 42
      selfdrive/common/messagehelp.h
  6. 12
      selfdrive/locationd/paramsd.cc
  7. 17
      selfdrive/locationd/ubloxd_main.cc
  8. 20
      selfdrive/loggerd/loggerd.cc
  9. 17
      selfdrive/modeld/dmonitoringmodeld.cc
  10. 27
      selfdrive/modeld/modeld.cc

1
release/files_common
Unescape View File

@ -173,6 +173,7 @@ selfdrive/common/efd.[c,h]
selfdrive/common/cqueue.[c,h] selfdrive/common/cqueue.[c,h]
selfdrive/common/clutil.[c,h] selfdrive/common/clutil.[c,h]
selfdrive/common/messaging.h selfdrive/common/messaging.h
selfdrive/common/messagehelp.h
selfdrive/common/params.h selfdrive/common/params.h
selfdrive/common/params.cc selfdrive/common/params.cc
selfdrive/common/mutex.h selfdrive/common/mutex.h

29
selfdrive/boardd/boardd.cc
Unescape View File

@ -26,6 +26,7 @@
#include "common/params.h" #include "common/params.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/timing.h" #include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp" #include "messaging.hpp"
#include <algorithm> #include <algorithm>
@ -548,16 +549,12 @@ void can_send(SubSocket *subscriber) {
int err; int err;
// recv from sendcan // recv from sendcan
Message * msg = subscriber->receive(); MessageReader amsg = subscriber->receive();
if (!amsg) return;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); auto event = amsg.getEvent();
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
if (nanos_since_boot() - event.getLogMonoTime() > 1e9) { if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
//Older than 1 second. Dont send. //Older than 1 second. Dont send.
delete msg;
return; return;
} }
int msg_count = event.getSendcan().size(); int msg_count = event.getSendcan().size();
@ -578,15 +575,10 @@ void can_send(SubSocket *subscriber) {
send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4); send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4);
memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size()); memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size());
} }
// release msg
delete msg;
// send to board // send to board
int sent; int sent;
pthread_mutex_lock(&usb_lock); pthread_mutex_lock(&usb_lock);
if (!fake_send) { if (!fake_send) {
do { do {
// Try sending can messages. If the receive buffer on the panda is full it will NAK // Try sending can messages. If the receive buffer on the panda is full it will NAK
@ -716,17 +708,10 @@ void *hardware_control_thread(void *crap) {
while (!do_exit) { while (!do_exit) {
cnt++; cnt++;
for (auto sock : poller->poll(1000)){ for (auto sock : poller->poll(1000)){
Message * msg = sock->receive(); MessageReader amsg = sock->receive();
if (msg == NULL) continue; if (!amsg) continue;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
delete msg;
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto event = amsg.getEvent();
auto type = event.which(); auto type = event.which();
if(type == cereal::Event::THERMAL){ if(type == cereal::Event::THERMAL){
uint16_t fan_speed = event.getThermal().getFanSpeed(); uint16_t fan_speed = event.getThermal().getFanSpeed();

14
selfdrive/camerad/cameras/camera_frame_stream.cc
Unescape View File

@ -15,6 +15,7 @@
#include "common/util.h" #include "common/util.h"
#include "common/timing.h" #include "common/timing.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/messagehelp.h"
#include "buffering.h" #include "buffering.h"
extern "C" { extern "C" {
@ -61,15 +62,10 @@ void run_frame_stream(DualCameraState *s) {
auto *tb = &rear_camera->camera_tb; auto *tb = &rear_camera->camera_tb;
while (!do_exit) { while (!do_exit) {
Message * msg = recorder_sock->receive(); MessageReader amsg = recorder_sock->receive();
if (!amsg) continue;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto frame = event.getFrame();
auto frame = amsg.getEvent().getFrame();
const int buf_idx = tbuffer_select(tb); const int buf_idx = tbuffer_select(tb);
rear_camera->camera_bufs_metadata[buf_idx] = { rear_camera->camera_bufs_metadata[buf_idx] = {
.frame_id = frame.getFrameId(), .frame_id = frame.getFrameId(),
@ -94,8 +90,6 @@ void run_frame_stream(DualCameraState *s) {
clWaitForEvents(1, &map_event); clWaitForEvents(1, &map_event);
clReleaseEvent(map_event); clReleaseEvent(map_event);
tbuffer_dispatch(tb, buf_idx); tbuffer_dispatch(tb, buf_idx);
delete msg;
} }
delete recorder_sock; delete recorder_sock;
delete context; delete context;

37
selfdrive/camerad/main.cc
Unescape View File

@ -18,6 +18,7 @@
#include "common/visionipc.h" #include "common/visionipc.h"
#include "common/visionbuf.h" #include "common/visionbuf.h"
#include "common/visionimg.h" #include "common/visionimg.h"
#include "common/messagehelp.h"
#include "messaging.hpp" #include "messaging.hpp"
@ -202,33 +203,21 @@ void* frontview_thread(void *arg) {
// no more check after gps check // no more check after gps check
if (!s->rhd_front_checked) { if (!s->rhd_front_checked) {
Message *msg_dmon = dmonstate_sock->receive(true); MessageReader amsg = dmonstate_sock->receive(true);
if (msg_dmon != NULL) { if (amsg) {
auto amsg = kj::heapArray<capnp::word>((msg_dmon->getSize() / sizeof(capnp::word)) + 1); auto state = amsg.getEvent().getDMonitoringState();
memcpy(amsg.begin(), msg_dmon->getData(), msg_dmon->getSize()); s->rhd_front = state.getIsRHD();
s->rhd_front_checked = state.getRhdChecked();
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
s->rhd_front = event.getDMonitoringState().getIsRHD();
s->rhd_front_checked = event.getDMonitoringState().getRhdChecked();
delete msg_dmon;
} }
} }
Message *msg = monitoring_sock->receive(true); MessageReader amsg = monitoring_sock->receive(true);
if (msg != NULL) { if (amsg) {
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); auto state = amsg.getEvent().getDriverState();
memcpy(amsg.begin(), msg->getData(), msg->getSize()); float face_prob = state.getFaceProb();
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
float face_prob = event.getDriverState().getFaceProb();
float face_position[2]; float face_position[2];
face_position[0] = event.getDriverState().getFacePosition()[0]; face_position[0] = state.getFacePosition()[0];
face_position[1] = event.getDriverState().getFacePosition()[1]; face_position[1] = state.getFacePosition()[1];
// set front camera metering target // set front camera metering target
if (face_prob > 0.4) if (face_prob > 0.4)
@ -246,8 +235,6 @@ void* frontview_thread(void *arg) {
s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5; s->front_meteringbox_xmin = s->rhd_front ? 0:s->rgb_front_width * 3 / 5;
s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width; s->front_meteringbox_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
} }
delete msg;
} }
// auto exposure // auto exposure

42
selfdrive/common/messagehelp.h
Unescape View File

@ -0,0 +1,42 @@
#pragma once
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "messaging.hpp"
class MessageReader {
public:
MessageReader(Message *msg) : msg(msg), buf(nullptr), msg_reader(nullptr) {}
~MessageReader() {
if (msg_reader) delete msg_reader;
if (buf) free(buf);
if (msg) delete msg;
}
inline operator bool() { return msg != NULL; }
inline const char* getData() { return msg->getData(); }
inline size_t getSize() { return msg->getSize(); }
cereal::Event::Reader &getEvent() {
if (!msg_reader) {
msg_reader = newReader();
event = msg_reader->getRoot<cereal::Event>();
}
return event;
}
private:
capnp::FlatArrayMessageReader *newReader() {
const char *data = msg->getData();
const size_t size = msg->getSize();
if (((reinterpret_cast<uintptr_t>(data)) % sizeof(capnp::word) == 0) && size % sizeof(capnp::word) == 0) {
return new capnp::FlatArrayMessageReader(kj::ArrayPtr<capnp::word>((capnp::word *)data, size / sizeof(capnp::word)));
} else {
const size_t words = size / sizeof(capnp::word) + 1;
buf = (capnp::word *)malloc(words * sizeof(capnp::word));
memcpy(buf, data, size);
return new capnp::FlatArrayMessageReader(kj::ArrayPtr<capnp::word>(buf, words));
}
}
capnp::word *buf;
Message *msg;
capnp::FlatArrayMessageReader *msg_reader;
cereal::Event::Reader event;
};

12
selfdrive/locationd/paramsd.cc
Unescape View File

@ -15,6 +15,7 @@
#include "common/messaging.h" #include "common/messaging.h"
#include "common/params.h" #include "common/params.h"
#include "common/timing.h" #include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp" #include "messaging.hpp"
#include "locationd_yawrate.h" #include "locationd_yawrate.h"
@ -105,14 +106,10 @@ int main(int argc, char *argv[]) {
int save_counter = 0; int save_counter = 0;
while (true){ while (true){
for (auto s : poller->poll(100)){ for (auto s : poller->poll(100)){
Message * msg = s->receive(); MessageReader amsg = s->receive();
if (!amsg) continue;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader capnp_msg(amsg);
cereal::Event::Reader event = capnp_msg.getRoot<cereal::Event>();
auto event = amsg.getEvent();
localizer.handle_log(event); localizer.handle_log(event);
auto which = event.which(); auto which = event.which();
@ -172,7 +169,6 @@ int main(int argc, char *argv[]) {
}); });
} }
} }
delete msg;
} }
} }

17
selfdrive/locationd/ubloxd_main.cc
Unescape View File

@ -23,6 +23,7 @@
#include "common/params.h" #include "common/params.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/timing.h" #include "common/timing.h"
#include "common/messagehelp.h"
#include "ublox_msg.h" #include "ublox_msg.h"
@ -54,17 +55,12 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
while (!do_exit) { while (!do_exit) {
Message * msg = poll_func(poller); MessageReader amsg = poll_func(poller);
if (msg == NULL) continue; if (!amsg) continue;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); auto ubloxRaw = amsg.getEvent().getUbloxRaw();
memcpy(amsg.begin(), msg->getData(), msg->getSize()); const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
const uint8_t *data = event.getUbloxRaw().begin();
size_t len = event.getUbloxRaw().size();
size_t bytes_consumed = 0; size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) { while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U; size_t bytes_consumed_this_time = 0U;
@ -114,7 +110,6 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
} }
bytes_consumed += bytes_consumed_this_time; bytes_consumed += bytes_consumed_this_time;
} }
delete msg;
} }
delete poller; delete poller;

20
selfdrive/loggerd/loggerd.cc
Unescape View File

@ -36,6 +36,7 @@
#include "common/visionipc.h" #include "common/visionipc.h"
#include "common/utilpp.h" #include "common/utilpp.h"
#include "common/util.h" #include "common/util.h"
#include "common/messagehelp.h"
#include "logger.h" #include "logger.h"
#include "messaging.hpp" #include "messaging.hpp"
@ -654,21 +655,13 @@ int main(int argc, char** argv) {
while (!do_exit) { while (!do_exit) {
for (auto sock : poller->poll(100 * 1000)){ for (auto sock : poller->poll(100 * 1000)){
while (true) { while (true) {
Message * msg = sock->receive(true); MessageReader amsg = sock->receive(true);
if (msg == NULL){ if (!amsg){
break; break;
} }
uint8_t* data = (uint8_t*)msg->getData();
size_t len = msg->getSize();
if (sock == frame_sock) { if (sock == frame_sock) {
auto event = amsg.getEvent();
// track camera frames to sync to encoder // track camera frames to sync to encoder
auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), data, len);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
if (event.isFrame()) { if (event.isFrame()) {
std::unique_lock<std::mutex> lk(s.lock); std::unique_lock<std::mutex> lk(s.lock);
s.last_frame_id = event.getFrame().getFrameId(); s.last_frame_id = event.getFrame().getFrameId();
@ -677,8 +670,7 @@ int main(int argc, char** argv) {
} }
} }
logger_log(&s.logger, data, len, qlog_counter[sock] == 0); logger_log(&s.logger, (uint8_t*)amsg.getData(), amsg.getSize(), qlog_counter[sock] == 0);
delete msg;
if (qlog_counter[sock] != -1) { if (qlog_counter[sock] != -1) {
//printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]); //printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]);
@ -686,7 +678,7 @@ int main(int argc, char** argv) {
qlog_counter[sock] %= qlog_freqs[sock]; qlog_counter[sock] %= qlog_freqs[sock];
} }
bytes_count += len; bytes_count += amsg.getSize();
msg_count++; msg_count++;
} }
} }

17
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -7,6 +7,7 @@
#include "common/visionbuf.h" #include "common/visionbuf.h"
#include "common/visionipc.h" #include "common/visionipc.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/messagehelp.h"
#include "models/dmonitoring.h" #include "models/dmonitoring.h"
@ -61,17 +62,11 @@ int main(int argc, char **argv) {
//printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height); //printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
if (!dmonitoringmodel.is_rhd_checked) { if (!dmonitoringmodel.is_rhd_checked) {
if (chk_counter >= RHD_CHECK_INTERVAL) { if (chk_counter >= RHD_CHECK_INTERVAL) {
Message *msg = dmonstate_sock->receive(true); MessageReader amsg = dmonstate_sock->receive(true);
if (msg != NULL) { if (amsg) {
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1); auto state = amsg.getEvent().getDMonitoringState();
memcpy(amsg.begin(), msg->getData(), msg->getSize()); dmonitoringmodel.is_rhd = state.getIsRHD();
dmonitoringmodel.is_rhd_checked = state.getRhdChecked();
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD();
dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked();
delete msg;
} }
chk_counter = 0; chk_counter = 0;
} }

27
selfdrive/modeld/modeld.cc
Unescape View File

@ -4,6 +4,7 @@
#include "common/visionbuf.h" #include "common/visionbuf.h"
#include "common/visionipc.h" #include "common/visionipc.h"
#include "common/swaglog.h" #include "common/swaglog.h"
#include "common/messagehelp.h"
#include "models/driving.h" #include "models/driving.h"
@ -48,14 +49,10 @@ void* live_thread(void *arg) {
while (!do_exit) { while (!do_exit) {
for (auto sock : poller->poll(10)){ for (auto sock : poller->poll(10)){
Message * msg = sock->receive(); MessageReader amsg = sock->receive();
if (!amsg) continue;
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto event = amsg.getEvent();
if (event.isLiveCalibration()) { if (event.isLiveCalibration()) {
pthread_mutex_lock(&transform_lock); pthread_mutex_lock(&transform_lock);
@ -80,8 +77,6 @@ void* live_thread(void *arg) {
run_model = true; run_model = true;
pthread_mutex_unlock(&transform_lock); pthread_mutex_unlock(&transform_lock);
} }
delete msg;
} }
} }
@ -193,18 +188,10 @@ int main(int argc, char **argv) {
const bool run_model_this_iter = run_model; const bool run_model_this_iter = run_model;
pthread_mutex_unlock(&transform_lock); pthread_mutex_unlock(&transform_lock);
Message *msg = pathplan_sock->receive(true); MessageReader amsg = pathplan_sock->receive(true);
if (msg != NULL) { if (amsg) {
// TODO: copy and pasted from camerad/main.cc
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
// TODO: path planner timeout? // TODO: path planner timeout?
desire = ((int)event.getPathPlan().getDesire()) - 1; desire = ((int)amsg.getEvent().getPathPlan().getDesire()) - 1;
delete msg;
} }
double mt1 = 0, mt2 = 0; double mt1 = 0, mt2 = 0;

Write Preview
Loading…
Cancel
Save