nav model: fix flicker in nav enabled state (#28815)

* Fix flicker in nav enabled state

* Move all relevant information for validity checks into navModel packet

* Ignore locationMonoTime in replay tests

* Check route valid in navmodeld

* sm update

* check that

* update refs

---------

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

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);
     }

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) {

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);

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));

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);

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;

selfdrive/navd/map_renderer.cc

@@ -176,12 +176,16 @@ void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte>
 
 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);

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-de866e42d7d81fbfcab8c8cd98897fa4a25ff9db
+c464c63c1e36710a2eee53f11e982d60989bbb1d