diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 23aae757a8..691d3f32e8 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -82,7 +82,7 @@ int main(int argc, char **argv) { // messaging PubMaster pm({"model", "cameraOdometry"}); - SubMaster sm({"pathPlan"}); + SubMaster sm({"pathPlan", "frame"}); #ifdef QCOM cl_device_type device_type = CL_DEVICE_TYPE_DEFAULT; @@ -202,6 +202,7 @@ int main(int argc, char **argv) { } mat3 model_transform = matmul3(yuv_transform, transform); + uint32_t frame_id = sm["frame"].getFrame().getFrameId(); mt1 = millis_since_boot(); @@ -213,10 +214,10 @@ int main(int argc, char **argv) { model_transform, NULL, vec_desire); mt2 = millis_since_boot(); - model_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof); - posenet_publish(pm, extra.frame_id, model_buf, extra.timestamp_eof); + model_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof); + posenet_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof); - LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last); + LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu", mt2-mt1, mt1-last, extra.frame_id, frame_id); last = mt1; } diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b24762ebf0..88674fb604 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -245,7 +245,7 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float longi.setAccelerations(accel); } -void model_publish(PubMaster &pm, uint32_t frame_id, +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { // make msg capnp::MallocMessageBuilder msg; @@ -253,7 +253,7 @@ void model_publish(PubMaster &pm, uint32_t frame_id, event.setLogMonoTime(nanos_since_boot()); auto framed = event.initModel(); - framed.setFrameId(frame_id); + framed.setFrameId(vipc_frame_id); framed.setTimestampEof(timestamp_eof); auto lpath = framed.initPath(); @@ -290,11 +290,12 @@ void model_publish(PubMaster &pm, uint32_t frame_id, auto meta = framed.initMeta(); fill_meta(meta, net_outputs.meta); + event.setValid(frame_id - vipc_frame_id < MAX_FRAME_AGE); pm.send("model", msg); } -void posenet_publish(PubMaster &pm, uint32_t frame_id, +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { capnp::MallocMessageBuilder msg; cereal::Event::Builder event = msg.initRoot(); @@ -324,7 +325,8 @@ void posenet_publish(PubMaster &pm, uint32_t frame_id, posenetd.setRotStd(rot_std_vs); posenetd.setTimestampEof(timestamp_eof); - posenetd.setFrameId(frame_id); + posenetd.setFrameId(vipc_frame_id); + event.setValid(frame_id - vipc_frame_id < MAX_FRAME_AGE); pm.send("cameraOdometry", msg); } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 223a540b37..29d7a68f6a 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -35,6 +35,8 @@ #define TIME_DISTANCE 100 #define POSE_SIZE 12 +#define MAX_FRAME_AGE 5 + struct ModelDataRaw { float *path; float *left_lane; @@ -71,8 +73,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, void model_free(ModelState* s); void poly_fit(float *in_pts, float *in_stds, float *out); -void model_publish(PubMaster &pm, uint32_t frame_id, +void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &data, uint64_t timestamp_eof); -void posenet_publish(PubMaster &pm, uint32_t frame_id, +void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &data, uint64_t timestamp_eof); #endif