Longfixes

selfdrive/controls/lib/longitudinal_planner.py

@@ -100,16 +100,16 @@ VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA = 0.24
 VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED = 20.0
 VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0
 VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35
-VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5
-VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75
+VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 6.5
+VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 3.25
 VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9
 VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35
-VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.2
+VISION_LOW_SPEED_STOP_BUFFER_BASE = 3.8
 VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80
 VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25
 VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.9
 VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.8
-VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.1
+VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25
 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
 LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0
 LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5

selfdrive/controls/tests/test_conditional_experimental_mode.py

@@ -286,6 +286,40 @@ def test_stopped_lead_handoff_does_not_hold_cem_on_empty_road(monkeypatch):
   assert not cem.stop_light_detected
 
 
+def test_stopped_lead_toggle_does_not_self_trigger_without_a_real_lead():
+  v_ego = 25 * CV.MPH_TO_MS
+  cem = make_cem(
+    model_length=v_ego * 6.0,
+    tracking_lead=False,
+    lead_status=False,
+    lead_v_lead=0.0,
+  )
+  toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True)
+
+  for _ in range(24):
+    cem.slow_lead(toggles, v_ego)
+
+  assert not cem.slow_lead_detected
+
+
+def test_stopped_lead_toggle_still_triggers_for_real_close_stopped_lead():
+  v_ego = 25 * CV.MPH_TO_MS
+  cem = make_cem(
+    model_length=v_ego * 4.0,
+    tracking_lead=True,
+    lead_status=True,
+    lead_d_rel=18.0,
+    lead_v_lead=0.4,
+    lead_model_prob=0.99,
+  )
+  toggles = SimpleNamespace(conditional_slower_lead=False, conditional_stopped_lead=True)
+
+  for _ in range(24):
+    cem.slow_lead(toggles, v_ego)
+
+  assert cem.slow_lead_detected
+
+
 def test_borderline_empty_road_model_dip_does_not_refresh_long_hold(monkeypatch):
   v_ego = 20 * CV.MPH_TO_MS
   stop_threshold = v_ego * 7.0

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -758,6 +758,29 @@ def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(
   assert planner.output_a_target < -1.0
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_vision_lead(model_version):
+  v_ego = 6.2
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.1,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=9.35, v_lead=2.8, a_lead=-0.2, radar=False, model_prob=0.99),
+  )
+  sm["starpilotPlan"].vCruise = v_ego + 4.0
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.mode == "acc"
+  assert planner.output_should_stop
+  assert planner.output_a_target <= -2.7
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
 def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_version):
   v_ego = 0.0

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -1,7 +1,57 @@
 import pytest
 
 from openpilot.common.constants import CV
-from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target
+from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise, get_active_slc_control_target
+from types import SimpleNamespace
+
+
+class FakeParams:
+  def get(self, *args, **kwargs):
+    return None
+
+  def get_float(self, *args, **kwargs):
+    return 0.0
+
+  def put_nonblocking(self, *args, **kwargs):
+    pass
+
+
+def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False):
+  planner = SimpleNamespace(
+    params=FakeParams(),
+    params_memory=FakeParams(),
+    lead_one=SimpleNamespace(status=False, dRel=float("inf"), vLead=0.0),
+    starpilot_cem=SimpleNamespace(stop_light_detected=red_light),
+    tracking_lead=False,
+    driving_in_curve=False,
+    model_length=60.0,
+    raw_model_stopped=raw_model_stopped,
+    road_curvature_detected=False,
+  )
+  vcruise = StarPilotVCruise(planner)
+  vcruise.forcing_stop = forcing_stop
+  vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0
+  vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length
+  return planner, vcruise
+
+
+def make_sm(*, standstill=True):
+  return {
+    "carControl": SimpleNamespace(longActive=True),
+    "carState": SimpleNamespace(standstill=standstill, gasPressed=False, vCruiseCluster=0.0, vEgoCluster=0.0),
+    "starpilotCarState": SimpleNamespace(accelPressed=False, dashboardStopSign=0, dashboardSpeedLimit=0),
+  }
+
+
+def make_toggles():
+  return SimpleNamespace(
+    force_stops=True,
+    force_standstill=False,
+    curve_speed_controller=False,
+    speed_limit_controller=False,
+    show_speed_limits=False,
+    force_stop_distance_offset=0,
+  )
 
 
 def test_active_slc_control_target_does_not_require_set_speed_limit():
@@ -28,3 +78,40 @@ def test_active_slc_control_target_applies_offset_and_cluster_diff():
   )
 
   assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4)
+
+
+def test_force_stop_clears_at_standstill_once_scene_opens():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True)
+
+  result = vcruise.update(
+    controls_enabled=True,
+    now=0.0,
+    time_validated=True,
+    v_cruise=20.0,
+    v_ego=0.0,
+    sm=make_sm(standstill=True),
+    starpilot_toggles=make_toggles(),
+  )
+
+  assert result == pytest.approx(20.0)
+  assert vcruise.force_stop_timer == 0.0
+  assert not vcruise.forcing_stop
+  assert vcruise.tracked_model_length == pytest.approx(planner.model_length)
+
+
+def test_force_stop_stays_committed_while_model_still_sees_stop():
+  planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=True, forcing_stop=True)
+
+  result = vcruise.update(
+    controls_enabled=True,
+    now=0.0,
+    time_validated=True,
+    v_cruise=20.0,
+    v_ego=0.0,
+    sm=make_sm(standstill=True),
+    starpilot_toggles=make_toggles(),
+  )
+
+  assert result == pytest.approx(0.0)
+  assert vcruise.force_stop_timer >= 0.5
+  assert vcruise.forcing_stop

starpilot/controls/lib/conditional_experimental_mode.py

@@ -239,7 +239,12 @@ class ConditionalExperimentalMode:
       return
 
     slower_lead = starpilot_toggles.conditional_slower_lead and self.starpilot_planner.starpilot_following.slower_lead
-    stopped_lead = starpilot_toggles.conditional_stopped_lead and lead_speed < 1
+    stopped_lead = bool(
+      starpilot_toggles.conditional_stopped_lead and
+      lead_status and
+      lead_speed < 1 and
+      lead_distance < max(40.0, v_ego * self.SLOW_LEAD_CONTINUITY_MAX_DISTANCE_TIME)
+    )
     vision_slow_lead_candidate = bool(
       lead_status and
       lead_prob >= self.SLOW_LEAD_CONTINUITY_MIN_MODEL_PROB and

starpilot/controls/lib/starpilot_vcruise.py

@@ -103,10 +103,15 @@ class StarPilotVCruise:
     if dash_path:
       self.stop_sign_confirmed = True
 
+    raw_model_stopped = bool(getattr(self.starpilot_planner, "raw_model_stopped", False))
+
     # Timer ramp. Faster commitment when the dashboard confirms.
     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 (self.forcing_stop and sm["carState"].standstill and not dash_active and
+          not self.starpilot_planner.starpilot_cem.stop_light_detected and not raw_model_stopped):
+      self.force_stop_timer = 0.0
     else:
       self.force_stop_timer = max(self.force_stop_timer - DT_MDL * 0.25, 0.0)
 

starpilot/controls/starpilot_planner.py

@@ -56,6 +56,7 @@ class StarPilotPlanner:
     self.gps_valid = False
     self.lateral_check = False
     self.model_stopped = False
+    self.raw_model_stopped = False
     self.road_curvature_detected = False
     self.tracking_lead = False
     self._prev_gps_bearing = 0
@@ -129,8 +130,8 @@ class StarPilotPlanner:
 
     self.model_length = sm["modelV2"].position.x[-1]
 
-    self.model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
-    self.model_stopped |= self.starpilot_vcruise.forcing_stop
+    self.raw_model_stopped = self.model_length < CRUISING_SPEED * PLANNER_TIME
+    self.model_stopped = self.raw_model_stopped or self.starpilot_vcruise.forcing_stop
 
     self.road_curvature, self.time_to_curve = calculate_road_curvature(sm["modelV2"], v_ego)