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

Lead

Browse Source
This commit is contained in:
firestar5683
2026-05-08 11:37:20 -05:00
parent 7dc26d1a68
commit 1b2b13881c
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
+17
View File
@@ -44,6 +44,11 @@ VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.30
VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0
VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20
VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN = 0.75
VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE = 0.45
VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE = 1.20
VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL = 1.30
VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL = 1.75
VISION_UNTRACKED_SLOW_LEAD_MIN_MODEL_PROB = 0.9
VISION_UNTRACKED_SLOW_LEAD_FULL_MODEL_PROB = 0.97
VISION_UNTRACKED_SLOW_LEAD_MIN_CLOSING_SPEED = 3.0
@@ -396,6 +401,18 @@ class LongitudinalPlanner:
deficit_decel = VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL * deficit_factor * prob_factor
deficit_decel *= 0.5 + 0.5 * closing_factor
approach_decel = max(approach_decel, deficit_decel)
# If a tracked vision lead is already far inside the tight-follow window and
# it is actively braking, don't stay stuck at the softer comfort cap.
if deficit_factor >= VISION_LEAD_APPROACH_BRAKING_DEFICIT_MIN and lead_brake >= VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE:
braking_floor = float(np.interp(
lead_brake,
[VISION_LEAD_APPROACH_BRAKING_MIN_LEAD_BRAKE, VISION_LEAD_APPROACH_BRAKING_FULL_LEAD_BRAKE],
[VISION_LEAD_APPROACH_BRAKING_FLOOR_MIN_DECEL, VISION_LEAD_APPROACH_BRAKING_FLOOR_MAX_DECEL],
))
braking_floor *= 0.85 + 0.15 * max(closing_factor, prob_factor)
approach_decel = max(approach_decel, braking_floor)
if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL:
return None
selfdrive/controls/tests/test_longitudinal_planner.py
+15
View File
@@ -228,6 +228,21 @@ def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap():
assert approach_cap < -0.5
def test_vision_lead_approach_cap_brakes_harder_for_braking_tracked_lead_inside_tight_gap():
v_ego = 19.50
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=19.7, v_lead=16.25, a_lead=-0.83, radar=False, model_prob=0.98)
hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -3.0)
approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -3.0, 1.45)
assert hard_cap == pytest.approx(-1.01, abs=0.03)
assert approach_cap is not None
assert approach_cap < -1.35
assert approach_cap < hard_cap
def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
v_ego = 19.37
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)