Modeld: send confidence class (#28625)

* 0.7

* magic

* faster magic

* more simple

* up

* empty

* more mid bits

* naive

* flatten

* dz

* that can stay

* this is fine

* what the

* what the

* giRevert "what the"

This reverts commit 1619ba68e6.

* Revert "what the"

This reverts commit 0037dd3682.

* 1x fine

* that was fine

* combined

* independent cum

* 0 is fine

* use metrics

* up cereal

* process and publish from modeld

* cleanup

* use s.output

* bg

* a greener approach

* dns

* serial

* update ref commit

* rebase

* ref

* cereal master

---------

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: ce2dffe566

common/modeldata.h

@@ -10,6 +10,9 @@ const int LON_MPC_N = 32;
 const float MIN_DRAW_DISTANCE = 10.0;
 const float MAX_DRAW_DISTANCE = 100.0;
 
+const float RYG_GREEN = 0.01165;
+const float RYG_YELLOW = 0.06157;
+
 template <typename T, size_t size>
 constexpr std::array<T, size> build_idxs(float max_val) {
   std::array<T, size> result{};

selfdrive/modeld/modeld.cc

@@ -178,8 +178,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
     float model_execution_time = (mt2 - mt1) / 1000.0;
 
     if (model_output != nullptr) {
-      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()), nav_enabled, live_calib_seen);
+      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,
+                    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

@@ -199,6 +199,44 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMet
   meta.setHardBrakePredicted(above_fcw_threshold);
 }
 
+void fill_confidence(ModelState* s, cereal::ModelDataV2::Builder &framed) {
+  if (framed.getFrameId() % (2*MODEL_FREQ) == 0) {
+    // update every 2s to match predictions interval
+    auto dbps = framed.getMeta().getDisengagePredictions().getBrakeDisengageProbs();
+    auto dgps = framed.getMeta().getDisengagePredictions().getGasDisengageProbs();
+    auto dsps = framed.getMeta().getDisengagePredictions().getSteerOverrideProbs();
+
+    float any_dp[DISENGAGE_LEN];
+    float dp_ind[DISENGAGE_LEN];
+
+    for (int i = 0; i < DISENGAGE_LEN; i++) {
+      any_dp[i] = 1 - ((1-dbps[i])*(1-dgps[i])*(1-dsps[i])); // any disengage prob
+    }
+
+    dp_ind[0] = any_dp[0];
+    for (int i = 0; i < DISENGAGE_LEN-1; i++) {
+      dp_ind[i+1] = (any_dp[i+1] - any_dp[i]) / (1 - any_dp[i]); // independent disengage prob for each 2s slice
+    }
+
+    // rolling buf for 2, 4, 6, 8, 10s
+    std::memmove(&s->disengage_buffer[0], &s->disengage_buffer[DISENGAGE_LEN], sizeof(float) * DISENGAGE_LEN * (DISENGAGE_LEN-1));
+    std::memcpy(&s->disengage_buffer[DISENGAGE_LEN * (DISENGAGE_LEN-1)], &dp_ind[0], sizeof(float) * DISENGAGE_LEN);
+  }
+
+  float score = 0;
+  for (int i = 0; i < DISENGAGE_LEN; i++) {
+    score += s->disengage_buffer[i*DISENGAGE_LEN+DISENGAGE_LEN-1-i] / DISENGAGE_LEN;
+  }
+
+  if (score < RYG_GREEN) {
+    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::GREEN);
+  } else if (score < RYG_YELLOW) {
+    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::YELLOW);
+  } else {
+    framed.setConfidence(cereal::ModelDataV2::ConfidenceClass::RED);
+  }
+}
+
 template<size_t size>
 void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array<float, size> &t,
                const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
@@ -313,7 +351,7 @@ void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<floa
   });
 }
 
-void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) {
+void fill_model(ModelState* s, cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) {
   const auto &best_plan = net_outputs.plans.get_best_prediction();
   std::array<float, TRAJECTORY_SIZE> plan_t;
   std::fill_n(plan_t.data(), plan_t.size(), NAN);
@@ -343,6 +381,9 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
   // meta
   fill_meta(framed.initMeta(), net_outputs.meta);
 
+  // confidence
+  fill_confidence(s, framed);
+
   // leads
   auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
   std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
@@ -362,9 +403,9 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_out
   temporal_pose.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
 }
 
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+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,
-                   float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool nav_enabled, const bool valid) {
+                   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;
   auto framed = msg.initEvent(valid).initModelV2();
@@ -376,9 +417,9 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id
   framed.setModelExecutionTime(model_execution_time);
   framed.setNavEnabled(nav_enabled);
   if (send_raw_pred) {
-    framed.setRawPredictions(raw_pred.asBytes());
+    framed.setRawPredictions((kj::ArrayPtr<const float>(s->output.data(), s->output.size())).asBytes());
   }
-  fill_model(framed, net_outputs);
+  fill_model(s, framed, net_outputs);
   pm.send("modelV2", msg);
 }
 

selfdrive/modeld/models/driving.h

@@ -262,6 +262,7 @@ struct ModelState {
   ModelFrame *frame = nullptr;
   ModelFrame *wide_frame = nullptr;
   std::array<float, HISTORY_BUFFER_LEN * FEATURE_LEN> feature_buffer = {};
+  std::array<float, DISENGAGE_LEN * DISENGAGE_LEN> disengage_buffer = {};
   std::array<float, NET_OUTPUT_SIZE> output = {};
   std::unique_ptr<RunModel> m;
 #ifdef DESIRE
@@ -283,8 +284,8 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 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(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
+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,
-                   float model_execution_time, kj::ArrayPtr<const float> raw_pred, const bool nav_enabled, const bool valid);
+                   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/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-965fa8cc8c131a8978c142813658b724a519ac9e
+3257b354b31c3d42c85a86fc4883f29c47ef56cb