diff --git a/common/params.cc b/common/params.cc index c803dcf..b099579 100644 --- a/common/params.cc +++ b/common/params.cc @@ -307,6 +307,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 b0b9350..e49efef 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -167,9 +167,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 = .5, 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 @@ -177,7 +177,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: @@ -208,6 +208,12 @@ class RadarD: self.ready = False + # FrogPilot variables + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.update_frogpilot_params() + def update(self, sm: messaging.SubMaster, rr: Optional[car.RadarData]): self.ready = sm.seen['modelV2'] self.current_time = 1e-9*max(sm.logMonoTime.values()) @@ -257,8 +263,12 @@ 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"): + self.update_frogpilot_params() def publish(self, pm: messaging.PubMaster, lag_ms: float): assert self.radar_state is not None @@ -281,6 +291,9 @@ class RadarD: } pm.send('liveTracks', tracks_msg) + 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/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index b9bf17c..903884d 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -40,6 +40,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"DecelerationProfile", "Deceleration Profile", "Change the deceleration rate to be either sporty or eco-friendly.", ""}, {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""}, {"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""}, + {"LeadDetectionThreshold", "Lead Detection Threshold", "Increase or decrease the lead detection threshold to either detect leads sooner, or increase model confidence.", ""}, {"SmoothBraking", "Smoother Braking Behind Lead", "Smoothen out the braking behavior when approaching slower vehicles.", ""}, {"TrafficMode", "Traffic Mode", "Hold down the 'distance' button for 2.5 seconds to enable more aggressive driving behavior catered towards stop and go traffic.", ""}, @@ -254,6 +255,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, " feet"); + } else if (param == "LeadDetectionThreshold") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 100, std::map(), this, false, "%"); } else if (param == "Model") { modelSelectorButton = new FrogPilotButtonIconControl(title, tr("SELECT"), desc, icon); diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 9ba5481..688bcd7 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -45,7 +45,7 @@ private: std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance", "TrafficMode"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "LeadDetectionThreshold", "SmoothBraking", "StoppingDistance", "TrafficMode"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};