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

final plan

Browse Source
This commit is contained in:
firestar5683
2026-06-16 18:58:49 -05:00
parent 29b777f12c
commit 6f907e2060
2 changed files with 100 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+28 -8
View File
@@ -40,6 +40,7 @@ RAW_LEAD_LOW_SPEED_HOLD_MAX_LATERAL_OFFSET = 1.75
RAW_LEAD_LOW_SPEED_HOLD_MIN_CLOSING_SPEED = 0.15
STANDSTILL_LEAD_NUDGE_ACCEL = 0.05
STANDSTILL_LEAD_NUDGE_MIN_SPEED = 0.0
STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL = 0.2
STANDSTILL_LEAD_DEPART_MIN_ACCEL = 0.35
STANDSTILL_LEAD_DEPART_MAX_EGO_SPEED = 1.5
STANDSTILL_LEAD_DEPART_MIN_LEAD_SPEED = 0.6
@@ -1363,7 +1364,7 @@ class LongitudinalPlanner:
return None
lead_brake = max(0.0, -float(getattr(lead, "aLeadK", 0.0)))
if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE:
if lead_brake > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LEAD_BRAKE and not tracking_lead_active:
return None
if abs(float(getattr(lead, "yRel", 0.0))) > CRUISE_TRACKED_LEAD_ACCEL_CAP_MAX_LATERAL_OFFSET:
@@ -2366,6 +2367,13 @@ class LongitudinalPlanner:
moving_leads = [lead for lead in (self.lead_one, self.lead_two)
if lead.status and
lead.vLead > STANDSTILL_LEAD_NUDGE_MIN_SPEED and lead.dRel >= standstill_nudge_gap]
accelerating_nudge_lead = any(
lead.status and
float(getattr(lead, "vLead", 0.0)) > STANDSTILL_LEAD_NUDGE_MIN_SPEED and
float(getattr(lead, "aLeadK", 0.0)) >= STANDSTILL_LEAD_NUDGE_MIN_LEAD_ACCEL and
float(getattr(lead, "dRel", 0.0)) >= standstill_nudge_gap
for lead in (self.lead_one, self.lead_two)
)
confident_depart_detected = any(self.is_confident_lead_depart(lead, float(sm['carState'].vEgo))
for lead in (self.lead_one, self.lead_two))
lead_depart_ready = any(
@@ -2396,7 +2404,8 @@ class LongitudinalPlanner:
)
standstill_stopped_lead_guard_cap = None
if lead_control_active and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED):
standstill_guard_lead_present = any(bool(getattr(lead, "status", False)) for lead in (self.lead_one, self.lead_two))
if standstill_guard_lead_present and (bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED):
release_ready = bool(lead_depart_ready or confident_depart_ready)
standstill_stopped_lead_guard_caps = [
cap for cap in (
@@ -2540,12 +2549,14 @@ class LongitudinalPlanner:
if vision_brake_cap_active:
output_accel_min = min(output_accel_min, vision_cap_accel_min)
follow_control_lead = self.get_follow_control_lead(
lead_control_active,
scene_v_ego,
effective_t_follow,
allow_optional_far_lead_logic=allow_complex_follow_logic,
)
follow_control_lead = None
if allow_complex_follow_logic:
follow_control_lead = self.get_follow_control_lead(
lead_control_active,
scene_v_ego,
effective_t_follow,
allow_optional_far_lead_logic=True,
)
if allow_complex_follow_logic and follow_control_lead is not None and not panic_bypass:
if not output_should_stop and not vision_low_speed_stop_active:
tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor(
@@ -2701,6 +2712,15 @@ class LongitudinalPlanner:
self.a_desired = min(self.a_desired, low_speed_weak_lead_accel_cap)
output_a_target = min(output_a_target, low_speed_weak_lead_accel_cap)
if (
lead_control_active and
(bool(sm['carState'].standstill) or float(sm['carState'].vEgo) <= STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED) and
output_should_stop and
accelerating_nudge_lead and
not depart_safety_veto
):
output_a_target = max(output_a_target, STANDSTILL_LEAD_NUDGE_ACCEL)
force_stop_handoff = bool(
getattr(sm['starpilotPlan'], 'forcingStop', False) and
not lead_control_active and
selfdrive/controls/tests/test_longitudinal_planner.py
+72
View File
@@ -1149,6 +1149,78 @@ def test_standstill_moving_lead_does_not_force_resume_while_should_stop(model_ve
assert planner.output_a_target < 0.1
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_standstill_accelerating_lead_keeps_small_nudge_while_stop_holds(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
sm = make_sm(
0.0,
desired_accel=0.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.40, radar=False, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["starpilotPlan"].vCruise = 10.0
sm["modelV2"].action.shouldStop = False
planner.update(sm, make_toggles(model_version))
assert planner.output_should_stop
assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_near_standstill_accelerating_lead_keeps_nudge_during_creep_frame(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.03)
sm = make_sm(
0.03,
desired_accel=0.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=5.60, v_lead=0.06, a_lead=0.40, radar=False, model_prob=1.0),
)
sm["carState"].standstill = False
sm["controlsState"].longControlState = LongCtrlState.pid
sm["starpilotPlan"].vCruise = 10.0
sm["modelV2"].action.shouldStop = False
planner.update(sm, make_toggles(model_version))
assert planner.output_should_stop
assert longitudinal_planner_module.STANDSTILL_LEAD_NUDGE_ACCEL <= planner.output_a_target < 0.1
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_standstill_tiny_opening_lead_without_accel_does_not_get_nudge_floor(model_version):
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=0.0)
sm = make_sm(
0.0,
desired_accel=0.0,
min_accel=-0.5,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=5.55, v_lead=0.02, a_lead=0.0, radar=False, model_prob=1.0),
)
sm["carState"].standstill = True
sm["controlsState"].longControlState = LongCtrlState.stopping
sm["starpilotPlan"].vCruise = 10.0
sm["modelV2"].action.shouldStop = False
planner.update(sm, make_toggles(model_version))
assert planner.output_should_stop
assert planner.output_a_target <= 0.0
@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"])
def test_standstill_moving_lead_applies_resume_floor_once_stop_clears(model_version):
v_ego = 0.0