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

planner work

Browse Source
This commit is contained in:
firestar5683
2026-06-14 17:10:32 -05:00
parent 8a4d2b558b
commit 7d939e1c76
2 changed files with 57 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+35 -12
View File
@@ -77,9 +77,14 @@ FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0
FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25
FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0
FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9
RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED = 12.0
RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0
RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14
STABLE_FOLLOW_CRUISE_MIN_SPEED = 12.0
STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0
STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14
STABLE_FOLLOW_CRUISE_MAX_REL_SPEED = 2.5
STABLE_FOLLOW_CRUISE_MIN_HEADWAY = 0.95
STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET = 0.35
STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET = 0.90
STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE = 0.35
NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0
NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9
NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE = 0.35
@@ -573,24 +578,42 @@ class LongitudinalMpc:
return lead_xv
@staticmethod
def get_radarless_matched_follow_cruise_hysteresis(lead, v_ego, t_follow):
def get_stable_follow_cruise_hysteresis(lead, v_ego, t_follow):
if lead is None or not lead.status:
return 0.0
if not is_radarless_matched_follow_window(
lead_radar = bool(getattr(lead, "radar", False))
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
if lead_brake > STABLE_FOLLOW_CRUISE_MAX_LEAD_BRAKE:
return 0.0
if lead_radar:
if float(t_follow) <= 0.0 or float(v_ego) < STABLE_FOLLOW_CRUISE_MIN_SPEED:
return 0.0
relative_speed = float(v_ego) - float(lead.vLead)
if abs(relative_speed) > STABLE_FOLLOW_CRUISE_MAX_REL_SPEED:
return 0.0
actual_headway = float(lead.dRel) / max(float(v_ego), 1e-3)
min_headway = max(STABLE_FOLLOW_CRUISE_MIN_HEADWAY,
float(t_follow) - STABLE_FOLLOW_CRUISE_HEADWAY_BELOW_TARGET)
max_headway = float(t_follow) + STABLE_FOLLOW_CRUISE_HEADWAY_ABOVE_TARGET
if not (min_headway <= actual_headway <= max_headway):
return 0.0
elif not is_radarless_matched_follow_window(
v_ego,
lead.dRel,
lead.vLead,
t_follow,
radar=bool(getattr(lead, "radar", False)),
lead_brake=max(0.0, -float(getattr(lead, "aLeadK", 0.0))),
radar=lead_radar,
lead_brake=lead_brake,
lead_prob=float(getattr(lead, "modelProb", 0.0)),
min_speed=RADARLESS_MATCHED_FOLLOW_CRUISE_MIN_SPEED,
min_speed=STABLE_FOLLOW_CRUISE_MIN_SPEED,
):
return 0.0
return max(RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_MIN,
RADARLESS_MATCHED_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego))
return max(STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN,
STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN * float(v_ego))
@staticmethod
def leads_are_near_duplicates(lead_one, lead_two, v_ego):
@@ -670,9 +693,9 @@ class LongitudinalMpc:
prev_source = self.source
if optional_far_lead_comfort:
if prev_source == 'lead0':
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow)
cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_one, v_ego, t_follow)
elif prev_source == 'lead1':
cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow)
cruise_obstacle += self.get_stable_follow_cruise_hysteresis(lead_two, v_ego, t_follow)
if optional_far_lead_comfort and tracking_lead and lead_one.status:
desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow)
closing_speed = max(0.0, v_ego - lead_one.vLead)
selfdrive/controls/tests/test_longitudinal_planner.py
+22
View File
@@ -2368,6 +2368,28 @@ def test_near_duplicate_lead_source_hysteresis_prefers_previous_source():
assert lead_1_bias > 0.0
def test_stable_follow_cruise_hysteresis_applies_for_radar_lead():
v_ego = 27.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=40.5, v_lead=26.3, a_lead=-0.02, radar=True, model_prob=1.0)
hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45)
assert hysteresis > 0.0
def test_stable_follow_cruise_hysteresis_skips_fast_closing_radar_lead():
v_ego = 27.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=40.5, v_lead=21.5, a_lead=-0.02, radar=True, model_prob=1.0)
hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45)
assert hysteresis == 0.0
def test_near_duplicate_lead_source_hysteresis_skips_distinct_leads():
v_ego = 27.0
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)