diff --git a/cereal b/cereal index 80a5b91e3..5c466ec8a 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 80a5b91e34ecac3e0d504c35f76dd25a34467672 +Subproject commit 5c466ec8a4fa9cb688a935b16721b89d7bea31ab diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 7f51b9a54..271437010 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -51,6 +51,11 @@ mat3 update_calibration(Eigen::Matrix &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(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); diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 83e69cebb..18ab82223 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -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 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); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index c78bfd734..f83cb8693 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -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 raw_pred, const bool valid); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 850b8561a..a875d709c 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -15821a7f867f6b497a17e8a36c9d42ad548acacd +710313545a2b5e0899c6ee0b99ecd9d322cfecb7