diff --git a/cereal b/cereal index 089f96117a..c94c7c61cc 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 089f96117a513711755d34d7f919ac481f23a0fc +Subproject commit c94c7c61cc576e950a1604e1a3c9a91b1f86964c diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 2271ee5276..1c9c6e8ce7 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -60,7 +60,7 @@ mat3 update_calibration(Eigen::Vector3d device_from_calib_euler, bool wide_camer 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"}); - SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "mapRenderState", "navInstruction", "navModel"}); + SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration", "driverMonitoringState", "navModel"}); // setup filter to track dropped frames FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ); @@ -138,10 +138,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl } // Enable/disable nav features - double tsm = (float)(nanos_since_boot() - sm["mapRenderState"].getMapRenderState().getLocationMonoTime()) / 1e6; - bool route_valid = sm["navInstruction"].getValid() && (tsm < 1000); - bool render_valid = sm["mapRenderState"].getValid() && (sm["navModel"].getNavModel().getFrameId() == sm["mapRenderState"].getMapRenderState().getFrameId()); - bool nav_valid = route_valid && render_valid; + uint64_t timestamp_llk = sm["navModel"].getNavModel().getLocationMonoTime(); + bool nav_valid = sm["navModel"].getValid() && (nanos_since_boot() - timestamp_llk < 1e9); if (!nav_enabled && nav_valid) { nav_enabled = true; } else if (nav_enabled && !nav_valid) { @@ -178,7 +176,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl float model_execution_time = (mt2 - mt1) / 1000.0; if (model_output != nullptr) { - model_publish(&model, pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time, + model_publish(&model, pm, meta_main.frame_id, meta_extra.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, timestamp_llk, model_execution_time, nav_enabled, 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 01f8f7234c..dedbc8edac 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -408,7 +408,7 @@ void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const Model } void model_publish(ModelState* s, 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, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid) { const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0; MessageBuilder msg; @@ -418,6 +418,7 @@ void model_publish(ModelState* s, PubMaster &pm, uint32_t vipc_frame_id, uint32_ framed.setFrameAge(frame_age); framed.setFrameDropPerc(frame_drop * 100); framed.setTimestampEof(timestamp_eof); + framed.setLocationMonoTime(timestamp_llk); framed.setModelExecutionTime(model_execution_time); framed.setNavEnabled(nav_enabled); if (send_raw_pred) { diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index ae55e9c8a0..f664a4f397 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -285,7 +285,7 @@ ModelOutput *model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* buf_wide const mat3 &transform, const mat3 &transform_wide, float *desire_in, bool is_rhd, float *driving_style, float *nav_features, bool prepare_only); void model_free(ModelState* s); void model_publish(ModelState* s, 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, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, const ModelOutput &net_outputs, uint64_t timestamp_eof, const bool valid); diff --git a/selfdrive/modeld/models/nav.cc b/selfdrive/modeld/models/nav.cc index 48d354ae90..8208b0bfef 100644 --- a/selfdrive/modeld/models/nav.cc +++ b/selfdrive/modeld/models/nav.cc @@ -49,11 +49,14 @@ void fill_plan(cereal::NavModelData::Builder &framed, const NavModelOutputPlan & position.setYStd(to_kj_array_ptr(pos_y_std)); } -void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time) { +void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid) { // make msg MessageBuilder msg; - auto framed = msg.initEvent().initNavModel(); - framed.setFrameId(frame_id); + auto evt = msg.initEvent(); + auto framed = evt.initNavModel(); + evt.setValid(extra.valid && route_valid); + framed.setFrameId(extra.frame_id); + framed.setLocationMonoTime(extra.timestamp_sof); framed.setModelExecutionTime(execution_time); framed.setDspExecutionTime(model_res.dsp_execution_time); framed.setFeatures(to_kj_array_ptr(model_res.features.values)); diff --git a/selfdrive/modeld/models/nav.h b/selfdrive/modeld/models/nav.h index c9d49bb8a0..c6a517f558 100644 --- a/selfdrive/modeld/models/nav.h +++ b/selfdrive/modeld/models/nav.h @@ -52,5 +52,5 @@ struct NavModelState { void navmodel_init(NavModelState* s); NavModelResult* navmodel_eval_frame(NavModelState* s, VisionBuf* buf); -void navmodel_publish(PubMaster &pm, uint32_t frame_id, const NavModelResult &model_res, float execution_time); +void navmodel_publish(PubMaster &pm, VisionIpcBufExtra &extra, const NavModelResult &model_res, float execution_time, bool route_valid); void navmodel_free(NavModelState* s); diff --git a/selfdrive/modeld/navmodeld.cc b/selfdrive/modeld/navmodeld.cc index 75f2c62ab3..96578119e2 100644 --- a/selfdrive/modeld/navmodeld.cc +++ b/selfdrive/modeld/navmodeld.cc @@ -12,6 +12,7 @@ ExitHandler do_exit; void run_model(NavModelState &model, VisionIpcClient &vipc_client) { + SubMaster sm({"navInstruction"}); PubMaster pm({"navModel"}); double last_ts = 0; @@ -22,12 +23,14 @@ void run_model(NavModelState &model, VisionIpcClient &vipc_client) { VisionBuf *buf = vipc_client.recv(&extra); if (buf == nullptr) continue; + sm.update(0); + double t1 = millis_since_boot(); NavModelResult *model_res = navmodel_eval_frame(&model, buf); double t2 = millis_since_boot(); // send navmodel packet - navmodel_publish(pm, extra.frame_id, *model_res, (t2 - t1) / 1000.0); + navmodel_publish(pm, extra, *model_res, (t2 - t1) / 1000.0, sm["navInstruction"].getValid()); //printf("navmodel process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last_ts); last_ts = t1; diff --git a/selfdrive/navd/map_renderer.cc b/selfdrive/navd/map_renderer.cc index 7dd3f04a04..dee99e66b0 100644 --- a/selfdrive/navd/map_renderer.cc +++ b/selfdrive/navd/map_renderer.cc @@ -176,12 +176,16 @@ void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array void MapRenderer::publish(const double render_time, const bool loaded) { QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor); + + auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); + bool valid = loaded && (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid(); uint64_t ts = nanos_since_boot(); VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP); VisionIpcBufExtra extra = { .frame_id = frame_id, .timestamp_sof = (*sm)["liveLocationKalman"].getLogMonoTime(), .timestamp_eof = ts, + .valid = valid, }; assert(cap.sizeInBytes() >= buf->len); @@ -213,13 +217,10 @@ void MapRenderer::publish(const double render_time, const bool loaded) { } // Send state msg - auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman(); - bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid(); - MessageBuilder msg; auto evt = msg.initEvent(); auto state = evt.initMapRenderState(); - evt.setValid(loaded && localizer_valid); + evt.setValid(valid); state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime()); state.setRenderTime(render_time); state.setFrameId(frame_id); diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 825432e659..667b0f179d 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -de866e42d7d81fbfcab8c8cd98897fa4a25ff9db +c464c63c1e36710a2eee53f11e982d60989bbb1d