Increase stopping distance from lead

Added toggle to increase the stop distance from lead vehicles.

common/params.cc

@@ -324,6 +324,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"StandardFollow", PERSISTENT},
     {"StandardJerk", PERSISTENT},
     {"StockTune", PERSISTENT},
+    {"StoppingDistance", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"UseSI", PERSISTENT},
     {"WarningImmediateVolume", PERSISTENT},

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()

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

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<QString> 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<int, QString>(), 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<FrogPilotParamValueControl*>(toggles["CustomCruise"]);
   FrogPilotParamValueControl *customCruiseLongToggle = static_cast<FrogPilotParamValueControl*>(toggles["CustomCruiseLong"]);
   FrogPilotParamValueControl *pauseAOLOnBrakeToggle = static_cast<FrogPilotParamValueControl*>(toggles["PauseAOLOnBrake"]);
+  FrogPilotParamValueControl *stoppingDistanceToggle = static_cast<FrogPilotParamValueControl*>(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() {

selfdrive/frogpilot/ui/qt/offroad/control_settings.h

@@ -37,7 +37,7 @@ private:
   std::set<QString> experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaTap"};
   std::set<QString> laneChangeKeys = {};
   std::set<QString> lateralTuneKeys = {"ForceAutoTune"};
-  std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile"};
+  std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "DecelerationProfile", "StoppingDistance"};
   std::set<QString> mtscKeys = {};
   std::set<QString> qolKeys = {"CustomCruise", "CustomCruiseLong", "DisableOnroadUploads", "ReverseCruise"};
   std::set<QString> speedLimitControllerKeys = {};