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

long boi

Browse Source
This commit is contained in:
firestar5683
2026-06-27 13:30:18 -05:00
parent 2798c12c90
commit 562c5eb58d
2 changed files with 54 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+14
View File
@@ -220,6 +220,9 @@ LEAD_CATCHUP_ACCEL_MIN_EGO = 8.0
LEAD_CATCHUP_ACCEL_MIN_LEAD_DELTA = -0.5
LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_MIN = 4.0
LEAD_CATCHUP_ACCEL_MAX_GAP_BUFFER_GAIN = 0.15
RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA = 0.10
RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL = 0.12
RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN = 0.18
RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN = 0.75
RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_CAP = 0.04
RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR = 0.75
@@ -1505,9 +1508,20 @@ class LongitudinalPlanner:
tracking_lead_active and
self.lead_is_matched_follow_window(lead, v_ego, t_follow)
)
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
headway_margin = actual_headway - float(t_follow)
if radar_matched_follow_active and gap_error > (gap_buffer - RADAR_MATCHED_FOLLOW_CATCHUP_CAP_BUFFER_MARGIN):
return None
if (
radar_matched_follow_active and
current_source in ("lead0", "lead1") and
lead_delta >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_DELTA and
float(getattr(lead, "aLeadK", 0.0)) >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_LEAD_ACCEL and
headway_margin >= RADAR_MATCHED_FOLLOW_PULLAWAY_BYPASS_MIN_HEADWAY_MARGIN
):
return None
if (radar_matched_follow_active and current_source == "cruise" and
gap_error <= RADAR_MATCHED_FOLLOW_CATCHUP_HOLD_MAX_GAP_ERROR and
lead_delta < min_lead_delta):
selfdrive/controls/tests/test_longitudinal_planner.py
+40
View File
@@ -2604,6 +2604,46 @@ def test_route_8bc6_catchup_cap_skips_comfortable_accelerating_lead_when_source_
assert cap is None
def test_route_8bc6_radar_matched_follow_catchup_cap_skips_mild_pullaway_after_lead_lock():
v_ego = 23.96
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=33.6, v_lead=24.23, a_lead=0.16, radar=True, model_prob=1.0, y_rel=0.0,
)
cap = planner.get_lead_catchup_accel_cap(
lead,
v_ego,
1.16,
current_source="lead0",
tracking_lead_active=True,
)
assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16)
assert cap is None
def test_route_8bc6_radar_matched_follow_catchup_cap_keeps_cap_when_pullaway_is_not_confirmed():
v_ego = 23.96
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=33.6, v_lead=24.23, a_lead=0.08, radar=True, model_prob=1.0, y_rel=0.0,
)
cap = planner.get_lead_catchup_accel_cap(
lead,
v_ego,
1.16,
current_source="lead0",
tracking_lead_active=True,
)
assert planner.lead_is_matched_follow_window(lead, v_ego, 1.16)
assert cap is not None
def test_route_8bc6_radar_matched_follow_catchup_cap_skips_buffer_edge_square_wave():
v_ego = 22.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)