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

REEEEEE

Browse Source
This commit is contained in:
firestar5683
2026-06-15 16:27:29 -05:00
parent 322364c1ca
commit 56564d55c8
2 changed files with 88 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+35 -3
View File
@@ -210,6 +210,11 @@ 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
POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED = 12.0
POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB = 0.95
POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA = 0.35
POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL = 0.25
POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN = 0.10
LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_SPEED = 12.0
LOW_SPEED_FOLLOW_ACCEL_CAP_MIN_MODEL_PROB = 0.85
LOW_SPEED_FOLLOW_ACCEL_CAP_MAX_LEAD_BRAKE = 0.20
@@ -1240,6 +1245,28 @@ class LongitudinalPlanner:
brake_floor = -hold_brake
return brake_floor if accel_min >= 0.0 else max(accel_min, brake_floor)
def is_stable_post_departure_pullaway(self, lead, v_ego, t_follow):
if lead is None or not lead.status or float(v_ego) < POST_DEPARTURE_FOLLOW_BYPASS_MIN_SPEED:
return False
lead_radar = bool(getattr(lead, "radar", False))
lead_prob = float(getattr(lead, "modelProb", 1.0 if lead_radar else 0.0))
if not lead_radar and lead_prob < POST_DEPARTURE_FOLLOW_BYPASS_MIN_MODEL_PROB:
return False
if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
return False
lead_delta = float(lead.vLead) - float(v_ego)
lead_accel = float(getattr(lead, "aLeadK", 0.0))
if (lead_delta < POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_DELTA or
lead_accel < POST_DEPARTURE_FOLLOW_BYPASS_MIN_LEAD_ACCEL):
return False
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
headway_margin = actual_headway - float(t_follow)
return headway_margin >= POST_DEPARTURE_FOLLOW_BYPASS_MIN_HEADWAY_MARGIN
def get_lead_catchup_accel_cap(self, lead, v_ego, t_follow):
if lead is None or not lead.status:
return None
@@ -1272,6 +1299,9 @@ class LongitudinalPlanner:
if gap_error > gap_buffer:
return None
if not low_speed_follow_window and self.is_stable_post_departure_pullaway(lead, v_ego, t_follow):
return None
# If the lead is already pace-matched or pulling away, keep any catch-up
# accel very small while we're near the follow target so we don't surge into
# the lead and immediately ask for brake again.
@@ -1361,9 +1391,11 @@ class LongitudinalPlanner:
# momentarily falls near the pull-away threshold. That produces the repeated
# 0.18 m/s^2 "surge / give up / surge" behavior seen in real logs.
lead_accel = float(getattr(lead, "aLeadK", 0.0))
if (lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and
lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and
gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN):
if self.is_stable_post_departure_pullaway(lead, v_ego, t_follow) or (
lead_delta >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_LEAD_DELTA and
lead_accel >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN and
gap_error >= CRUISE_TRACKED_LEAD_ACCEL_CAP_ACCEL_AWAY_MIN_GAP_MARGIN
):
return None
base_cap = float(np.interp(
selfdrive/controls/tests/test_longitudinal_planner.py
+53
View File
@@ -1827,6 +1827,19 @@ def test_low_speed_follow_catchup_accel_cap_limits_close_vision_catchup():
assert 0.15 <= cap <= 0.45
def test_route_8bc6_post_departure_catchup_cap_skips_accelerating_away_radar_lead():
v_ego = 19.03
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30,
)
cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45)
assert cap is None
def test_low_speed_follow_catchup_uses_raw_vehicle_speed_when_cluster_runs_high():
v_ego = 7.8
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
@@ -2225,6 +2238,46 @@ def test_cruise_tracking_lead_accel_cap_skips_accelerating_away_radar_lead():
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)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=33.85, v_lead=19.47, a_lead=0.33, radar=True, model_prob=1.0, y_rel=-0.30,
)
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_post_departure_pullaway_bypass_does_not_skip_when_lead_brakes_again():
v_ego = 19.03
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(
status=True, d_rel=33.85, v_lead=18.90, a_lead=-0.35, radar=True, model_prob=1.0, y_rel=-0.30,
)
catchup_cap = planner.get_lead_catchup_accel_cap(lead, v_ego, 1.45)
cruise_cap = planner.get_cruise_tracking_lead_accel_cap(
lead,
v_ego,
1.45,
current_source="cruise",
tracking_lead_active=True,
)
assert catchup_cap is not None
assert cruise_cap is not None
def test_cruise_tracking_lead_accel_transition_target_damps_mid_speed_reacquisition_burst():
v_ego = 16.2
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)