diff --git a/selfdrive/controls/lib/lead_behavior.py b/selfdrive/controls/lib/lead_behavior.py index 7068a10e0..f345f7d45 100644 --- a/selfdrive/controls/lib/lead_behavior.py +++ b/selfdrive/controls/lib/lead_behavior.py @@ -34,6 +34,7 @@ RADARLESS_MATCHED_FOLLOW_HEADWAY_BELOW_TARGET = 0.35 RADARLESS_MATCHED_FOLLOW_HEADWAY_ABOVE_TARGET = 0.90 RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35 RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70 +FAR_LEAD_COAST_MIN_GAP = 3.5 def _smoothstep(value: float, start: float, end: float) -> float: @@ -173,4 +174,5 @@ def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired gentle_closing = 0.35 < closing_speed < max(1.35, 0.05 * 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 > 7.5 and lead_distance > desired_gap + 7.0 + return (coast_window_open and coast_window_far and gentle_closing and ttc > 7.5 and + lead_distance > desired_gap + FAR_LEAD_COAST_MIN_GAP) diff --git a/selfdrive/controls/tests/test_lead_behavior.py b/selfdrive/controls/tests/test_lead_behavior.py index 2df853e93..088a2d0c2 100644 --- a/selfdrive/controls/tests/test_lead_behavior.py +++ b/selfdrive/controls/tests/test_lead_behavior.py @@ -74,6 +74,11 @@ def test_disable_far_lead_throttle_keeps_mild_coast_near_target_gap(): assert should_disable +def test_disable_far_lead_throttle_waits_until_reduced_gap(): + should_disable = should_disable_far_lead_throttle(31.4, 43.5, 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