From 6d959ddc36b914044d1b87ff716c324bedcff787 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Mon, 8 Apr 2024 15:11:28 -0700 Subject: [PATCH] Increase stopping distance from lead Added toggle to increase the stop distance from lead vehicles. --- common/params.cc | 1 + selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 9 ++++++--- selfdrive/frogpilot/controls/frogpilot_planner.py | 4 +++- selfdrive/frogpilot/ui/qt/offroad/control_settings.cc | 8 ++++++++ selfdrive/frogpilot/ui/qt/offroad/control_settings.h | 2 +- 5 files changed, 19 insertions(+), 5 deletions(-) diff --git a/common/params.cc b/common/params.cc index 3390cefad..3eb369d92 100644 --- a/common/params.cc +++ b/common/params.cc @@ -324,6 +324,7 @@ std::unordered_map keys = { {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, + {"StoppingDistance", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"UseSI", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 87b5f59ce..2156fa33d 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -327,10 +327,12 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead): + def process_lead(self, lead, increased_stopping_distance=0): v_ego = self.x0[1] + increased_stopping_distance = max(increased_stopping_distance - v_ego, 0) + if lead is not None and lead.status: - x_lead = lead.dRel + x_lead = lead.dRel - increased_stopping_distance v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau @@ -360,7 +362,7 @@ class LongitudinalMpc: v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0 = self.process_lead(radarstate.leadOne) + lead_xv_0 = self.process_lead(radarstate.leadOne, self.increased_stopping_distance) lead_xv_1 = self.process_lead(radarstate.leadTwo) # To estimate a safe distance from a moving lead, we calculate how much stopping @@ -486,6 +488,7 @@ class LongitudinalMpc: is_metric = params.get_bool("IsMetric") longitudinal_tune = params.get_bool("LongitudinalTune") + self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 if __name__ == "__main__": ocp = gen_long_ocp() diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index ae72e7143..5ba6f93ce 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -109,7 +109,8 @@ class FrogPilotPlanner: self.cem.update(carState, controlsState.enabled, frogpilotNavigation, modelData, radarState, road_curvature, self.t_follow, v_ego) def update_follow_values(self, jerk, radarState, t_follow, v_ego, v_lead): - lead_distance = radarState.leadOne.dRel + stopping_distance = STOP_DISTANCE + max(self.increased_stopping_distance - v_ego, 0) + lead_distance = self.lead_one.dRel + stopping_distance # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead if self.aggressive_acceleration and not self.release: @@ -192,3 +193,4 @@ class FrogPilotPlanner: self.acceleration_profile = self.params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.deceleration_profile = self.params.get_int("DecelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = longitudinal_tune and self.params.get_bool("AggressiveAcceleration") + self.increased_stopping_distance = self.params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc index 3d3b4202b..75a28ce54 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.cc @@ -41,6 +41,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"AccelerationProfile", tr("Acceleration Profile"), tr("Change the acceleration rate to be either sporty or eco-friendly."), ""}, {"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."), ""}, {"QOLControls", tr("Quality of Life"), tr("Miscellaneous quality of life changes to improve your overall openpilot experience."), "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"CustomCruise", tr("Cruise Increase Interval"), tr("Set a custom interval to increase the max set speed by."), ""}, @@ -222,6 +223,8 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector profileOptions{tr("Standard"), tr("Eco"), tr("Sport")}; FrogPilotButtonParamControl *profileSelection = new FrogPilotButtonParamControl(param, title, desc, icon, profileOptions); toggle = profileSelection; + } else if (param == "StoppingDistance") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, std::map(), this, false, tr(" feet")); } else if (param == "QOLControls") { FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); @@ -390,25 +393,30 @@ void FrogPilotControlsPanel::updateMetric() { params.putIntNonBlocking("CustomCruise", std::nearbyint(params.getInt("CustomCruise") * speedConversion)); params.putIntNonBlocking("CustomCruiseLong", std::nearbyint(params.getInt("CustomCruiseLong") * speedConversion)); params.putIntNonBlocking("PauseAOLOnBrake", std::nearbyint(params.getInt("PauseAOLOnBrake") * speedConversion)); + params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } FrogPilotParamValueControl *customCruiseToggle = static_cast(toggles["CustomCruise"]); FrogPilotParamValueControl *customCruiseLongToggle = static_cast(toggles["CustomCruiseLong"]); FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast(toggles["PauseAOLOnBrake"]); + FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { customCruiseToggle->updateControl(1, 150, tr(" kph")); customCruiseLongToggle->updateControl(1, 150, tr(" kph")); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" kph")); + stoppingDistanceToggle->updateControl(0, 5, tr(" meters")); } else { customCruiseToggle->updateControl(1, 99, tr(" mph")); customCruiseLongToggle->updateControl(1, 99, tr(" mph")); pauseAOLOnBrakeToggle->updateControl(0, 99, tr(" mph")); + stoppingDistanceToggle->updateControl(0, 10, tr(" feet")); } customCruiseToggle->refresh(); customCruiseLongToggle->refresh(); pauseAOLOnBrakeToggle->refresh(); + stoppingDistanceToggle->refresh(); } void FrogPilotControlsPanel::hideToggles() { diff --git a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h index f5279367f..3a44774be 100644 --- a/selfdrive/frogpilot/ui/qt/offroad/control_settings.h +++ b/selfdrive/frogpilot/ui/qt/offroad/control_settings.h @@ -37,7 +37,7 @@ private: std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {"ForceAutoTune"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"}; std::set mtscKeys = {}; std::set qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "ReverseCruise"}; std::set speedLimitControllerKeys = {};