longy long long

selfdrive/controls/lib/longcontrol.py

@@ -13,6 +13,7 @@ clip = np.clip
 interp = np.interp
 STOPPING_RELEASE_HYSTERESIS = 0.35
 STOPPING_RELEASE_MIN_ACCEL = 0.15
+STOPPING_RELEASE_STRONG_ACCEL = 0.45
 MOVING_STOP_FOLLOW_MIN_GAP = 0.25
 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35
 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40
@@ -184,6 +185,10 @@ class LongControl:
       self.stop_release_counter = int(round(STOPPING_RELEASE_HYSTERESIS / DT_CTRL))
       return True
 
+    if a_target >= STOPPING_RELEASE_STRONG_ACCEL and not CS.cruiseState.standstill:
+      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)

selfdrive/controls/lib/longitudinal_planner.py

@@ -46,6 +46,11 @@ STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5
 STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6
 STANDSTILL_LEAD_DEPART_MIN_GAP_MARGIN = 0.8
 STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL = 0.08
+STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL = 0.18
+STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25
+STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.12
+STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 1.4
+STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME = 0.30
 LEAD_DEPART_CONFIDENT_MIN_GAP = 3.75
 LEAD_DEPART_CONFIDENT_MAX_GAP = 5.25
 LEAD_DEPART_CONFIDENT_MIN_LEAD_SPEED = 0.3
@@ -562,6 +567,7 @@ class LongitudinalPlanner:
     self.output_a_target = 0.0
     self.output_should_stop = False
     self.confident_lead_depart_elapsed = 0.0
+    self.slow_creep_lead_depart_elapsed = 0.0
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -1096,6 +1102,28 @@ class LongitudinalPlanner:
       lead_accel >= LEAD_DEPART_CONFIDENT_MIN_LEAD_ACCEL
     )
 
+  def is_slow_creep_lead_depart(self, lead, v_ego, standstill_nudge_gap):
+    if lead is None or not lead.status:
+      return False
+
+    lead_radar = bool(getattr(lead, "radar", False))
+    lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
+    if not lead_radar and lead_prob < STANDSTILL_STOPPED_LEAD_GUARD_MIN_MODEL_PROB:
+      return False
+
+    if abs(float(getattr(lead, "yRel", 0.0))) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_LATERAL_OFFSET:
+      return False
+
+    lead_gap = float(getattr(lead, "dRel", 0.0))
+    lead_speed = max(float(getattr(lead, "vLead", 0.0)), 0.0)
+    lead_accel = float(getattr(lead, "aLeadK", 0.0))
+    return bool(
+      float(v_ego) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED and
+      lead_gap >= standstill_nudge_gap + STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN and
+      lead_speed >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED and
+      lead_accel >= STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL
+    )
+
   @staticmethod
   def get_centered_model_lead(model_data):
     try:
@@ -2646,11 +2674,33 @@ class LongitudinalPlanner:
       confident_depart_detected and
       self.confident_lead_depart_elapsed >= LEAD_DEPART_CONFIDENT_CONFIRM_TIME
     )
+    slow_creep_depart_detected = any(
+      self.is_slow_creep_lead_depart(lead, float(sm['carState'].vEgo), standstill_nudge_gap)
+      for lead in (self.lead_one, self.lead_two)
+    )
+    if (
+      lead_control_active and
+      sm['carState'].standstill and
+      not depart_safety_veto and
+      not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
+      not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
+      slow_creep_depart_detected
+    ):
+      self.slow_creep_lead_depart_elapsed = min(
+        STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME,
+        self.slow_creep_lead_depart_elapsed + self.dt,
+      )
+    else:
+      self.slow_creep_lead_depart_elapsed = 0.0
+    slow_creep_depart_ready = (
+      slow_creep_depart_detected and
+      self.slow_creep_lead_depart_elapsed >= STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME
+    )
 
     standstill_stopped_lead_guard_cap = None
     standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two))
     if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED):
-      release_ready = bool(lead_depart_ready or confident_depart_ready)
+      release_ready = bool(lead_depart_ready or confident_depart_ready or slow_creep_depart_ready)
       standstill_stopped_lead_guard_caps = [
         cap for cap in (
           self.get_standstill_stopped_lead_guard_cap(
@@ -2681,15 +2731,18 @@ class LongitudinalPlanner:
     if (
       lead_control_active and
       sm['carState'].standstill and
-      (confident_depart_ready or lead_depart_ready) and
+      (confident_depart_ready or lead_depart_ready or slow_creep_depart_ready) and
       not depart_safety_veto and
       not bool(getattr(sm['starpilotPlan'], 'forcingStop', False)) and
       not bool(getattr(sm['starpilotPlan'], 'redLight', False)) and
-      (confident_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL)
+      (confident_depart_ready or slow_creep_depart_ready or model_desired_accel >= STANDSTILL_LEAD_DEPART_MIN_MODEL_ACCEL)
     ):
       vision_low_speed_stop_active = False
       output_should_stop = False
-      output_a_target = max(output_a_target, STANDSTILL_LEAD_DEPART_MIN_ACCEL)
+      depart_min_accel = STANDSTILL_LEAD_DEPART_MIN_ACCEL
+      if slow_creep_depart_ready and not (confident_depart_ready or lead_depart_ready):
+        depart_min_accel = STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL
+      output_a_target = max(output_a_target, depart_min_accel)
       self.post_departure_follow_settle_until = now_t + POST_DEPARTURE_FOLLOW_SETTLE_LATCH_TIME
 
     if lead_control_active and lead_depart_ready and not depart_safety_veto and not output_should_stop and float(sm['carState'].vEgo) <= STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED:

selfdrive/controls/tests/test_longcontrol.py

@@ -151,7 +151,7 @@ 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():
+def test_update_requires_sustained_moderate_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]
@@ -168,7 +168,7 @@ def test_update_requires_sustained_positive_target_to_leave_stopping():
     output_accel = lc.update(
       active=True,
       CS=CS,
-      a_target=0.5,
+      a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01,
       should_stop=False,
       accel_limits=(-3.0, 2.0),
       starpilot_toggles=make_toggles(startAccel=1.5),
@@ -179,7 +179,7 @@ def test_update_requires_sustained_positive_target_to_leave_stopping():
   lc.update(
     active=True,
     CS=CS,
-    a_target=0.5,
+    a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL - 0.01,
     should_stop=False,
     accel_limits=(-3.0, 2.0),
     starpilot_toggles=make_toggles(startAccel=1.5),
@@ -188,6 +188,31 @@ def test_update_requires_sustained_positive_target_to_leave_stopping():
   assert lc.long_control_state == LongCtrlState.starting
 
 
+def test_update_releases_stopping_immediately_on_strong_positive_target():
+  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
+
+  output_accel = lc.update(
+    active=True,
+    CS=CS,
+    a_target=longcontrol.STOPPING_RELEASE_STRONG_ACCEL,
+    should_stop=False,
+    accel_limits=(-3.0, 2.0),
+    starpilot_toggles=make_toggles(startAccel=1.5),
+  )
+
+  assert lc.long_control_state == LongCtrlState.starting
+  assert output_accel > 0.0
+
+
 def test_update_releases_stopping_on_small_sustained_positive_target():
   CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
   CP.longitudinalTuning.kpBP = [0.0]

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -1222,6 +1222,57 @@ def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(mod
   assert planner.output_a_target <= 0.0
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_standstill_slow_creep_depart_releases_after_short_confirm(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm = make_sm(
+    0.0,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.33, a_lead=0.28, radar=True, model_prob=1.0),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+
+  frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt))
+  for _ in range(max(frames, 1)):
+    planner.update(sm, make_toggles(model_version))
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target >= longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_MIN_ACCEL
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
+def test_standstill_slow_creep_depart_does_not_release_on_gap_without_motion_signal(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.0)
+
+  sm = make_sm(
+    0.0,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=7.2, v_lead=0.20, a_lead=0.05, radar=True, model_prob=1.0),
+  )
+  sm["carState"].standstill = True
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = False
+
+  frames = int(round(longitudinal_planner_module.STANDSTILL_LEAD_CREEP_RELEASE_CONFIRM_TIME / planner.dt)) + 2
+  for _ in range(max(frames, 1)):
+    planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_should_stop
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
 def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version):
   v_ego = 0.0

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -44,7 +44,15 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
 def make_sm(*, standstill=True, min_steer_speed=0.0):
   return {
     "carControl": SimpleNamespace(longActive=True),
-    "carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0),
+    "carState": SimpleNamespace(
+      standstill=standstill,
+      gasPressed=False,
+      vCruiseCluster=0.0,
+      vEgoCluster=0.0,
+      leftBlinker=False,
+      rightBlinker=False,
+      steeringAngleDeg=0.0,
+    ),
     "carParams": SimpleNamespace(minSteerSpeed=min_steer_speed),
     "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0),
   }
@@ -155,6 +163,32 @@ def test_force_stop_stays_committed_while_moving_even_if_scene_opens():
   assert vcruise.forcing_stop
 
 
+def test_force_stop_turn_scene_veto_blocks_new_activation():
+  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
+  sm = make_sm(standstill=False)
+  sm["carState"].leftBlinker = True
+  sm["carState"].steeringAngleDeg = 15.0
+
+  result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=7.0)
+
+  assert result == pytest.approx(20.0)
+  assert vcruise.force_stop_timer == pytest.approx(0.0)
+  assert not vcruise.forcing_stop
+
+
+def test_force_stop_turn_scene_clears_moving_commitment():
+  _, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+  sm = make_sm(standstill=False)
+  sm["carState"].rightBlinker = True
+  sm["carState"].steeringAngleDeg = -15.0
+
+  result = update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=8.0)
+
+  assert result == pytest.approx(20.0)
+  assert vcruise.force_stop_timer == pytest.approx(0.0)
+  assert not vcruise.forcing_stop
+
+
 def test_engage_while_already_stopped_in_red_light_scene_seeds_force_stop_hold():
   _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False)
 

starpilot/controls/lib/starpilot_vcruise.py

@@ -25,15 +25,18 @@ NAV_TURN_TARGET_SPEEDS = {
 }
 
 # Force-stop kinematic profile. The user tunes one signed knob (ForceStopDistanceOffset,
-# in feet); positive = stop later/longer, negative = stop sooner/shorter. All other
-# shape parameters are fixed constants converged from FP-Testing Sessions A-O.
-COMFORT_DECEL = 1.0       # m/s^2 — kinematic decel ceiling
+# in feet); positive = stop later/longer, negative = stop sooner/shorter.
+# Smaller values pull speed down earlier on approach.
+FORCE_STOP_MODEL_APPROACH_DECEL = 0.8
+FORCE_STOP_DASH_APPROACH_DECEL = 1.0
 ACTIVATION_M = 75.0       # m — CEM/model path activates when model_length < this
 MPC_HANDOFF_M = 6.0       # m — below this, command 0 and let MPC finish the stop
 ADAS_MAX_MS = 17.88       # 40 mph — cross-street ADAS guard
 DASH_SEED_M = 27.0        # ~88 ft — typical ADAS detection distance, used to snap
                           # tracked length closer when dashboard confirms a sign
 FT_TO_M = 0.3048
+FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS
+FORCE_STOP_TURN_VETO_STEERING_ANGLE = 12.0
 
 # Knob bounds (mirror of UI slider; defense in depth)
 OFFSET_FT_MIN = -20
@@ -190,6 +193,11 @@ class StarPilotVCruise:
 
   def update(self, controls_enabled, now, time_validated, v_cruise, v_ego, sm, starpilot_toggles):
     long_control_active = sm["carControl"].longActive
+    turn_scene_active = bool(
+      v_ego <= FORCE_STOP_TURN_VETO_MAX_SPEED and
+      (getattr(sm["carState"], "leftBlinker", False) or getattr(sm["carState"], "rightBlinker", False)) and
+      abs(float(getattr(sm["carState"], "steeringAngleDeg", 0.0))) >= FORCE_STOP_TURN_VETO_STEERING_ANGLE
+    )
 
     # ----- Activation paths -----
     # Raw lead check: block Force Stop as soon as a relevant lead is present, without
@@ -209,6 +217,7 @@ class StarPilotVCruise:
                 and self.starpilot_planner.model_length < ACTIVATION_M
                 and self.override_force_stop_timer <= 0
                 and not self.starpilot_planner.driving_in_curve
+                and not turn_scene_active
                 and not self.starpilot_planner.tracking_lead
                 and not lead_present)
 
@@ -220,6 +229,7 @@ class StarPilotVCruise:
                  and v_ego < ADAS_MAX_MS
                  and self.override_force_stop_timer <= 0
                  and not self.starpilot_planner.driving_in_curve
+                 and not turn_scene_active
                  and not self.starpilot_planner.tracking_lead
                  and not lead_present)
 
@@ -275,6 +285,8 @@ class StarPilotVCruise:
     if force_stop_active and not sm["carState"].standstill:
       rate = DT_MDL * 2 if dash_active else DT_MDL
       self.force_stop_timer = min(self.force_stop_timer + rate, 2.0)
+    elif turn_scene_active and not sm["carState"].standstill:
+      self.force_stop_timer = 0.0
     elif self.standstill_force_stop_hold:
       self.force_stop_timer = max(self.force_stop_timer, 0.5)
     elif (self.forcing_stop and sm["carState"].standstill and not dash_active and
@@ -285,7 +297,7 @@ class StarPilotVCruise:
 
     force_stop_enabled = self.force_stop_timer >= 0.5
     # Stay committed across model dropouts until standstill
-    force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill
+    force_stop_enabled |= self.forcing_stop and not sm["carState"].standstill and not turn_scene_active
     force_stop_enabled |= self.standstill_force_stop_hold
 
     # Override: gas/accel pedal during an active force stop
@@ -379,7 +391,8 @@ class StarPilotVCruise:
         if effective_d <= MPC_HANDOFF_M:
           v_target = 0.0
         else:
-          v_target = math.sqrt(2.0 * COMFORT_DECEL * (effective_d - MPC_HANDOFF_M))
+          approach_decel = FORCE_STOP_DASH_APPROACH_DECEL if dash_active else FORCE_STOP_MODEL_APPROACH_DECEL
+          v_target = math.sqrt(2.0 * approach_decel * (effective_d - MPC_HANDOFF_M))
 
         v_cruise = min(v_target, v_cruise)