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
Split 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/clutil.[c,h]
selfdrive/common/messaging.h
selfdrive/common/messagehelp.h
selfdrive/common/params.h
selfdrive/common/params.cc
selfdrive/common/mutex.h

29
selfdrive/boardd/boardd.cc
Unescape View File

@ -26,6 +26,7 @@
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
#include <algorithm>
@ -548,16 +549,12 @@ void can_send(SubSocket *subscriber) {
int err;
// 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);
memcpy(amsg.begin(), msg->getData(), msg->getSize());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
auto event = amsg.getEvent();
if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
//Older than 1 second. Dont send.
delete msg;
return;
}
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);
memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size());
}
// release msg
delete msg;
// send to board
int sent;
pthread_mutex_lock(&usb_lock);
if (!fake_send) {
do {
// 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) {
cnt++;
for (auto sock : poller->poll(1000)){
Message * msg = sock->receive();
if (msg == NULL) 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>();
MessageReader amsg = sock->receive();
if (!amsg) continue;
auto event = amsg.getEvent();
auto type = event.which();
if(type == cereal::Event::THERMAL){
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/timing.h"
#include "common/swaglog.h"
#include "common/messagehelp.h"
#include "buffering.h"
extern "C" {
@ -61,15 +62,10 @@ void run_frame_stream(DualCameraState *s) {
auto *tb = &rear_camera->camera_tb;
while (!do_exit) {
Message * msg = recorder_sock->receive();
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();
MessageReader amsg = recorder_sock->receive();
if (!amsg) continue;
auto frame = amsg.getEvent().getFrame();
const int buf_idx = tbuffer_select(tb);
rear_camera->camera_bufs_metadata[buf_idx] = {
.frame_id = frame.getFrameId(),
@ -94,8 +90,6 @@ void run_frame_stream(DualCameraState *s) {
clWaitForEvents(1, &map_event);
clReleaseEvent(map_event);
tbuffer_dispatch(tb, buf_idx);
delete msg;
}
delete recorder_sock;
delete context;

37
selfdrive/camerad/main.cc
Unescape View File

@ -18,6 +18,7 @@
#include "common/visionipc.h"
#include "common/visionbuf.h"
#include "common/visionimg.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
@ -202,33 +203,21 @@ void* frontview_thread(void *arg) {
// no more check after gps check
if (!s->rhd_front_checked) {
Message *msg_dmon = dmonstate_sock->receive(true);
if (msg_dmon != NULL) {
auto amsg = kj::heapArray<capnp::word>((msg_dmon->getSize() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), msg_dmon->getData(), msg_dmon->getSize());
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;
MessageReader amsg = dmonstate_sock->receive(true);
if (amsg) {
auto state = amsg.getEvent().getDMonitoringState();
s->rhd_front = state.getIsRHD();
s->rhd_front_checked = state.getRhdChecked();
}
}
Message *msg = monitoring_sock->receive(true);
if (msg != NULL) {
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>();
float face_prob = event.getDriverState().getFaceProb();
MessageReader amsg = monitoring_sock->receive(true);
if (amsg) {
auto state = amsg.getEvent().getDriverState();
float face_prob = state.getFaceProb();
float face_position[2];
face_position[0] = event.getDriverState().getFacePosition()[0];
face_position[1] = event.getDriverState().getFacePosition()[1];
face_position[0] = state.getFacePosition()[0];
face_position[1] = state.getFacePosition()[1];
// set front camera metering target
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_xmax = s->rhd_front ? s->rgb_front_width * 2 / 5:s->rgb_front_width;
}
delete msg;
}
// 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/params.h"
#include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
#include "locationd_yawrate.h"
@ -105,14 +106,10 @@ int main(int argc, char *argv[]) {
int save_counter = 0;
while (true){
for (auto s : poller->poll(100)){
Message * msg = s->receive();
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>();
MessageReader amsg = s->receive();
if (!amsg) continue;
auto event = amsg.getEvent();
localizer.handle_log(event);
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/swaglog.h"
#include "common/timing.h"
#include "common/messagehelp.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) {
Message * msg = poll_func(poller);
if (msg == NULL) continue;
MessageReader amsg = poll_func(poller);
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>();
const uint8_t *data = event.getUbloxRaw().begin();
size_t len = event.getUbloxRaw().size();
auto ubloxRaw = amsg.getEvent().getUbloxRaw();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) {
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;
}
delete msg;
}
delete poller;

20
selfdrive/loggerd/loggerd.cc
Unescape View File

@ -36,6 +36,7 @@
#include "common/visionipc.h"
#include "common/utilpp.h"
#include "common/util.h"
#include "common/messagehelp.h"
#include "logger.h"
#include "messaging.hpp"
@ -654,21 +655,13 @@ int main(int argc, char** argv) {
while (!do_exit) {
for (auto sock : poller->poll(100 * 1000)){
while (true) {
Message * msg = sock->receive(true);
if (msg == NULL){
MessageReader amsg = sock->receive(true);
if (!amsg){
break;
}
uint8_t* data = (uint8_t*)msg->getData();
size_t len = msg->getSize();
if (sock == frame_sock) {
auto event = amsg.getEvent();
// 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()) {
std::unique_lock<std::mutex> lk(s.lock);
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);
delete msg;
logger_log(&s.logger, (uint8_t*)amsg.getData(), amsg.getSize(), qlog_counter[sock] == 0);
if (qlog_counter[sock] != -1) {
//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];
}
bytes_count += len;
bytes_count += amsg.getSize();
msg_count++;
}
}

17
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -7,6 +7,7 @@
#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "common/messagehelp.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);
if (!dmonitoringmodel.is_rhd_checked) {
if (chk_counter >= RHD_CHECK_INTERVAL) {
Message *msg = dmonstate_sock->receive(true);
if (msg != NULL) {
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>();
dmonitoringmodel.is_rhd = event.getDMonitoringState().getIsRHD();
dmonitoringmodel.is_rhd_checked = event.getDMonitoringState().getRhdChecked();
delete msg;
MessageReader amsg = dmonstate_sock->receive(true);
if (amsg) {
auto state = amsg.getEvent().getDMonitoringState();
dmonitoringmodel.is_rhd = state.getIsRHD();
dmonitoringmodel.is_rhd_checked = state.getRhdChecked();
}
chk_counter = 0;
}

27
selfdrive/modeld/modeld.cc
Unescape View File

@ -4,6 +4,7 @@
#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "common/messagehelp.h"
#include "models/driving.h"
@ -48,14 +49,10 @@ void* live_thread(void *arg) {
while (!do_exit) {
for (auto sock : poller->poll(10)){
Message * msg = sock->receive();
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>();
MessageReader amsg = sock->receive();
if (!amsg) continue;
auto event = amsg.getEvent();
if (event.isLiveCalibration()) {
pthread_mutex_lock(&transform_lock);
@ -80,8 +77,6 @@ void* live_thread(void *arg) {
run_model = true;
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;
pthread_mutex_unlock(&transform_lock);
Message *msg = pathplan_sock->receive(true);
if (msg != NULL) {
// 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>();
MessageReader amsg = pathplan_sock->receive(true);
if (amsg) {
// TODO: path planner timeout?
desire = ((int)event.getPathPlan().getDesire()) - 1;
delete msg;
desire = ((int)amsg.getEvent().getPathPlan().getDesire()) - 1;
}
double mt1 = 0, mt2 = 0;

Write Preview
Loading…
Cancel
Save