Controls - Longitudinal Tuning - Increase Stop Distance Behind Lead

Increase the stopping distance for a more comfortable stop from lead vehicles.

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -16,6 +16,8 @@ if __name__ == '__main__':  # generating code
 else:
   from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython
 
+  from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT
+
 from casadi import SX, vertcat
 
 MODEL_NAME = 'long'
@@ -325,10 +327,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=0):
     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
@@ -357,8 +359,9 @@ class LongitudinalMpc:
   def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard):
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+    increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0)
 
-    lead_xv_0 = self.process_lead(radarstate.leadOne)
+    lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)
 
     # To estimate a safe distance from a moving lead, we calculate how much stopping

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -64,8 +64,9 @@ class FrogPilotPlanner:
 
     driving_gear = carState.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
-    lead_distance = self.lead_one.dRel
-    stopping_distance = STOP_DISTANCE
+    distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0)
+    lead_distance = self.lead_one.dRel - distance_offset
+    stopping_distance = STOP_DISTANCE + distance_offset
 
     run_cem = frogpilot_toggles.conditional_experimental_mode
     if run_cem and (controlsState.enabled or frogpilotCarControl.alwaysOnLateral) and driving_gear: