spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

modeld: sync based on frame timestamp (#23901) 

* sync based on frame ts

* log extra frame id

* ints only

* update ref

* no rounding errors
hb-stopping
Willem Melching 2022-03-07 10:46:15 +01:00 committed by GitHub
parent 37a8b09cad
commit 492398826f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 14 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

@ -1 +1 @@
Subproject commit 80a5b91e34ecac3e0d504c35f76dd25a34467672
Subproject commit 5c466ec8a4fa9cb688a935b16721b89d7bea31ab

28
selfdrive/modeld/modeld.cc
View File

@ -51,6 +51,11 @@ mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera
return matmul3(yuv_transform, transform);
}
static uint64_t get_ts(const VisionIpcBufExtra &extra) {
return Hardware::TICI() ? extra.timestamp_sof : extra.timestamp_eof;
}
void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
@ -74,18 +79,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
VisionIpcBufExtra meta_extra = {0};
while (!do_exit) {
// TODO: change sync logic to use timestamp start of frame in case camerad skips a frame
// log frame id in model packet
// Keep receiving frames until we are at least 1 frame ahead of previous extra frame
while (meta_main.frame_id <= meta_extra.frame_id) {
while (get_ts(meta_main) < get_ts(meta_extra) + 25000000ULL) {
buf_main = vipc_client_main.recv(&meta_main);
if (meta_main.frame_id <= meta_extra.frame_id) {
LOGE("main camera behind! main: %d (%.5f), extra: %d (%.5f)",
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
}
if (buf_main == nullptr) break;
}
@ -98,20 +94,14 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
// Keep receiving extra frames until frame id matches main camera
do {
buf_extra = vipc_client_extra.recv(&meta_extra);
if (meta_main.frame_id > meta_extra.frame_id) {
LOGE("extra camera behind! main: %d (%.5f), extra: %d (%.5f)",
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
}
} while (buf_extra != nullptr && meta_main.frame_id > meta_extra.frame_id);
} while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
if (buf_extra == nullptr) {
LOGE("vipc_client_extra no frame");
continue;
}
if (meta_main.frame_id != meta_extra.frame_id || std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
if (std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
@ -159,7 +149,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, meta_main.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
model_publish(pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);

3
selfdrive/modeld/models/driving.cc
View File

@ -314,13 +314,14 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
}
}
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg;
auto framed = msg.initEvent(valid).initModelV2();
framed.setFrameId(vipc_frame_id);
framed.setFrameIdExtra(vipc_frame_id_extra);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);

2
selfdrive/modeld/models/driving.h
View File

@ -270,7 +270,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
const mat3 &transform, const mat3 &transform_wide, float *desire_in);
void model_free(ModelState* s);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,

2
selfdrive/test/process_replay/model_replay_ref_commit
View File

@ -1 +1 @@
15821a7f867f6b497a17e8a36c9d42ad548acacd
710313545a2b5e0899c6ee0b99ecd9d322cfecb7