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

Revert "Bypass alignment related copy whenever possible (#1443)"

Browse Source 
This reverts commit 235d4f8957ee4566d6217783638fa58a586bb35a.

old-commit-hash: d5ca841b0f
commatwo_master
George Hotz 5 years ago
parent 004c0e277b
commit 16cbb5e255
10 changed files with 121 additions and 95 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,7 +173,6 @@ 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

@ -25,7 +25,6 @@
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
#include <algorithm>
@ -548,12 +547,16 @@ void can_send(SubSocket *subscriber) {
int err;
// recv from sendcan
MessageReader amsg = subscriber->receive();
if (!amsg) return;
Message * msg = subscriber->receive();
auto event = amsg.getEvent();
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>();
if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
//Older than 1 second. Dont send.
delete msg;
return;
}
int msg_count = event.getSendcan().size();
@ -574,10 +577,15 @@ 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
@ -707,10 +715,17 @@ void *hardware_control_thread(void *crap) {
while (!do_exit) {
cnt++;
for (auto sock : poller->poll(1000)){
MessageReader amsg = sock->receive();
if (!amsg) continue;
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>();
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,7 +15,6 @@
#include "common/util.h"
#include "common/timing.h"
#include "common/swaglog.h"
#include "common/messagehelp.h"
#include "buffering.h"
extern "C" {
@ -62,10 +61,15 @@ void run_frame_stream(DualCameraState *s) {
auto *tb = &rear_camera->camera_tb;
while (!do_exit) {
MessageReader amsg = recorder_sock->receive();
if (!amsg) continue;
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();
auto frame = amsg.getEvent().getFrame();
const int buf_idx = tbuffer_select(tb);
rear_camera->camera_bufs_metadata[buf_idx] = {
.frame_id = frame.getFrameId(),
@ -90,6 +94,8 @@ 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,7 +18,6 @@
#include "common/visionipc.h"
#include "common/visionbuf.h"
#include "common/visionimg.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
@ -214,21 +213,33 @@ void* frontview_thread(void *arg) {
// no more check after gps check
if (!s->rhd_front_checked) {
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_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 = monitoring_sock->receive(true);
if (amsg) {
auto state = amsg.getEvent().getDriverState();
float face_prob = state.getFaceProb();
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();
float face_position[2];
face_position[0] = state.getFacePosition()[0];
face_position[1] = state.getFacePosition()[1];
face_position[0] = event.getDriverState().getFacePosition()[0];
face_position[1] = event.getDriverState().getFacePosition()[1];
// set front camera metering target
if (face_prob > 0.4)
@ -246,6 +257,8 @@ 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

@ -1,42 +0,0 @@
#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,7 +15,6 @@
#include "common/messaging.h"
#include "common/params.h"
#include "common/timing.h"
#include "common/messagehelp.h"
#include "messaging.hpp"
#include "locationd_yawrate.h"
@ -106,10 +105,14 @@ int main(int argc, char *argv[]) {
int save_counter = 0;
while (true){
for (auto s : poller->poll(100)){
MessageReader amsg = s->receive();
if (!amsg) continue;
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>();
auto event = amsg.getEvent();
localizer.handle_log(event);
auto which = event.which();
@ -169,6 +172,7 @@ int main(int argc, char *argv[]) {
});
}
}
delete msg;
}
}

17
selfdrive/locationd/ubloxd_main.cc
Unescape View File

@ -23,7 +23,6 @@
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/messagehelp.h"
#include "ublox_msg.h"
@ -55,12 +54,17 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
while (!do_exit) {
MessageReader amsg = poll_func(poller);
if (!amsg) continue;
Message * msg = poll_func(poller);
if (msg == NULL) continue;
auto ubloxRaw = amsg.getEvent().getUbloxRaw();
const uint8_t *data = ubloxRaw.begin();
size_t len = ubloxRaw.size();
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();
size_t bytes_consumed = 0;
while(bytes_consumed < len && !do_exit) {
size_t bytes_consumed_this_time = 0U;
@ -110,6 +114,7 @@ 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,7 +36,6 @@
#include "common/visionipc.h"
#include "common/utilpp.h"
#include "common/util.h"
#include "common/messagehelp.h"
#include "logger.h"
#include "messaging.hpp"
@ -655,13 +654,21 @@ int main(int argc, char** argv) {
while (!do_exit) {
for (auto sock : poller->poll(100 * 1000)){
while (true) {
MessageReader amsg = sock->receive(true);
if (!amsg){
Message * msg = sock->receive(true);
if (msg == NULL){
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();
@ -670,7 +677,8 @@ int main(int argc, char** argv) {
}
}
logger_log(&s.logger, (uint8_t*)amsg.getData(), amsg.getSize(), qlog_counter[sock] == 0);
logger_log(&s.logger, data, len, qlog_counter[sock] == 0);
delete msg;
if (qlog_counter[sock] != -1) {
//printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]);
@ -678,7 +686,7 @@ int main(int argc, char** argv) {
qlog_counter[sock] %= qlog_freqs[sock];
}
bytes_count += amsg.getSize();
bytes_count += len;
msg_count++;
}
}

17
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -7,7 +7,6 @@
#include "common/visionbuf.h"
#include "common/visionipc.h"
#include "common/swaglog.h"
#include "common/messagehelp.h"
#include "models/dmonitoring.h"
@ -62,11 +61,17 @@ 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) {
MessageReader amsg = dmonstate_sock->receive(true);
if (amsg) {
auto state = amsg.getEvent().getDMonitoringState();
dmonitoringmodel.is_rhd = state.getIsRHD();
dmonitoringmodel.is_rhd_checked = state.getRhdChecked();
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;
}
chk_counter = 0;
}

27
selfdrive/modeld/modeld.cc
Unescape View File

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

Write Preview
Loading…
Cancel
Save