mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

plimothy

Browse Source
This commit is contained in:
firestar5683
2026-06-16 18:31:36 -05:00
parent 60305edeae
commit 335d6934fb
2 changed files with 32 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+15
View File
@@ -241,6 +241,9 @@ CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_GAIN = 0.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_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET = 0.95
CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED = 0.8
CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE = 0.10
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
@@ -1379,6 +1382,18 @@ class LongitudinalPlanner:
if not tracking_lead_active and not raw_close_lead and not unresolved_slow_lead:
return None
# Don't let a spacious, nearly pace-matched tracked lead toggle this cap on
# and off while cruise remains the source. That creates the square-wave
# accel "surge / give up / surge" behavior seen in real logs.
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
headway_margin = actual_headway - float(t_follow)
tracking_only_follow = tracking_lead_active and not raw_close_lead and not unresolved_slow_lead
if (tracking_only_follow and
headway_margin > CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_HEADWAY_ABOVE_TARGET and
closing_speed < CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_CLOSING_SPEED and
lead_brake <= CRUISE_TRACKED_LEAD_ACCEL_CAP_TRACKING_ONLY_MAX_LEAD_BRAKE):
return None
desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
gap_error = float(lead.dRel) - desired_gap
gap_buffer = max(CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_GAP_BUFFER_MIN,
selfdrive/controls/tests/test_longitudinal_planner.py
+17
View File
@@ -2238,6 +2238,23 @@ def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead():
assert cap is None
def test_cruise_tracking_lead_accel_cap_skips_spacious_tracking_only_follow():
v_ego = 18.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=45.0, v_lead=17.4, a_lead=0.0, 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_route_8bc6_post_departure_cruise_cap_skips_accelerating_away_radar_lead():
v_ego = 19.03
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)