diff --git a/common/modeldata.h b/common/modeldata.h index 86e14a4512..22ede60997 100644 --- a/common/modeldata.h +++ b/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 constexpr std::array build_idxs(float max_val) { std::array result{}; diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 6297f3768f..15d33f3be9 100644 --- a/selfdrive/modeld/modeld.cc +++ b/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(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); } diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index d940880cba..087a4acfa6 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/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 void fill_xyzt(cereal::XYZTData::Builder xyzt, const std::array &t, const std::array &x, const std::array &y, const std::array &z) { @@ -313,7 +351,7 @@ void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array 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 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 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(s->output.data(), s->output.size())).asBytes()); } - fill_model(framed, net_outputs); + fill_model(s, framed, net_outputs); pm.send("modelV2", msg); } diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 5385d4b953..ae55e9c8a0 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -262,6 +262,7 @@ struct ModelState { ModelFrame *frame = nullptr; ModelFrame *wide_frame = nullptr; std::array feature_buffer = {}; + std::array disengage_buffer = {}; std::array output = {}; std::unique_ptr 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 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); diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index f4d0b4fc98..19c316b604 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -965fa8cc8c131a8978c142813658b724a519ac9e +3257b354b31c3d42c85a86fc4883f29c47ef56cb