plan stan

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -621,8 +621,17 @@ class LongitudinalMpc:
       return False
     if float(v_ego) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED:
       return False
-    if bool(getattr(lead_one, "radar", False)) or bool(getattr(lead_two, "radar", False)):
-      return False
+    lead_one_radar = bool(getattr(lead_one, "radar", False))
+    lead_two_radar = bool(getattr(lead_two, "radar", False))
+    if lead_one_radar or lead_two_radar:
+      track_one = int(getattr(lead_one, "radarTrackId", -1))
+      track_two = int(getattr(lead_two, "radarTrackId", -1))
+      return (
+        lead_one_radar and lead_two_radar and
+        track_one >= 0 and track_one == track_two and
+        abs(float(lead_one.dRel) - float(lead_two.dRel)) <= NEAR_DUPLICATE_LEAD_SOURCE_MAX_DREL_DIFF and
+        abs(float(lead_one.vRel) - float(lead_two.vRel)) <= max(1.0, NEAR_DUPLICATE_LEAD_SOURCE_MAX_VREL_DIFF)
+      )
     if float(getattr(lead_one, "modelProb", 0.0)) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB:
       return False
     if float(getattr(lead_two, "modelProb", 0.0)) < NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB:

selfdrive/controls/lib/longitudinal_planner.py

@@ -237,6 +237,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_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
+CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN = 1.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_SPEED = 12.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MAX_SPEED = 22.0
 CRUISE_TRACKED_LEAD_ACCEL_TRANSITION_MIN_MODEL_PROB = 0.9
@@ -1353,6 +1356,16 @@ class LongitudinalPlanner:
     if gap_error > gap_buffer:
       return None
 
+    # If the same lead is already accelerating away and we're no longer tight to
+    # the follow target, don't slam the accel cap back on just because lead_delta
+    # momentarily falls near the pull-away threshold. That produces the repeated
+    # 0.18 m/s^2 "surge / give up / surge" behavior seen in real logs.
+    lead_accel = float(getattr(lead, "aLeadK", 0.0))
+    if (lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and
+        lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and
+        gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN):
+      return None
+
     base_cap = float(np.interp(
       lead_delta,
       [-1.5, -0.5, 0.0, 0.5, CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_PULLAWAY_SPEED],

selfdrive/controls/tests/test_longitudinal_planner.py

@@ -2208,6 +2208,23 @@ def test_cruise_tracking_lead_accel_cap_skips_when_lead_clearly_pulls_away():
   assert cap is None
 
 
+def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead():
+  v_ego = 11.4
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=20.5, v_lead=12.4, a_lead=0.46, 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_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst():
   v_ego = 16.2
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -2390,6 +2407,23 @@ def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead():
   assert hysteresis == 0.0
 
 
+def test_near_duplicate_lead_source_hysteresis_prefers_previous_source_for_identical_radar_track():
+  v_ego = 27.0
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead_one = make_lead(status=True, d_rel=34.6, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0)
+  lead_two = make_lead(status=True, d_rel=34.7, v_lead=24.2, a_lead=-0.04, radar=True, model_prob=1.0)
+  lead_one.vRel = -0.8
+  lead_two.vRel = -0.78
+  lead_one.radarTrackId = 123
+  lead_two.radarTrackId = 123
+
+  lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego)
+
+  assert lead_0_bias == 0.0
+  assert lead_1_bias > 0.0
+
+
 def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
   v_ego = 27.0
   CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)