From 2e53cc45e68043a40f2b8fafe4bbd99e629c8d80 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 13 Apr 2026 18:10:10 -0500 Subject: [PATCH] Lead catchup --- selfdrive/controls/lib/lead_behavior.py | 5 ++++- selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 2 +- selfdrive/controls/tests/test_lead_behavior.py | 5 +++++ 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index aced94b46..a909cf57a 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4d5ee00a8..0bcf9e521 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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])] diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 69f5189a1..0b71f8751 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/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