Lead catchup

selfdrive/controls/lib/lead_behavior.py

@@ -5,13 +5,16 @@ from openpilot.common.constants import CV
 HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS
 
 
-def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float) -> float:
+def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_gap: float, closing_speed: float,
+                                  v_cruise: float | None = None) -> float:
   gap_error = lead_distance - desired_gap
   actual_hw = lead_distance / max(v_ego, 1e-3)
   desired_hw = desired_gap / max(v_ego, 1e-3)
 
   if v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED:
     return 0.0
+  if v_cruise is not None and v_ego >= v_cruise:
+    return 0.0
   if gap_error <= 0.0:
     return 0.0
 

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -553,7 +553,7 @@ class LongitudinalMpc:
       if tracking_lead and lead_one.status:
         desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
         closing_speed = max(0.0, v_ego - lead_one.vLead)
-        cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed)
+        cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed, v_cruise=v_cruise)
       x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
       self.source = SOURCES[np.argmin(x_obstacles[0])]
 

selfdrive/controls/tests/test_lead_behavior.py

@@ -24,6 +24,11 @@ def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap():
   assert bias > 14.0
 
 
+def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed():
+  bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1, v_cruise=31.4)
+  assert bias == 0.0
+
+
 def test_disable_far_lead_throttle_rejects_two_second_plus_gap():
   should_disable = should_disable_far_lead_throttle(31.4, 78.7, 38.0, 0.1, False)
   assert not should_disable