Alert when modeld is lagging by more than a frame (#1823)

* alert when modeld is lagging by more than 1 frame

* log frameAge in modelData

* set posenet valid

* compute frame_age once

cereal

@@ -1 +1 @@
-Subproject commit f35491f0c5bf21d7d8c0cae9b5761fdf64a8d350
+Subproject commit 80bbbd4bf70698a3e7b7eaa1e463753c3db2ab5f

selfdrive/controls/controlsd.py

@@ -227,6 +227,8 @@ class Controls:
       self.events.add(EventName.relayMalfunction)
     if self.sm['plan'].fcw:
       self.events.add(EventName.fcw)
+    if self.sm['model'].frameAge > 1:
+      self.events.add(EventName.modeldLagging)
 
     # Only allow engagement with brake pressed when stopped behind another stopped car
     if CS.brakePressed and self.sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED \

selfdrive/controls/lib/events.py

@@ -204,8 +204,6 @@ def wrong_car_mode_alert(CP, sm, metric):
 EVENTS = {
   # ********** events with no alerts **********
 
-  EventName.modeldLagging: {},
-
   # ********** events only containing alerts displayed in all states **********
 
   EventName.debugAlert: {
@@ -651,6 +649,11 @@ EVENTS = {
     ET.NO_ENTRY : NoEntryAlert("Radar Error: Restart the Car"),
   },
 
+  EventName.modeldLagging: {
+    ET.SOFT_DISABLE: SoftDisableAlert("Driving model lagging"),
+    ET.NO_ENTRY : NoEntryAlert("Driving model lagging"),
+  },
+
   EventName.lowMemory: {
     ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
     ET.PERMANENT: Alert(

selfdrive/modeld/modeld.cc

@@ -217,8 +217,8 @@ int main(int argc, char **argv) {
                              model_transform, NULL, vec_desire);
         mt2 = millis_since_boot();
 
-        model_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof);
+        model_publish(pm, extra.frame_id, frame_id, sm.allAliveAndValid(), model_buf, extra.timestamp_eof);
-        posenet_publish(pm, extra.frame_id, frame_id, model_buf, extra.timestamp_eof);
+        posenet_publish(pm, extra.frame_id, frame_id, sm.allAliveAndValid(), model_buf, extra.timestamp_eof);
 
         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;

selfdrive/modeld/models/driving.cc

@@ -245,15 +245,18 @@ void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float
   longi.setAccelerations(accel);
 }
 
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid,
                    const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
   // make msg
   capnp::MallocMessageBuilder msg;
   cereal::Event::Builder event = msg.initRoot<cereal::Event>();
   event.setLogMonoTime(nanos_since_boot());
 
+  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
+
   auto framed = event.initModel();
   framed.setFrameId(vipc_frame_id);
+  framed.setFrameAge(frame_age);
   framed.setTimestampEof(timestamp_eof);
 
   auto lpath = framed.initPath();
@@ -290,13 +293,13 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
 
   auto meta = framed.initMeta();
   fill_meta(meta, net_outputs.meta);
-  event.setValid(frame_id < vipc_frame_id + MAX_FRAME_AGE);
+  event.setValid((frame_age < MAX_FRAME_AGE) && sm_alive_valid);
 
   pm.send("model", msg);
 }
 
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
+void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid,
-                   const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
+                     const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
   capnp::MallocMessageBuilder msg;
   cereal::Event::Builder event = msg.initRoot<cereal::Event>();
   event.setLogMonoTime(nanos_since_boot());
@@ -324,9 +327,12 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
   kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3);
   posenetd.setRotStd(rot_std_vs);
 
+
   posenetd.setTimestampEof(timestamp_eof);
   posenetd.setFrameId(vipc_frame_id);
-  event.setValid(frame_id < vipc_frame_id + MAX_FRAME_AGE);
+
+  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
+  event.setValid((frame_age < MAX_FRAME_AGE) && sm_alive_valid);
 
   pm.send("cameraOdometry", msg);
 }

selfdrive/modeld/models/driving.h

@@ -73,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 vipc_frame_id, uint32_t frame_id,
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, bool sm_alive_valid,
-                   const ModelDataRaw &data, uint64_t timestamp_eof);
-void posenet_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 vipc_frame_id, uint32_t frame_id, bool sm_alive_valid,
+                     const ModelDataRaw &data, uint64_t timestamp_eof);
 #endif