From 8ddf2b475703398f8c6c29d21aff269e7ce9e17f Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sun, 3 May 2026 12:32:55 -0500
Subject: [PATCH] longplan

---
 .../controls/lib/longitudinal_planner.py      |  4 +-
 .../tests/test_longitudinal_planner.py        | 55 +++++++++++++++++++
 2 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 48949def0..02103e3fd 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -51,10 +51,10 @@ VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_RATIO = 0.16
 VISION_UNTRACKED_SLOW_LEAD_FULL_CLOSING_RATIO = 0.24
 VISION_UNTRACKED_SLOW_LEAD_TRIGGER_TTC = 16.0
 VISION_UNTRACKED_SLOW_LEAD_FULL_TTC = 8.0
-VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.25
+VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE_TIME = 4.4
 VISION_UNTRACKED_SLOW_LEAD_MIN_DISTANCE = 80.0
 VISION_UNTRACKED_SLOW_LEAD_MAX_DISTANCE = 120.0
-VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.7
+VISION_UNTRACKED_SLOW_LEAD_MAX_DECEL = 0.85
 VISION_UNTRACKED_SLOW_LEAD_MIN_DECEL = 0.1
 VISION_SLOW_LEAD_MAX_SPEED = 5.0
 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index 0584fae26..1f196580e 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/selfdrive/controls/tests/test_longitudinal_planner.py
@@ -254,6 +254,20 @@ def test_vision_untracked_slow_lead_cap_triggers_only_for_meaningful_closing_cas
   assert far_cap is None
 
 
+def test_vision_untracked_slow_lead_cap_reaches_high_confidence_far_slower_lead_before_raw_close_lead():
+  v_ego = 21.48
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  route_like_lead = make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935)
+
+  route_cap = planner.get_vision_untracked_slow_lead_cap(route_like_lead, v_ego, -1.0)
+
+  assert route_cap is not None
+  assert route_cap < -0.5
+  assert not planner.raw_close_lead_needs_control(route_like_lead, v_ego)
+
+
 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)
@@ -422,6 +436,47 @@ def test_acc_mode_pretracking_vision_slow_lead_blocks_positive_catchup(model_ver
   assert planner_with_lead.output_a_target < -0.2
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
+def test_acc_mode_pretracking_vision_far_slower_lead_starts_braking_before_tracking(model_version):
+  v_ego = 21.48
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner_no_lead = LongitudinalPlanner(CP, init_v=v_ego)
+  planner_with_lead = LongitudinalPlanner(CP, init_v=v_ego)
+  sm_no_lead = make_sm(
+    v_ego,
+    desired_accel=0.2,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=False,
+  )
+  sm_with_lead = make_sm(
+    v_ego,
+    desired_accel=0.2,
+    min_accel=-1.0,
+    experimental_mode=False,
+    tracking_lead=False,
+    lead_one=make_lead(status=True, d_rel=93.0, v_lead=12.84, a_lead=0.0, radar=False, model_prob=0.935),
+  )
+  sm_no_lead["starpilotPlan"].vCruise = v_ego + 6.0
+  sm_with_lead["starpilotPlan"].vCruise = v_ego + 6.0
+
+  no_lead_outputs = []
+  lead_outputs = []
+  for _ in range(8):
+    planner_no_lead.update(sm_no_lead, make_toggles(model_version))
+    planner_with_lead.update(sm_with_lead, make_toggles(model_version))
+    no_lead_outputs.append(planner_no_lead.output_a_target)
+    lead_outputs.append(planner_with_lead.output_a_target)
+
+  assert planner_with_lead.mode == "acc"
+  assert not planner_with_lead.raw_close_lead_needs_control(sm_with_lead["radarState"].leadOne, v_ego)
+  assert all(lead_output <= no_lead_output + 1e-6
+             for lead_output, no_lead_output in zip(lead_outputs[5:], no_lead_outputs[5:]))
+  assert min(lead_outputs[5:]) < min(no_lead_outputs[5:]) - 0.08
+  assert lead_outputs[-1] < no_lead_outputs[-1] - 0.15
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13"])
 def test_acc_mode_tracked_pace_matched_lead_caps_positive_catchup(model_version):
   v_ego = 28.7