From c131087dc0063c964066e0d36a038b096c0f03f3 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Increase stop distance Added toggle to increase the stop distance from lead vehicles. --- common/params.cc | 1 + .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 8 ++++---- selfdrive/frogpilot/functions/frogpilot_planner.py | 1 + selfdrive/frogpilot/ui/control_settings.cc | 10 ++++++++++ selfdrive/frogpilot/ui/control_settings.h | 2 +- 5 files changed, 17 insertions(+), 5 deletions(-) diff --git a/common/params.cc b/common/params.cc index 7287938..844ef21 100644 --- a/common/params.cc +++ b/common/params.cc @@ -309,6 +309,7 @@ std::unordered_map keys = { {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, + {"StoppingDistance", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 857f46d..48ec7bb 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -326,10 +326,10 @@ 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): v_ego = self.x0[1] 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 @@ -361,8 +361,8 @@ 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_1 = self.process_lead(radarstate.leadTwo) + lead_xv_0 = self.process_lead(radarstate.leadOne, frogpilot_planner.increased_stopping_distance) + lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner.increased_stopping_distance) # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead if frogpilot_planner.aggressive_acceleration: diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 286e811..32a29f7 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -135,3 +135,4 @@ class FrogPilotPlanner: self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration") + self.increased_stopping_distance = 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/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 4db1825..6fe3700 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -31,6 +31,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"AccelerationProfile", "Acceleration Profile", "Change the acceleration rate to be either sporty or eco-friendly.", ""}, {"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.", ""}, {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"DisableOnroadUploads", "Disable Onroad Uploads", "Prevent large data uploads when onroad.", ""}, @@ -209,6 +210,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, " feet"); } else if (param == "QOLControls") { FrogPilotParamManageControl *qolToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); @@ -306,12 +309,19 @@ void FrogPilotControlsPanel::updateMetric() { double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); + params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } + FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); + if (isMetric) { + stoppingDistanceToggle->updateControl(0, 5, " meters"); } else { + stoppingDistanceToggle->updateControl(0, 10, " feet"); } + stoppingDistanceToggle->refresh(); + previousIsMetric = isMetric; } diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 94f1813..b25e07e 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -40,7 +40,7 @@ private: std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads"}; std::set laneChangeKeys = {}; std::set lateralTuneKeys = {"ForceAutoTune"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"}; std::set mtscKeys = {}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "ReverseCruise"}; std::set speedLimitControllerKeys = {};