Follow that trail; The trail that we blaze

selfdrive/controls/lib/lead_behavior.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+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:
+  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 gap_error <= 0.0:
+    return 0.0
+
+  # Encourage ACC to treat a tracked lead as the active constraint when we're
+  # hanging far above the requested time gap, but don't override cruise for a
+  # truly distant lead or one we're already closing on decisively.
+  if actual_hw <= max(desired_hw + 0.35, 1.75):
+    return 0.0
+  if actual_hw >= max(desired_hw + 1.6, 3.0):
+    return 0.0
+  if closing_speed > max(2.5, 0.12 * v_ego):
+    return 0.0
+
+  return min(gap_error * 0.5, max(12.0, 0.6 * v_ego))
+
+
+def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float,
+                                     closing_speed: float, following_lead: bool) -> bool:
+  actual_hw = lead_distance / max(v_ego, 1e-3)
+  desired_hw = desired_gap / max(v_ego, 1e-3)
+
+  if following_lead or v_ego <= HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED:
+    return False
+
+  # Don't coast if we're already materially above the requested headway.
+  if actual_hw > max(desired_hw + 0.45, 1.85):
+    return False
+
+  coast_window_open = lead_distance > desired_gap + max(4.0, 0.15 * v_ego)
+  coast_window_far = lead_distance < desired_gap + max(15.0, 0.75 * v_ego)
+  gentle_closing = closing_speed < max(1.5, 0.08 * v_ego)
+  ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6
+
+  return coast_window_open and coast_window_far and gentle_closing and ttc > 6.0 and lead_distance > desired_gap + 6.0

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -13,6 +13,7 @@ from openpilot.common.constants import CV
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import DT_MDL
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.controls.lib.lead_behavior import get_tracked_lead_catchup_bias
 # WARNING: imports outside of constants will not trigger a rebuild
 from openpilot.selfdrive.modeld.constants import index_function
 
@@ -549,6 +550,10 @@ class LongitudinalMpc:
                                  v_lower,
                                  v_upper)
       cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)
+      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)
       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

@@ -0,0 +1,34 @@
+from openpilot.selfdrive.controls.lib.lead_behavior import (
+  get_tracked_lead_catchup_bias,
+  should_disable_far_lead_throttle,
+)
+
+
+def test_tracked_lead_catchup_bias_for_hanging_gap():
+  bias = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 0.1)
+  assert bias > 10.0
+
+
+def test_tracked_lead_catchup_bias_ignores_near_desired_gap():
+  bias = get_tracked_lead_catchup_bias(31.4, 50.0, 38.0, 0.1)
+  assert bias == 0.0
+
+
+def test_tracked_lead_catchup_bias_ignores_very_far_gap():
+  bias = get_tracked_lead_catchup_bias(31.4, 110.0, 38.0, 0.1)
+  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
+
+
+def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap():
+  should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 0.5, False)
+  assert should_disable
+
+
+def test_disable_far_lead_throttle_rejects_fast_closing():
+  should_disable = should_disable_far_lead_throttle(31.4, 52.0, 38.0, 3.5, False)
+  assert not should_disable

starpilot/controls/lib/starpilot_following.py

@@ -2,13 +2,13 @@
 import numpy as np
 
 from openpilot.common.constants import CV
+from openpilot.selfdrive.controls.lib.lead_behavior import should_disable_far_lead_throttle
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import COMFORT_BRAKE, LEAD_DANGER_FACTOR, desired_follow_distance, get_jerk_factor, get_T_FOLLOW
 
 from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT, MAX_T_FOLLOW
 
 TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
 PERSONALITY_BP = [45. * CV.MPH_TO_MS, 70. * CV.MPH_TO_MS]
-HIGHWAY_DISABLE_THROTTLE_MIN_SPEED = 45. * CV.MPH_TO_MS
 
 
 def get_longitudinal_personality(sm):
@@ -96,18 +96,7 @@ class StarPilotFollowing:
       v_lead = self.starpilot_planner.lead_one.vLead
       closing_speed = max(0.0, v_ego - v_lead)
       desired_gap = float(desired_follow_distance(v_ego, v_lead, self.t_follow))
-      ttc = lead_distance / max(closing_speed, 1e-3) if closing_speed > 0.1 else 1e6
-
-      # Keep a mild coasting behavior only for far/low-risk slower leads.
-      coast_window_open = lead_distance > desired_gap + max(4.0, 0.2 * v_ego)
-      coast_window_far = lead_distance < desired_gap + max(25.0, 1.2 * v_ego)
-      gentle_closing = closing_speed < max(2.0, 0.12 * v_ego)
-
-      # Highway-only coast suppression: keep low-speed lead departures responsive.
-      self.disable_throttle = (not self.following_lead and v_ego > HIGHWAY_DISABLE_THROTTLE_MIN_SPEED and coast_window_open and
-                               coast_window_far and gentle_closing)
-      # Never coast when we are entering a potentially late-braking scenario.
-      self.disable_throttle &= ttc > 6.0 and lead_distance > desired_gap + 6.0
+      self.disable_throttle = should_disable_far_lead_throttle(v_ego, lead_distance, desired_gap, closing_speed, self.following_lead)
 
     if long_control_active and self.starpilot_planner.tracking_lead:
       self.update_follow_values(self.starpilot_planner.lead_one.dRel, v_ego, self.starpilot_planner.lead_one.vLead, starpilot_toggles)