modeld: sync based on frame timestamp (#23901)
* sync based on frame ts * log extra frame id * ints only * update ref * no rounding errorshb-stopping
parent
37a8b09cad
commit
492398826f
2
cereal
2
cereal
|
@ -1 +1 @@
|
||||||
Subproject commit 80a5b91e34ecac3e0d504c35f76dd25a34467672
|
Subproject commit 5c466ec8a4fa9cb688a935b16721b89d7bea31ab
|
|
@ -51,6 +51,11 @@ mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera
|
||||||
return matmul3(yuv_transform, transform);
|
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) {
|
void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra_client) {
|
||||||
// messaging
|
// messaging
|
||||||
PubMaster pm({"modelV2", "cameraOdometry"});
|
PubMaster pm({"modelV2", "cameraOdometry"});
|
||||||
|
@ -74,18 +79,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
|
||||||
VisionIpcBufExtra meta_extra = {0};
|
VisionIpcBufExtra meta_extra = {0};
|
||||||
|
|
||||||
while (!do_exit) {
|
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
|
// 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);
|
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;
|
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
|
// Keep receiving extra frames until frame id matches main camera
|
||||||
do {
|
do {
|
||||||
buf_extra = vipc_client_extra.recv(&meta_extra);
|
buf_extra = vipc_client_extra.recv(&meta_extra);
|
||||||
|
} while (buf_extra != nullptr && get_ts(meta_main) > get_ts(meta_extra) + 25000000ULL);
|
||||||
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);
|
|
||||||
|
|
||||||
if (buf_extra == nullptr) {
|
if (buf_extra == nullptr) {
|
||||||
LOGE("vipc_client_extra no frame");
|
LOGE("vipc_client_extra no frame");
|
||||||
continue;
|
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)",
|
LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
|
||||||
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
|
meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
|
||||||
meta_extra.frame_id, double(meta_extra.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);
|
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);
|
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);
|
posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
|
||||||
|
|
||||||
|
|
|
@ -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,
|
const ModelOutput &net_outputs, uint64_t timestamp_eof,
|
||||||
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid) {
|
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;
|
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
|
||||||
MessageBuilder msg;
|
MessageBuilder msg;
|
||||||
auto framed = msg.initEvent(valid).initModelV2();
|
auto framed = msg.initEvent(valid).initModelV2();
|
||||||
framed.setFrameId(vipc_frame_id);
|
framed.setFrameId(vipc_frame_id);
|
||||||
|
framed.setFrameIdExtra(vipc_frame_id_extra);
|
||||||
framed.setFrameAge(frame_age);
|
framed.setFrameAge(frame_age);
|
||||||
framed.setFrameDropPerc(frame_drop * 100);
|
framed.setFrameDropPerc(frame_drop * 100);
|
||||||
framed.setTimestampEof(timestamp_eof);
|
framed.setTimestampEof(timestamp_eof);
|
||||||
|
|
|
@ -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,
|
ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide,
|
||||||
const mat3 &transform, const mat3 &transform_wide, float *desire_in);
|
const mat3 &transform, const mat3 &transform_wide, float *desire_in);
|
||||||
void model_free(ModelState* s);
|
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,
|
const ModelOutput &net_outputs, uint64_t timestamp_eof,
|
||||||
float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool valid);
|
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,
|
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
|
||||||
|
|
|
@ -1 +1 @@
|
||||||
15821a7f867f6b497a17e8a36c9d42ad548acacd
|
710313545a2b5e0899c6ee0b99ecd9d322cfecb7
|
||||||
|
|
Loading…
Reference in New Issue