mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-05 13:32:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Dom's Plan

Browse Source
This commit is contained in:
firestar5683
2026-06-26 16:02:47 -05:00
parent 497eb3f771
commit 057ca874ce
2 changed files with 65 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+16 -7
View File
@@ -373,6 +373,8 @@ NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED = 3.5
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC = 8.0
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET = 0.45
NEAR_DUPLICATE_LEAD_TRANSITION_MAX_HEADWAY_ABOVE_TARGET = 0.85
NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN = 0.55
NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED = 1.25
LOW_SPEED_IDENTICAL_RADAR_DUPLICATE_TRANSITION_EXTRA_HEADWAY = 0.15
NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A = 0.35
NEAR_DUPLICATE_LEAD_TRANSITION_POSITIVE_STEP = 0.22
@@ -2020,13 +2022,6 @@ class LongitudinalPlanner:
relative_speed = float(v_ego) - float(lead.vLead)
closing_speed = max(0.0, relative_speed)
if closing_speed > NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED:
return None
ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC:
return None
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
if actual_headway < max(0.0, float(base_t_follow) - NEAR_DUPLICATE_LEAD_TRANSITION_MIN_HEADWAY_BELOW_TARGET):
return None
@@ -2036,6 +2031,20 @@ class LongitudinalPlanner:
if actual_headway > float(base_t_follow) + max_headway_above_target:
return None
max_closing_speed = NEAR_DUPLICATE_LEAD_TRANSITION_MAX_CLOSING_SPEED
if (
not identical_radar_duplicates and
not lead_radar and
actual_headway >= float(base_t_follow) + NEAR_DUPLICATE_VISION_TRANSITION_MIN_HEADWAY_MARGIN
):
max_closing_speed += NEAR_DUPLICATE_VISION_TRANSITION_EXTRA_CLOSING_SPEED
if closing_speed > max_closing_speed:
return None
ttc = float(lead.dRel) / max(closing_speed, 0.1) if closing_speed > 0.1 else float("inf")
if ttc < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_TTC:
return None
target_delta = float(output_a_target) - float(prev_output_a_target)
if abs(target_delta) < NEAR_DUPLICATE_LEAD_TRANSITION_MIN_DELTA_A:
return None
selfdrive/controls/tests/test_longitudinal_planner.py
+49
View File
@@ -3231,6 +3231,55 @@ def test_near_duplicate_lead_transition_target_damps_low_speed_duplicate_radar_h
assert smoothed == pytest.approx(0.57, abs=1e-6)
def test_near_duplicate_lead_transition_target_damps_generous_headway_duplicate_vision_sign_flip():
v_ego = 25.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=54.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99)
lead_two = make_lead(status=True, d_rel=54.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
smoothed = planner.get_near_duplicate_lead_transition_target(
lead_two,
v_ego,
1.45,
prev_output_a_target=0.70,
output_a_target=-0.61,
current_source="lead1",
tracking_lead_active=True,
)
assert smoothed is not None
assert smoothed == pytest.approx(0.52, abs=1e-6)
def test_near_duplicate_lead_transition_target_skips_fast_duplicate_vision_sign_flip_when_headway_is_not_generous():
v_ego = 25.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=43.0, v_lead=20.8, a_lead=-0.02, radar=False, model_prob=0.99)
lead_two = make_lead(status=True, d_rel=43.3, v_lead=20.82, a_lead=-0.01, radar=False, model_prob=0.99)
lead_one.vRel = lead_one.vLead - v_ego
lead_two.vRel = lead_two.vLead - v_ego
planner.lead_one = lead_one
planner.lead_two = lead_two
smoothed = planner.get_near_duplicate_lead_transition_target(
lead_two,
v_ego,
1.45,
prev_output_a_target=0.70,
output_a_target=-0.61,
current_source="lead1",
tracking_lead_active=True,
)
assert smoothed is None
def test_duplicate_slow_lead_brake_hold_prevents_zero_cross_from_duplicate_voacc_leads():
v_ego = 24.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)