The Domi Effect

selfdrive/controls/tests/test_starpilot_vcruise.py

@@ -26,7 +26,7 @@ class FakeParams:
     pass
 
 
-def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None):
+def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False, nav_state=None, road_curvature=0.0):
   planner = SimpleNamespace(
     params=FakeParams(),
     params_memory=FakeParams({"NavInstructionState": nav_state or {}}),
@@ -36,6 +36,7 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False
     driving_in_curve=False,
     model_length=60.0,
     raw_model_stopped=raw_model_stopped,
+    road_curvature=road_curvature,
     road_curvature_detected=False,
   )
   vcruise = StarPilotVCruise(planner)
@@ -256,6 +257,32 @@ def test_force_stop_turn_scene_veto_blocks_new_activation():
   assert not vcruise.forcing_stop
 
 
+def test_force_stop_curve_veto_blocks_new_activation():
+  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False, road_curvature=0.005)
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+
+  for frame in range(12):
+    result = update_vcruise(vcruise, sm, toggles, now=frame * 0.05, 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_still_activates_for_straight_red_light_approach():
+  _, vcruise = make_vcruise(red_light=True, raw_model_stopped=False, forcing_stop=False, road_curvature=0.001)
+  sm = make_sm(standstill=False)
+  toggles = make_toggles()
+
+  for frame in range(12):
+    result = update_vcruise(vcruise, sm, toggles, now=frame * 0.05, v_ego=7.0)
+
+  assert 0.0 < result < 20.0
+  assert vcruise.force_stop_timer >= 0.5
+  assert 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)

starpilot/controls/lib/starpilot_vcruise.py

@@ -44,6 +44,7 @@ DASH_SEED_M = 27.0        # ~88 ft — typical ADAS detection distance, used to
 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
+FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003
 
 # Knob bounds (mirror of UI slider; defense in depth)
 OFFSET_FT_MIN = -20
@@ -271,6 +272,7 @@ class StarPilotVCruise:
     lead_present = (bool(getattr(lead, "status", False))
                     and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M
                     and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0)
+    curved_approach_scene = abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
 
     # CEM/model path: model predicted stop within ACTIVATION_M.
     # Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path
@@ -281,6 +283,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 curved_approach_scene
                 and not turn_scene_active
                 and not self.starpilot_planner.tracking_lead
                 and not lead_present)