mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-29 19:03:59 +08:00
Code Issues Packages Projects Releases Wiki Activity

Dom's Plan

Browse Source
This commit is contained in:
firestar5683
2026-07-11 11:54:34 -05:00
parent a0f4028aaa
commit 6c0ee59a3e
2 changed files with 50 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lead_behavior.py
+31 -26
View File
@@ -3,20 +3,22 @@ from openpilot.common.constants import CV
HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED = 45. * CV.MPH_TO_MS
TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED = 52. * CV.MPH_TO_MS
TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL = 1.5
VISION_LEAD_TRACK_MIN_DISTANCE = 25.0
VISION_LEAD_TRACK_BASE_TIME_GAP = 1.75
VISION_LEAD_TRACK_CLOSING_GAIN = 0.20
VISION_LEAD_TRACK_CLOSING_CAP = 2.50
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.35
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.65
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN = 0.40
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN = 0.70
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN = 0.75
TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN = 1.05
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START = 2.75
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END = 3.10
TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET = 0.90
TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET = 1.60
TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.65
TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.75
TRACKED_LEAD_CATCHUP_BIAS_GAIN = 0.45
TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR = 0.55
RADARLESS_MATCHED_FOLLOW_MIN_SPEED = 22.0
RADARLESS_MATCHED_FOLLOW_MAX_REL_SPEED = 2.0
RADARLESS_MATCHED_FOLLOW_MIN_HEADWAY = 0.95
@@ -26,6 +28,11 @@ RADARLESS_MATCHED_FOLLOW_MAX_LEAD_BRAKE = 0.35
RADARLESS_MATCHED_FOLLOW_MIN_MODEL_PROB = 0.70
def _smoothstep(value: float, start: float, end: float) -> float:
factor = min(1.0, max(0.0, (float(value) - float(start)) / max(float(end) - float(start), 1e-3)))
return factor * factor * (3.0 - 2.0 * factor)
def should_track_lead(lead_status: bool, lead_distance: float, model_length: float, stop_distance: float,
v_ego: float, *, v_lead: float | None = None, radar: bool = False) -> bool:
if not lead_status:
@@ -72,44 +79,42 @@ def get_tracked_lead_catchup_bias(v_ego: float, lead_distance: float, desired_ga
desired_hw = desired_gap / max(v_ego, 1e-3)
headway_margin = actual_hw - desired_hw
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
speed_factor = _smoothstep(v_ego, HIGHWAY_LEAD_BEHAVIOR_MIN_SPEED, TRACKED_LEAD_CATCHUP_BIAS_FULL_SPEED)
cruise_factor = 1.0
if v_cruise is not None:
cruise_factor = _smoothstep(v_cruise - v_ego, 0.0, TRACKED_LEAD_CATCHUP_BIAS_CRUISE_ERROR_FULL)
if speed_factor == 0.0 or cruise_factor == 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 headway_margin <= TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN:
return 0.0
fade_start_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_START_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_START - desired_hw)
fade_end_margin = max(TRACKED_LEAD_CATCHUP_BIAS_MIN_FADE_END_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_ABSOLUTE_FADE_END - desired_hw)
if headway_margin >= fade_end_margin:
return 0.0
if closing_speed > max(2.5, 0.12 * v_ego):
return 0.0
entry_factor = _smoothstep(headway_margin,
TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN,
TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN)
exit_factor = 1.0 - _smoothstep(headway_margin, fade_start_margin, fade_end_margin)
entry_factor = min(1.0, max(0.0, (headway_margin - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN) /
max(TRACKED_LEAD_CATCHUP_BIAS_FULL_HEADWAY_MARGIN - TRACKED_LEAD_CATCHUP_BIAS_MIN_HEADWAY_MARGIN, 1e-3)))
exit_factor = 1.0
if headway_margin > fade_start_margin:
exit_factor = min(1.0, max(0.0, (fade_end_margin - headway_margin) / max(fade_end_margin - fade_start_margin, 1e-3)))
closing_fade_end = max(2.5, 0.12 * v_ego)
closing_fade_start = max(1.75, 0.08 * v_ego)
closing_factor = 1.0 - _smoothstep(closing_speed, closing_fade_start, closing_fade_end)
lateral_factor = 1.0
if y_rel is not None:
lateral_offset = abs(float(y_rel))
if lateral_offset >= TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET:
return 0.0
if lateral_offset > TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET:
lateral_factor = min(1.0, max(0.0, (TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - lateral_offset) /
max(TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET - TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET, 1e-3)))
lateral_factor = 1.0 - _smoothstep(lateral_offset,
TRACKED_LEAD_CATCHUP_BIAS_FULL_LATERAL_OFFSET,
TRACKED_LEAD_CATCHUP_BIAS_MAX_LATERAL_OFFSET)
bias_cap = max(14.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego)
return min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * entry_factor * exit_factor * lateral_factor
bias_cap = max(10.0, TRACKED_LEAD_CATCHUP_BIAS_SPEED_FACTOR * v_ego)
return (min(gap_error * TRACKED_LEAD_CATCHUP_BIAS_GAIN, bias_cap) * speed_factor * cruise_factor *
entry_factor * exit_factor * closing_factor * lateral_factor)
def should_disable_far_lead_throttle(v_ego: float, lead_distance: float, desired_gap: float,
selfdrive/controls/tests/test_lead_behavior.py
+19 -1
View File
@@ -23,7 +23,7 @@ def test_tracked_lead_catchup_bias_ignores_very_far_gap():
def test_tracked_lead_catchup_bias_applies_to_two_second_highway_gap():
bias = get_tracked_lead_catchup_bias(30.4, 63.0, 40.0, 0.4)
assert bias > 14.0
assert bias > 9.0
def test_tracked_lead_catchup_bias_reduces_for_laterally_offset_lead():
@@ -45,6 +45,24 @@ def test_tracked_lead_catchup_bias_stays_off_once_at_set_speed():
assert bias == 0.0
def test_tracked_lead_catchup_bias_fades_smoothly_near_set_speed():
below_set = get_tracked_lead_catchup_bias(27.70, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
at_set = get_tracked_lead_catchup_bias(27.78, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
assert 0.0 < below_set < 0.25
assert at_set == 0.0
def test_tracked_lead_catchup_bias_dials_back_camry_bookmark_case():
bias = get_tracked_lead_catchup_bias(27.40, 81.0, 46.0, 0.0, v_cruise=27.78, y_rel=0.8)
assert 0.0 < bias < 2.0
def test_tracked_lead_catchup_bias_fades_smoothly_at_closing_limit():
below_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.75)
above_limit = get_tracked_lead_catchup_bias(31.4, 78.7, 38.0, 3.77)
assert abs(below_limit - above_limit) < 0.1
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