From 335d6934fb6d79af20db2ef6cfbe291384469aeb Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Tue, 16 Jun 2026 18:31:36 -0500
Subject: [PATCH] plimothy

---
 selfdrive/controls/lib/longitudinal_planner.py  | 15 +++++++++++++++
 .../controls/tests/test_longitudinal_planner.py | 17 +++++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 249bd1959..87c53a822 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -241,6 +241,9 @@ CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.9
 CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET = 1.15
 CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MIN_CLOSING_SPEED = 1.5
 CRUISE_TRACKED_LEAD_ACCEL_CAP_UNRESOLVED_MAX_LEAD_DELTA = 0.25
+CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET = 0.95
+CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED = 0.8
+CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE = 0.10
 CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_ACCEL = 0.18
 CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN = 0.25
 CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA = 0.35
@@ -1379,6 +1382,18 @@ class LongitudinalPlanner:
     if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead:
       return None
 
+    # Don't let a spacious, nearly pace-matched tracked lead toggle this cap on
+    # and off while cruise remains the source. That creates the square-wave
+    # accel "surge / give up / surge" behavior seen in real logs.
+    actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
+    headway_margin = actual_headway - float(t_follow)
+    tracking_only_follow = tracking_lead_active and not raw_close_lead and not unresolved_slow_lead
+    if (tracking_only_follow and
+        headway_margin > CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET and
+        closing_speed < CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED and
+        lead_brake <= CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE):
+      return None
+
     desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
     gap_error = float(lead.dRel) - desired_gap
     gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN,
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index 7b2af9516..14ca2ba1a 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/selfdrive/controls/tests/test_longitudinal_planner.py
@@ -2238,6 +2238,23 @@ def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead():
   assert cap is None
 
 
+def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow():
+  v_ego = 18.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=45.0, v_lead=17.4, a_lead=0.0, radar=True, model_prob=1.0, y_rel=0.1)
+
+  cap = planner.get_cruise_tracking_lead_accel_cap(
+    lead,
+    v_ego,
+    1.45,
+    current_source="cruise",
+    tracking_lead_active=True,
+  )
+
+  assert cap is None
+
+
 def test_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead():
   v_ego = 19.03
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)