Dom's Plan 4

selfdrive/controls/lib/longcontrol.py

@@ -11,6 +11,8 @@ from openpilot.starpilot.common.testing_grounds import testing_ground
 CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
 clip = np.clip
 interp = np.interp
+STOPPING_RELEASE_HYSTERESIS = 0.35
+STOPPING_RELEASE_MIN_ACCEL = 0.2
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
@@ -25,13 +27,15 @@ def apply_deadzone(error, deadzone):
 
 
 def long_control_state_trans(CP, active, long_control_state, v_ego,
-                             should_stop, brake_pressed, cruise_standstill, starpilot_toggles):
+                             should_stop, brake_pressed, cruise_standstill, starpilot_toggles,
+                             allow_stopping_release=True):
   # Ignore cruise standstill if car has a gas interceptor
   cruise_standstill = cruise_standstill and not CP.enableGasInterceptorDEPRECATED
   stopping_condition = should_stop
   starting_condition = (not should_stop and
                         not cruise_standstill and
                         not brake_pressed)
+  stopping_release_condition = starting_condition and allow_stopping_release
   started_condition = v_ego > starpilot_toggles.vEgoStarting
 
   if not active:
@@ -48,9 +52,9 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
           long_control_state = LongCtrlState.pid
 
     elif long_control_state == LongCtrlState.stopping:
-      if starting_condition and CP.startingState:
+      if stopping_release_condition and CP.startingState:
         long_control_state = LongCtrlState.starting
-      elif starting_condition:
+      elif stopping_release_condition:
         long_control_state = LongCtrlState.pid
 
     elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
@@ -115,6 +119,7 @@ class LongControl:
     self.last_output_accel = 0.0
     self.last_a_target = 0.0
     self.integrator_hold_frames = 0
+    self.stop_release_counter = 0
     self.is_gm_pedal_long = bool(
       CP.brand == "gm" and CP.enableGasInterceptorDEPRECATED and (CP.flags & GMFlags.PEDAL_LONG.value)
     )
@@ -149,10 +154,33 @@ class LongControl:
     self.mode_transition_duration = 1.0
     self.transitioning = False
 
-  def reset(self):
+  def reset(self, preserve_stop_release=False):
     self.pid.reset()
     self.last_a_target = 0.0
     self.integrator_hold_frames = 0
+    if not preserve_stop_release:
+      self.stop_release_counter = 0
+
+  def _stop_release_ready(self, CS, a_target, should_stop, starpilot_toggles):
+    if self.long_control_state != LongCtrlState.stopping:
+      self.stop_release_counter = 0
+      return True
+
+    if should_stop or CS.brakePressed or CS.cruiseState.standstill:
+      self.stop_release_counter = 0
+      return False
+
+    if CS.vEgo > starpilot_toggles.vEgoStarting:
+      self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
+      return True
+
+    if a_target > STOPPING_RELEASE_MIN_ACCEL:
+      max_frames = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
+      self.stop_release_counter = min(self.stop_release_counter + 1, max_frames)
+    else:
+      self.stop_release_counter = 0
+
+    return self.stop_release_counter >= int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
 
   def _get_pedal_long_freeze(self, a_target, error, v_ego, accel_limits):
     volt_test_tune_handoff = self.is_volt and testing_ground.use_2
@@ -198,9 +226,11 @@ class LongControl:
     self.pid.neg_limit = accel_limits[0]
     self.pid.pos_limit = accel_limits[1]
 
+    allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, starpilot_toggles)
     self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
                                                        should_stop, CS.brakePressed,
-                                                       CS.cruiseState.standstill, starpilot_toggles)
+                                                       CS.cruiseState.standstill, starpilot_toggles,
+                                                       allow_stopping_release=allow_stopping_release)
     if self.long_control_state == LongCtrlState.off:
       self.reset()
       output_accel = 0.
@@ -210,7 +240,7 @@ class LongControl:
       if output_accel > starpilot_toggles.stopAccel:
         output_accel = min(output_accel, 0.0)
         output_accel -= starpilot_toggles.stoppingDecelRate * DT_CTRL
-      self.reset()
+      self.reset(preserve_stop_release=True)
 
     elif self.long_control_state == LongCtrlState.starting:
       if starpilot_toggles.human_acceleration:

selfdrive/controls/lib/longitudinal_planner.py

@@ -39,6 +39,13 @@ VISION_LEAD_APPROACH_MAX_DECEL = 0.45
 VISION_LEAD_APPROACH_MIN_DECEL = 0.12
 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85
 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
+VISION_SLOW_LEAD_MAX_SPEED = 5.0
+VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
+VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
+VISION_SLOW_LEAD_FULL_TTC = 2.0
+VISION_SLOW_LEAD_MAX_DECEL = 1.2
+VISION_SLOW_LEAD_MIN_DECEL = 0.18
+VISION_SLOW_LEAD_MIN_MODEL_PROB = 0.9
 LEAD_APPROACH_TFOLLOW_TRIGGER_TIME = 4.5
 LEAD_APPROACH_TFOLLOW_FULL_TIME = 1.5
 LEAD_APPROACH_TFOLLOW_MAX_DELTA = 0.18
@@ -304,6 +311,43 @@ class LongitudinalPlanner:
 
     return max(accel_min, -approach_decel)
 
+  def get_vision_slow_stopped_lead_cap(self, lead, v_ego, accel_min, t_follow):
+    if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < VISION_SLOW_LEAD_MIN_MODEL_PROB or float(lead.vLead) > VISION_SLOW_LEAD_MAX_SPEED:
+      return None
+
+    lead_brake = max(0.0, -float(lead.aLeadK))
+    reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
+    closing_speed = max(0.0, v_ego - lead.vLead)
+    projected_closing_speed = closing_speed + lead_brake * reaction_t
+    if projected_closing_speed < VISION_SLOW_LEAD_MIN_CLOSING_SPEED:
+      return None
+
+    stop_gap = float(max(STOP_DISTANCE + 1.0, 2.5 + 0.15 * max(float(lead.vLead), 0.0)))
+    delay_buffer = projected_closing_speed * reaction_t
+    available_gap = max(float(lead.dRel) - stop_gap - delay_buffer, 0.5)
+    projected_ttc = available_gap / max(projected_closing_speed, 0.1)
+    if projected_ttc > VISION_SLOW_LEAD_TRIGGER_TTC:
+      return None
+
+    time_factor = float(np.clip((VISION_SLOW_LEAD_TRIGGER_TTC - projected_ttc) /
+                                (VISION_SLOW_LEAD_TRIGGER_TTC - VISION_SLOW_LEAD_FULL_TTC), 0.0, 1.0))
+    prob_factor = float(np.clip((lead_prob - VISION_SLOW_LEAD_MIN_MODEL_PROB) /
+                                (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_SLOW_LEAD_MIN_MODEL_PROB), 0.0, 1.0))
+    speed_factor = float(np.clip((VISION_SLOW_LEAD_MAX_SPEED - max(float(lead.vLead), 0.0)) /
+                                 VISION_SLOW_LEAD_MAX_SPEED, 0.0, 1.0))
+    required_decel = (projected_closing_speed ** 2) / (2.0 * available_gap)
+    decel_scale = 0.45 + 0.35 * time_factor + 0.20 * speed_factor
+    approach_decel = min(VISION_SLOW_LEAD_MAX_DECEL, required_decel * decel_scale)
+    approach_decel *= 0.65 + 0.35 * prob_factor
+    if approach_decel < VISION_SLOW_LEAD_MIN_DECEL:
+      return None
+
+    return max(accel_min, -approach_decel)
+
   def get_dynamic_t_follow(self, base_t_follow, lead, v_ego):
     base_t_follow = float(base_t_follow)
     target_t_follow = base_t_follow
@@ -628,6 +672,9 @@ class LongitudinalPlanner:
         cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
         if cap is not None:
           close_lead_caps.append(cap)
+        slow_stop_cap = self.get_vision_slow_stopped_lead_cap(lead, v_ego, output_accel_min, effective_t_follow)
+        if slow_stop_cap is not None:
+          close_lead_caps.append(slow_stop_cap)
         approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow)
         if approach_cap is not None:
           close_lead_caps.append(approach_cap)

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -4,18 +4,22 @@ from types import SimpleNamespace
 from openpilot.common.constants import CV
 from openpilot.starpilot.controls.starpilot_planner import StarPilotPlanner
 import openpilot.starpilot.controls.starpilot_planner as starpilot_planner_module
+import openpilot.starpilot.controls.lib.conditional_experimental_mode as conditional_experimental_mode_module
 from openpilot.starpilot.controls.lib.conditional_experimental_mode import ConditionalExperimentalMode
 
 
 def make_cem(*, model_length: float, model_stopped: bool = False, tracking_lead: bool = False,
-             lead_status: bool = False, lead_d_rel: float = float("inf")):
+             lead_status: bool = False, lead_d_rel: float = float("inf"),
+             lead_v_lead: float = 0.0, lead_model_prob: float = 0.0, lead_radar: bool = False):
   planner = SimpleNamespace(
     params=None,
     params_memory=None,
     model_length=model_length,
     model_stopped=model_stopped,
     tracking_lead=tracking_lead,
-    lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel),
+    starpilot_vcruise=SimpleNamespace(stop_sign_confirmed=False),
+    lead_one=SimpleNamespace(status=lead_status, dRel=lead_d_rel, vLead=lead_v_lead,
+                             modelProb=lead_model_prob, radar=lead_radar),
   )
   return ConditionalExperimentalMode(planner)
 
@@ -102,6 +106,33 @@ def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff():
   assert cem.stop_light_detected
 
 
+def test_stop_light_latch_holds_slow_high_confidence_vision_lead_during_model_flicker(monkeypatch):
+  v_ego = 40 * CV.MPH_TO_MS
+  model_length = v_ego * 3.8
+  cem = make_cem(
+    model_length=model_length,
+    lead_status=True,
+    lead_d_rel=model_length - 5.0,
+    lead_v_lead=3.0,
+    lead_model_prob=0.98,
+  )
+
+  run_stop_light_detector(cem, v_ego, steps=20)
+  assert cem.stop_light_detected
+
+  monotonic_values = iter([10.0, 10.2])
+  monkeypatch.setattr(conditional_experimental_mode_module.time, "monotonic", lambda: next(monotonic_values))
+
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+  cem.stop_light_detected = False
+  cem.stop_light_model_detected = False
+  cem.stop_light_filter.x = 0.0
+  cem.starpilot_planner.model_length = v_ego * 9.0
+  cem.stop_sign_and_light(v_ego, make_sm(), model_time=7.0)
+
+  assert cem.stop_light_detected
+
+
 class DummyThemeManager:
   def update_wheel_image(self, *args, **kwargs):
     pass

selfdrive/controls/tests/test_longcontrol.py

@@ -77,6 +77,18 @@ def test_starting():
   assert next_state == LongCtrlState.pid
 
 
+def test_stopping_release_hysteresis_blocks_immediate_launch():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  toggles = make_toggles(vEgoStarting=0.5)
+  active = True
+  current_state = LongCtrlState.stopping
+
+  next_state = long_control_state_trans(CP, active, current_state, v_ego=0.0,
+                             should_stop=False, brake_pressed=False, cruise_standstill=False, starpilot_toggles=toggles,
+                             allow_stopping_release=False)
+  assert next_state == LongCtrlState.stopping
+
+
 def test_starting_accel_unchanged_when_custom_profile_disabled():
   CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
   CP.longitudinalTuning.kpBP = [0.0]
@@ -125,6 +137,43 @@ def test_starting_accel_obeys_a_target_cap_when_custom_profile_enabled():
   assert output_accel == 0.1
 
 
+def test_update_requires_sustained_positive_target_to_leave_stopping():
+  CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
+  CP.longitudinalTuning.kpBP = [0.0]
+  CP.longitudinalTuning.kpV = [0.1]
+  CP.longitudinalTuning.kiBP = [0.0]
+  CP.longitudinalTuning.kiV = [0.03]
+
+  lc = LongControl(CP)
+  lc.long_control_state = LongCtrlState.stopping
+  CS = car.CarState.new_message(vEgo=0.0, aEgo=0.0, brakePressed=False)
+  CS.cruiseState.standstill = False
+
+  release_frames = int(round(longcontrol.STOPPING_RELEASE_HYSTERESIS / longcontrol.DT_CTRL))
+  for _ in range(release_frames - 1):
+    output_accel = lc.update(
+      active=True,
+      CS=CS,
+      a_target=0.5,
+      should_stop=False,
+      accel_limits=(-3.0, 2.0),
+      starpilot_toggles=make_toggles(startAccel=1.5),
+    )
+    assert lc.long_control_state == LongCtrlState.stopping
+    assert output_accel <= 0.0
+
+  lc.update(
+    active=True,
+    CS=CS,
+    a_target=0.5,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(startAccel=1.5),
+  )
+
+  assert lc.long_control_state == LongCtrlState.starting
+
+
 def test_volt_testing_ground_handoff_freezes_integrator(monkeypatch):
   CP = car.CarParams.new_message()
   CP.brand = "gm"

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -225,6 +225,28 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
   assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None
 
 
+def test_vision_slow_stopped_lead_cap_brakes_earlier_for_confident_stop():
+  v_ego = 13.207
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=49.131, v_lead=1.837, a_lead=-0.312, radar=False, model_prob=0.942)
+
+  slow_stop_cap = planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.75)
+
+  assert slow_stop_cap is not None
+  assert slow_stop_cap < -0.9
+  assert slow_stop_cap > -1.25
+
+
+def test_vision_slow_stopped_lead_cap_ignores_far_high_speed_stop_candidate():
+  v_ego = 33.5
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=183.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=0.995)
+
+  assert planner.get_vision_slow_stopped_lead_cap(lead, v_ego, -1.0, 1.45) is None
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12"])
 def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version):
   v_ego = 21.535

selfdrive/controls/tests/test_starpilot_acceleration.py

@@ -0,0 +1,117 @@
+from types import SimpleNamespace
+
+import pytest
+
+from openpilot.common.constants import CV
+from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN
+from openpilot.starpilot.common.accel_profile import ACCELERATION_PROFILES, DECELERATION_PROFILES
+from openpilot.starpilot.controls.lib.starpilot_acceleration import (
+  A_CRUISE_MIN_ECO,
+  StarPilotAcceleration,
+  get_slc_shaped_min_accel,
+)
+
+
+class FakePlanner:
+  def __init__(self):
+    self.v_cruise = 0.0
+    self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0)
+
+
+def make_toggles(**overrides):
+  defaults = {
+    "acceleration_profile": ACCELERATION_PROFILES["STANDARD"],
+    "deceleration_profile": DECELERATION_PROFILES["ECO"],
+    "custom_accel_profile": False,
+    "custom_accel_profile_values": [],
+    "ev_tuning": True,
+    "truck_tuning": False,
+    "human_acceleration": False,
+    "map_acceleration": False,
+    "map_deceleration": False,
+    "set_speed_offset": 0,
+    "speed_limit_controller": True,
+  }
+  defaults.update(overrides)
+  return SimpleNamespace(**defaults)
+
+
+def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0):
+  return SimpleNamespace(status=status, dRel=d_rel, vLead=v_lead, aLeadK=a_lead_k)
+
+
+def make_sm(*, set_speed_kph=100.0, v_target=0.0, slc_target=0.0, lead_one=None, lead_two=None,
+            standstill=False, force_decel=False, red_light=False, forcing_stop=False,
+            disable_throttle=False, eco_gear=False, sport_gear=False, force_coast=False,
+            traffic_mode=False):
+  return {
+    "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill),
+    "controlsState": SimpleNamespace(forceDecel=force_decel),
+    "radarState": SimpleNamespace(
+      leadOne=lead_one or make_lead(),
+      leadTwo=lead_two or make_lead(),
+    ),
+    "starpilotCarState": SimpleNamespace(
+      ecoGear=eco_gear,
+      sportGear=sport_gear,
+      forceCoast=force_coast,
+      trafficModeEnabled=traffic_mode,
+    ),
+    "starpilotPlan": SimpleNamespace(
+      vCruise=v_target,
+      slcSpeedLimit=slc_target,
+      redLight=red_light,
+      forcingStop=forcing_stop,
+      disableThrottle=disable_throttle,
+    ),
+  }
+
+
+def test_slc_coast_window_prefers_coast_for_small_overspeed():
+  accel = StarPilotAcceleration(FakePlanner())
+  target = 56.0 * CV.MPH_TO_MS
+  sm = make_sm(set_speed_kph=100.0, v_target=target, slc_target=target)
+
+  accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"]))
+
+  assert accel.min_accel == pytest.approx(-0.02, abs=1e-3)
+
+
+def test_slc_coast_window_scales_by_profile_strength():
+  v_ego = 65.0 * CV.MPH_TO_MS
+  v_target = 60.0 * CV.MPH_TO_MS
+
+  eco = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["ECO"], A_CRUISE_MIN_ECO)
+  standard = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["STANDARD"], A_CRUISE_MIN)
+  sport = get_slc_shaped_min_accel(v_ego, v_target, DECELERATION_PROFILES["SPORT"], A_CRUISE_MIN * 2)
+
+  assert eco > standard > sport
+
+
+def test_slc_coast_window_disabled_for_relevant_lead():
+  accel = StarPilotAcceleration(FakePlanner())
+  target = 56.0 * CV.MPH_TO_MS
+  sm = make_sm(
+    set_speed_kph=100.0,
+    v_target=target,
+    slc_target=target,
+    lead_one=make_lead(status=True, d_rel=20.0, v_lead=45.0 * CV.MPH_TO_MS, a_lead_k=0.0),
+  )
+
+  accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"]))
+
+  assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
+
+
+def test_slc_coast_window_disabled_when_target_drop_is_not_slc():
+  accel = StarPilotAcceleration(FakePlanner())
+  slc_target = 60.0 * CV.MPH_TO_MS
+  sm = make_sm(
+    set_speed_kph=100.0,
+    v_target=55.0 * CV.MPH_TO_MS,
+    slc_target=slc_target,
+  )
+
+  accel.update(57.0 * CV.MPH_TO_MS, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"]))
+
+  assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)

starpilot/controls/lib/conditional_experimental_mode.py

@@ -45,6 +45,9 @@ class ConditionalExperimentalMode:
   STOP_LIGHT_OFF_MARGIN = 4.0
   STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0
   STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0
+  STOP_APPROACH_LATCH_TIME = 1.0
+  STOP_APPROACH_MAX_LEAD_SPEED = 4.5
+  STOP_APPROACH_MIN_MODEL_PROB = 0.9
 
   # ===== END TUNING PARAMETERS =====
 
@@ -80,6 +83,7 @@ class ConditionalExperimentalMode:
     self.experimental_mode = False
     self.stop_light_detected = False
     self.stop_light_model_detected = False
+    self.stop_approach_hold_until = 0.0
     self.prev_experimental_mode = False  # For hysteresis
     self.mode_hold_until = 0.0
     self.mode_false_since = 0.0
@@ -170,6 +174,8 @@ class ConditionalExperimentalMode:
       self.slow_lead_detected = False
 
   def stop_sign_and_light(self, v_ego, sm, model_time):
+    now = time.monotonic()
+
     # While the dashboard has confirmed a stop sign on this approach, pin CEM in EXP.
     # Approaches routinely exceed the mode_hold_until/mode_false_since hysteresis (0.5s/0.25s),
     # so without this the model briefly losing the sign drops CEM to CHILL and stalls the
@@ -237,12 +243,25 @@ class ConditionalExperimentalMode:
       lead = getattr(self.starpilot_planner, "lead_one", None)
       lead_distance = float(getattr(lead, "dRel", float("inf")))
       lead_speed = float(getattr(lead, "vLead", float("inf")))
+      lead_radar = bool(getattr(lead, "radar", False))
+      lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
       lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN
+      vision_stop_approach = (
+        lead_relevant and
+        not lead_radar and
+        lead_prob >= self.STOP_APPROACH_MIN_MODEL_PROB and
+        lead_speed < self.STOP_APPROACH_MAX_LEAD_SPEED
+      )
+      if (self.stop_light_detected or self.stop_light_model_detected) and vision_stop_approach:
+        self.stop_approach_hold_until = now + self.STOP_APPROACH_LATCH_TIME
+      stop_approach_latched = now < self.stop_approach_hold_until and vision_stop_approach
       handoff_to_stopped_lead = (
-        self.stop_light_detected and
         lead_relevant and
         not self.starpilot_planner.tracking_lead and
-        lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED
+        (
+          (self.stop_light_detected and lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED) or
+          stop_approach_latched
+        )
       )
       if handoff_to_stopped_lead:
         lead_cleared = True
@@ -258,3 +277,4 @@ class ConditionalExperimentalMode:
       self.stop_light_detected = False
       self.stop_light_model_detected = False
       self.lead_clear_filter.x = 0
+      self.stop_approach_hold_until = 0.0

starpilot/controls/lib/starpilot_acceleration.py

@@ -1,7 +1,9 @@
 #!/usr/bin/env python3
 import numpy as np
 
+from openpilot.common.constants import CV
 from openpilot.common.params import Params
+from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET
 from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel
 
 from openpilot.starpilot.common.accel_profile import (
@@ -11,6 +13,7 @@ from openpilot.starpilot.common.accel_profile import (
   get_accel_profile_curve_values,
   get_max_allowed_accel as get_profile_max_allowed_accel,
   interpolate_accel_profile,
+  normalize_deceleration_profile,
 )
 from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT
 
@@ -53,6 +56,24 @@ def akima_interp(x, xp, fp):
 
 A_CRUISE_MIN_ECO = A_CRUISE_MIN / 2
 A_CRUISE_MIN_SPORT = A_CRUISE_MIN * 2
+SLC_COAST_WINDOW_BP = [0.0, 10.0, 20.0, 35.0]
+SLC_COAST_WINDOW_BASE = [0.20, 0.40, 0.65, 1.10]
+SLC_EXCESS_SCALE_BP = [0.0, 10.0, 20.0, 35.0]
+SLC_EXCESS_SCALE_V = [0.8, 1.8, 3.5, 5.5]
+SLC_COAST_WINDOW_MULTIPLIER = {
+  DECELERATION_PROFILES["ECO"]: 1.20,
+  DECELERATION_PROFILES["STANDARD"]: 1.00,
+  DECELERATION_PROFILES["SPORT"]: 0.75,
+}
+SLC_COAST_FLOOR = {
+  DECELERATION_PROFILES["ECO"]: -0.02,
+  DECELERATION_PROFILES["STANDARD"]: -0.03,
+  DECELERATION_PROFILES["SPORT"]: -0.04,
+}
+SLC_COAST_MIN_SPEED = 4.0
+SLC_TARGET_EPS = 0.15
+RELEVANT_LEAD_MIN_CLOSING_SPEED = 0.5
+RELEVANT_LEAD_MIN_BRAKE = -0.4
 
 def get_max_accel_eco(v_ego, ev_tuning=True, truck_tuning=False):
   return interpolate_accel_profile(v_ego, get_accel_profile_curve_values(ACCELERATION_PROFILES["ECO"], ev_tuning, truck_tuning))
@@ -78,6 +99,41 @@ def get_max_accel_ramp_off(max_accel, v_cruise, v_ego):
 def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False):
   return float(get_profile_max_allowed_accel(v_ego, ev_tuning, truck_tuning))
 
+def get_profile_min_accel_floor(deceleration_profile):
+  if deceleration_profile == DECELERATION_PROFILES["ECO"]:
+    return A_CRUISE_MIN_ECO
+  if deceleration_profile == DECELERATION_PROFILES["SPORT"]:
+    return A_CRUISE_MIN_SPORT
+  return A_CRUISE_MIN
+
+def lead_is_braking_relevant(lead, v_ego):
+  if lead is None or not getattr(lead, "status", False):
+    return False
+
+  closing_speed = float(v_ego - getattr(lead, "vLead", 0.0))
+  if closing_speed > RELEVANT_LEAD_MIN_CLOSING_SPEED:
+    return True
+
+  if float(getattr(lead, "aLeadK", 0.0)) < RELEVANT_LEAD_MIN_BRAKE:
+    return True
+
+  return float(getattr(lead, "dRel", 1e6)) < max(18.0, 2.0 * float(v_ego))
+
+def get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, full_brake_floor):
+  profile = DECELERATION_PROFILES["STANDARD"] if deceleration_profile is None else deceleration_profile
+  coast_floor = SLC_COAST_FLOOR.get(profile, SLC_COAST_FLOOR[DECELERATION_PROFILES["STANDARD"]])
+  coast_window = float(akima_interp(v_ego, SLC_COAST_WINDOW_BP, SLC_COAST_WINDOW_BASE))
+  coast_window *= SLC_COAST_WINDOW_MULTIPLIER.get(profile, 1.0)
+  excess_scale = float(akima_interp(v_ego, SLC_EXCESS_SCALE_BP, SLC_EXCESS_SCALE_V))
+  excess_scale = max(excess_scale, coast_window + 0.1)
+
+  excess = max(0.0, float(v_ego) - float(v_target))
+  if excess <= coast_window:
+    return coast_floor
+
+  t = float(np.clip((excess - coast_window) / (excess_scale - coast_window), 0.0, 1.0)) ** 2
+  return coast_floor + t * (full_brake_floor - coast_floor)
+
 class StarPilotAcceleration:
   def __init__(self, StarPilotPlanner):
     self.starpilot_planner = StarPilotPlanner
@@ -95,6 +151,9 @@ class StarPilotAcceleration:
     truck_tuning = getattr(starpilot_toggles, "truck_tuning", False)
     custom_accel_profile = getattr(starpilot_toggles, "custom_accel_profile", False)
     custom_accel_profile_values = getattr(starpilot_toggles, "custom_accel_profile_values", [])
+    deceleration_profile = normalize_deceleration_profile(
+      getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"])
+    )
 
     if custom_accel_profile:
       self.max_accel = get_max_accel_custom(v_ego, custom_accel_profile_values, starpilot_toggles.acceleration_profile, ev_tuning, truck_tuning)
@@ -130,12 +189,31 @@ class StarPilotAcceleration:
       else:
         self.min_accel = A_CRUISE_MIN_SPORT
     else:
-      if starpilot_toggles.deceleration_profile == DECELERATION_PROFILES["ECO"]:
-        self.min_accel = A_CRUISE_MIN_ECO
-      elif starpilot_toggles.deceleration_profile == DECELERATION_PROFILES["SPORT"]:
-        self.min_accel = A_CRUISE_MIN_SPORT
-      else:
-        self.min_accel = A_CRUISE_MIN
+      self.min_accel = get_profile_min_accel_floor(deceleration_profile)
+
+      raw_v_cruise_kph = 0.0 if sm["carState"].vCruise == V_CRUISE_UNSET else min(sm["carState"].vCruise, V_CRUISE_MAX)
+      if 0 < raw_v_cruise_kph < V_CRUISE_UNSET and getattr(starpilot_toggles, "set_speed_offset", 0) > 0:
+        raw_v_cruise_kph += starpilot_toggles.set_speed_offset
+      raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS
+
+      v_target = float(getattr(sm["starpilotPlan"], "vCruise", raw_v_cruise))
+      slc_target = float(getattr(sm["starpilotPlan"], "slcSpeedLimit", 0.0))
+      slc_limited = slc_target > 0.0 and abs(v_target - slc_target) <= SLC_TARGET_EPS and v_target < raw_v_cruise - SLC_TARGET_EPS
+      has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo))
+      stop_context = (
+        sm["carState"].standstill or
+        getattr(sm["controlsState"], "forceDecel", False) or
+        getattr(sm["starpilotPlan"], "redLight", False) or
+        getattr(sm["starpilotPlan"], "forcingStop", False) or
+        getattr(sm["starpilotPlan"], "disableThrottle", False)
+      )
+      if (getattr(starpilot_toggles, "speed_limit_controller", False) and
+          v_ego > SLC_COAST_MIN_SPEED and
+          v_ego > v_target + 0.05 and
+          slc_limited and
+          not has_relevant_lead and
+          not stop_context):
+        self.min_accel = get_slc_shaped_min_accel(v_ego, v_target, deceleration_profile, self.min_accel)
 
     # Sync AccelerationProfile and DecelerationProfile params so the UI reflects the active drive mode
     # Eco → Eco, Normal → Standard, Sport → Sport+