mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 11:02:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Dom's plan

Browse Source
This commit is contained in:
firestar5683
2026-04-27 00:35:13 -05:00
parent 8bae6a1063
commit 426dbd1718
2 changed files with 108 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+47
View File
@@ -10,6 +10,7 @@ from openpilot.common.realtime import DT_MDL
from openpilot.selfdrive.modeld.constants import ModelConstants
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import desired_follow_distance
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
@@ -30,6 +31,14 @@ RAW_LEAD_SAFETY_MIN_CLOSING_SPEED = 0.5
RAW_LEAD_SAFETY_TTC = 7.0
RAW_LEAD_SAFETY_DISTANCE = 40.0
CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
VISION_LEAD_APPROACH_TRIGGER_TIME = 4.0
VISION_LEAD_APPROACH_FULL_TIME = 1.0
VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0
VISION_LEAD_APPROACH_MAX_DECEL = 0.45
VISION_LEAD_APPROACH_MIN_DECEL = 0.12
VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85
VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
# Uncertainty-based filter disable thresholds
UNCERT_SLOPE_TRIG = 0.12 # per second
@@ -248,6 +257,41 @@ class LongitudinalPlanner:
return max(accel_min, -required_decel)
def get_vision_lead_approach_cap(self, lead, v_ego, accel_min, t_follow):
if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
return None
lead_prob = float(getattr(lead, "modelProb", 0.0))
if lead_prob < VISION_LEAD_APPROACH_MIN_MODEL_PROB:
return None
lead_brake = max(0.0, -float(lead.aLeadK))
reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
closing_speed = max(0.0, v_ego - lead.vLead)
projected_closing_speed = closing_speed + lead_brake * reaction_t
if projected_closing_speed < VISION_LEAD_APPROACH_MIN_CLOSING_SPEED:
return None
tight_follow_gap = float(t_follow * v_ego + VISION_LEAD_APPROACH_TIGHT_BUFFER)
gap_to_tight_follow = float(lead.dRel) - tight_follow_gap
time_to_tight_follow = gap_to_tight_follow / max(projected_closing_speed, 0.1)
if time_to_tight_follow > VISION_LEAD_APPROACH_TRIGGER_TIME:
return None
desired_gap = float(desired_follow_distance(v_ego, lead.vLead, t_follow))
if float(lead.dRel) > desired_gap + VISION_LEAD_APPROACH_TIGHT_BUFFER:
return None
time_factor = float(np.clip((VISION_LEAD_APPROACH_TRIGGER_TIME - time_to_tight_follow) /
(VISION_LEAD_APPROACH_TRIGGER_TIME - VISION_LEAD_APPROACH_FULL_TIME), 0.0, 1.0))
prob_factor = float(np.clip((lead_prob - VISION_LEAD_APPROACH_MIN_MODEL_PROB) /
(VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_LEAD_APPROACH_MIN_MODEL_PROB), 0.0, 1.0))
approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * prob_factor
if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL:
return None
return max(accel_min, -approach_decel)
@staticmethod
def raw_close_lead_needs_control(lead, v_ego):
if lead is None or not lead.status:
@@ -537,6 +581,9 @@ class LongitudinalPlanner:
cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
if cap is not None:
close_lead_caps.append(cap)
approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, sm['starpilotPlan'].tFollow)
if approach_cap is not None:
close_lead_caps.append(approach_cap)
if close_lead_caps:
close_lead_brake_cap = min(close_lead_caps)
self.a_desired = min(self.a_desired, close_lead_brake_cap)
selfdrive/controls/tests/test_longitudinal_planner.py
+61
View File
@@ -201,6 +201,67 @@ def test_soften_far_radar_lead_accel_keeps_close_closing_brake():
assert softened == pytest.approx(baseline)
def test_vision_lead_approach_cap_brakes_before_hard_cap():
v_ego = 21.535
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984)
hard_cap = planner.get_close_lead_brake_cap(lead, v_ego, -1.0)
approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45)
assert hard_cap == pytest.approx(-0.212, abs=1e-2)
assert approach_cap is not None
assert approach_cap < hard_cap
assert approach_cap > -0.6
def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
v_ego = 19.37
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
lead = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975)
assert planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.45) is None
@pytest.mark.parametrize("model_version", ["v11", "v12"])
def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_version):
approach_v_ego = 21.535
close_v_ego = 21.435
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner_approach = LongitudinalPlanner(CP, init_v=approach_v_ego)
planner_close = LongitudinalPlanner(CP, init_v=close_v_ego)
sm_approach = make_sm(
approach_v_ego,
desired_accel=0.2,
min_accel=-3.0,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=38.9, v_lead=18.04, a_lead=-0.026, radar=False, model_prob=0.984),
)
sm_close = make_sm(
close_v_ego,
desired_accel=0.2,
min_accel=-3.0,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=27.18, v_lead=15.76, a_lead=-0.824, radar=False, model_prob=0.988),
)
sm_approach["starpilotPlan"].vCruise = approach_v_ego + 8.0
sm_close["starpilotPlan"].vCruise = close_v_ego + 8.0
planner_approach.update(sm_approach, make_toggles(model_version))
planner_close.update(sm_close, make_toggles(model_version))
assert planner_approach.mode == "acc"
assert planner_close.mode == "acc"
assert planner_approach.output_a_target < -0.3
assert planner_close.output_a_target < planner_approach.output_a_target - 0.25
@pytest.mark.parametrize("model_version", ["v11", "v12"])
def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
far_v_ego = 29.26