From 322364c1ca44274859df57a0d8c3f200bb0f21cc Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 15 Jun 2026 14:47:21 -0500 Subject: [PATCH] plan stan --- .../lib/longitudinal_mpc_lib/long_mpc.py | 13 +++++-- .../controls/lib/longitudinal_planner.py | 13 +++++++ .../tests/test_longitudinal_planner.py | 34 +++++++++++++++++++ 3 files changed, 58 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 761af133c6..be8ef3acb6 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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: diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 87c356521b..b3a99eb6cd 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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], diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 3c20c231c8..022972cbf0 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/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)