diff --git a/common/params.cc b/common/params.cc index 954f7fd9e..ccab37e4e 100644 --- a/common/params.cc +++ b/common/params.cc @@ -317,6 +317,7 @@ std::unordered_map keys = { {"LastMapsUpdate", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadDepartingAlert", PERSISTENT}, + {"LeadDetectionThreshold", PERSISTENT}, {"LeadInfo", PERSISTENT}, {"LockDoors", PERSISTENT}, {"LongitudinalTune", PERSISTENT}, diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d30086bb2..99199cef1 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -168,9 +168,9 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, lead_detection_threshold: float, low_speed_override: bool = True) -> dict[str, Any]: # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > .5: + if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_threshold: track = match_vision_to_track(v_ego, lead_msg, tracks) else: track = None @@ -178,7 +178,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = {'status': False} if track is not None: lead_dict = track.get_RadarState(lead_msg.prob) - elif (track is None) and ready and (lead_msg.prob > .5): + elif (track is None) and ready and (lead_msg.prob > lead_detection_threshold): lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: @@ -267,8 +267,8 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_detection_threshold, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_detection_threshold, low_speed_override=False) # Update FrogPilot parameters if self.params_memory.get_bool("FrogPilotTogglesUpdated"): @@ -323,6 +323,7 @@ class RadarD: def update_frogpilot_params(self): longitudinal_tune = self.params.get_bool("LongitudinalTune") + self.lead_detection_threshold = self.params.get_int("LeadDetectionThreshold") / 100 if longitudinal_tune else .5 # fuses camera and radar data for best lead detection def main(): diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index e89b123a4..5c07b1df6 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -87,6 +87,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"DecelerationProfile", tr("Deceleration Profile"), tr("Change the deceleration rate to be either sporty or eco-friendly."), ""}, {"AggressiveAcceleration", tr("Increase Acceleration Behind Faster Lead"), tr("Increase aggressiveness when following a faster lead."), ""}, {"StoppingDistance", tr("Increase Stop Distance Behind Lead"), tr("Increase the stopping distance for a more comfortable stop from lead vehicles."), ""}, + {"LeadDetectionThreshold", tr("Lead Detection Threshold"), tr("Increase or decrease the lead detection threshold to either detect leads sooner, or increase model confidence."), ""}, {"SmoothBraking", tr("Smoother Braking"), tr("Smoothen out the braking behavior when approaching slower vehicles."), ""}, {"TrafficMode", tr("Traffic Mode"), tr("Enable the ability to activate 'Traffic Mode' by holding down the 'distance' button for 2.5 seconds. When 'Traffic Mode' is active the onroad UI will turn red and openpilot will drive catered towards stop and go traffic."), ""}, @@ -348,6 +349,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil toggle = profileSelection; } else if (param == "StoppingDistance") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map(), this, false, tr(" feet")); + } else if (param == "LeadDetectionThreshold") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 100, std::map(), this, false, "%"); } else if (param == "SmoothBraking" && !isRelease) { std::vector brakingToggles{"SmoothBrakingJerk", "SmoothBrakingFarLead"}; std::vector brakingToggleNames{tr("Apply to Jerk"), tr("Far Lead Offset")}; diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index 85ff98bb1..16d97bdec 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -45,7 +45,7 @@ private: std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "NNFFLite", "TacoTune"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "SmoothBraking", "StoppingDistance", "TrafficMode"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "LeadDetectionThreshold", "SmoothBraking", "StoppingDistance", "TrafficMode"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck"}; std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "OnroadDistanceButton", "PauseLateralSpeed", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};