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

Dom's Plan V3

Browse Source
This commit is contained in:
firestar5683
2026-04-27 11:49:39 -05:00
parent 426dbd1718
commit 43a5c09412
4 changed files with 156 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_planner.py
+50 -3
View File
@@ -39,6 +39,17 @@ 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
LEAD_APPROACH_TFOLLOW_TRIGGER_TIME = 4.5
LEAD_APPROACH_TFOLLOW_FULL_TIME = 1.5
LEAD_APPROACH_TFOLLOW_MAX_DELTA = 0.18
LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED = 6.0
LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE = 2.5
LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED = 0.75
LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE = 0.2
LEAD_APPROACH_TFOLLOW_WINDOW_MIN = 6.0
LEAD_APPROACH_TFOLLOW_WINDOW_GAIN = 0.35
LEAD_APPROACH_TFOLLOW_RATE_UP = 1.0
LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.18
# Uncertainty-based filter disable thresholds
UNCERT_SLOPE_TRIG = 0.12 # per second
@@ -188,6 +199,7 @@ class LongitudinalPlanner:
# Uncertainty slope tracking
self._uncert_last = 0.0
self._uncert_last_t = None
self.effective_t_follow = None
@property
def mlsim(self):
@@ -292,6 +304,40 @@ class LongitudinalPlanner:
return max(accel_min, -approach_decel)
def get_dynamic_t_follow(self, base_t_follow, lead, v_ego):
base_t_follow = float(base_t_follow)
target_t_follow = base_t_follow
if lead is not None and lead.status:
lead_prob = float(getattr(lead, "modelProb", 1.0 if bool(getattr(lead, "radar", False)) else 0.0))
if bool(getattr(lead, "radar", False)) or lead_prob >= VISION_LEAD_APPROACH_MIN_MODEL_PROB:
lead_brake = max(0.0, -float(lead.aLeadK))
closing_speed = max(0.0, v_ego - lead.vLead)
if closing_speed >= LEAD_APPROACH_TFOLLOW_MIN_CLOSING_SPEED or lead_brake >= LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE:
desired_gap = float(desired_follow_distance(v_ego, lead.vLead, base_t_follow))
approach_window = max(LEAD_APPROACH_TFOLLOW_WINDOW_MIN, LEAD_APPROACH_TFOLLOW_WINDOW_GAIN * float(v_ego))
if float(lead.dRel) <= desired_gap + approach_window:
reaction_t = max(self.CP.longitudinalActuatorDelay, self.dt)
projected_closing_speed = closing_speed + 0.5 * lead_brake * reaction_t
gap_to_follow = max(float(lead.dRel) - desired_gap, 0.0)
time_to_follow = gap_to_follow / max(projected_closing_speed, 0.1)
time_factor = float(np.clip((LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - time_to_follow) /
(LEAD_APPROACH_TFOLLOW_TRIGGER_TIME - LEAD_APPROACH_TFOLLOW_FULL_TIME), 0.0, 1.0))
closing_factor = float(np.clip(closing_speed / LEAD_APPROACH_TFOLLOW_MAX_CLOSING_SPEED, 0.0, 1.0))
brake_factor = float(np.clip(lead_brake / LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE, 0.0, 1.0))
target_delta = LEAD_APPROACH_TFOLLOW_MAX_DELTA * np.clip(
0.55 * time_factor + 0.25 * closing_factor + 0.20 * brake_factor, 0.0, 1.0)
target_t_follow = base_t_follow + float(target_delta)
if self.effective_t_follow is None:
self.effective_t_follow = base_t_follow
rate = LEAD_APPROACH_TFOLLOW_RATE_UP if target_t_follow > self.effective_t_follow else LEAD_APPROACH_TFOLLOW_RATE_DOWN
step = rate * self.dt
self.effective_t_follow = float(np.clip(target_t_follow, self.effective_t_follow - step, self.effective_t_follow + step))
self.effective_t_follow = max(base_t_follow, self.effective_t_follow)
return self.effective_t_follow
@staticmethod
def raw_close_lead_needs_control(lead, v_ego):
if lead is None or not lead.status:
@@ -387,6 +433,7 @@ class LongitudinalPlanner:
# safety path so ACC/chill does not ignore a visible lead during that debounce.
lead_control_active = tracking_lead or raw_close_lead_control
lead_one_active = bool(self.lead_one.status and lead_control_active)
effective_t_follow = self.get_dynamic_t_follow(sm['starpilotPlan'].tFollow, self.lead_one if lead_one_active else None, v_ego)
lead_dist = self.lead_one.dRel if lead_one_active else 50.0
@@ -504,7 +551,7 @@ class LongitudinalPlanner:
if not self.mlsim:
self.mpc.mode = dec_mpc_mode
self.mpc.update(sm['radarState'], v_cruise, x, v, a, j,
sm['starpilotPlan'].dangerFactor, sm['starpilotPlan'].tFollow,
sm['starpilotPlan'].dangerFactor, effective_t_follow,
personality=personality, tracking_lead=lead_control_active)
self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
@@ -534,7 +581,7 @@ class LongitudinalPlanner:
if lead_one_active:
rel_v = max(0.0, v_ego - self.lead_one.vLead)
# dynamic time headway adds a small buffer when uncertainty is elevated
base_th = 1.6
base_th = max(1.6, effective_t_follow)
th = base_th + 0.6 * max(0.0, uncertainty - 0.42)
desired_gap = th * v_ego
if (self.lead_dist_f is not None and self.lead_dist_f < desired_gap and rel_v > 0.5):
@@ -581,7 +628,7 @@ 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)
approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow)
if approach_cap is not None:
close_lead_caps.append(approach_cap)
if close_lead_caps:
selfdrive/controls/tests/test_conditional_experimental_mode.py
+16
View File
@@ -86,6 +86,22 @@ def test_far_visible_lead_does_not_block_stop_light():
assert cem.stop_light_detected
def test_stop_light_stays_latched_until_untracked_stopped_lead_handoff():
v_ego = 45 * CV.MPH_TO_MS
model_length = v_ego * 4.0
cem = make_cem(model_length=model_length)
run_stop_light_detector(cem, v_ego, steps=30)
assert cem.stop_light_detected
cem.starpilot_planner.lead_one.status = True
cem.starpilot_planner.lead_one.dRel = model_length - 5.0
cem.starpilot_planner.lead_one.vLead = 0.5
run_stop_light_detector(cem, v_ego, steps=10, tracking_lead=False)
assert cem.stop_light_detected
class DummyThemeManager:
def update_wheel_image(self, *args, **kwargs):
pass
selfdrive/controls/tests/test_longitudinal_planner.py
+74
View File
@@ -225,6 +225,80 @@ def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
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_dynamic_t_follow_increases_modestly_for_closing_lead(model_version):
v_ego = 21.535
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
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["starpilotPlan"].vCruise = v_ego + 8.0
for _ in range(8):
planner.update(sm, make_toggles(model_version))
assert planner.effective_t_follow is not None
assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.05
assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.2
@pytest.mark.parametrize("model_version", ["v11", "v12"])
def test_dynamic_t_follow_stays_near_base_for_far_highway_lead(model_version):
v_ego = 29.26
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
v_ego,
desired_accel=0.2,
min_accel=-1.0,
experimental_mode=False,
tracking_lead=True,
lead_one=make_lead(status=True, d_rel=114.8, v_lead=28.88, a_lead=-0.75, radar=True, model_prob=0.9),
)
sm["starpilotPlan"].vCruise = v_ego + 3.0
for _ in range(12):
planner.update(sm, make_toggles(model_version))
assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02)
@pytest.mark.parametrize("model_version", ["v11", "v12"])
def test_dynamic_t_follow_releases_toward_base_after_lead_opens(model_version):
v_ego = 21.535
CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
planner = LongitudinalPlanner(CP, init_v=v_ego)
sm = make_sm(
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),
)
for _ in range(8):
planner.update(sm, make_toggles(model_version))
boosted_t_follow = planner.effective_t_follow
sm["radarState"].leadOne = make_lead(status=True, d_rel=66.168, v_lead=20.751, a_lead=0.261, radar=False, model_prob=0.975)
for _ in range(12):
planner.update(sm, make_toggles(model_version))
assert boosted_t_follow is not None
assert planner.effective_t_follow < boosted_t_follow
assert planner.effective_t_follow == pytest.approx(sm["starpilotPlan"].tFollow, abs=0.02)
@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
starpilot/controls/lib/conditional_experimental_mode.py
+16 -3
View File
@@ -44,6 +44,7 @@ class ConditionalExperimentalMode:
STOP_LIGHT_ON_MARGIN = 2.5
STOP_LIGHT_OFF_MARGIN = 4.0
STOP_LIGHT_LEAD_BLOCK_MARGIN = 15.0
STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED = 2.0
# ===== END TUNING PARAMETERS =====
@@ -226,11 +227,23 @@ class ConditionalExperimentalMode:
# present; far/stale leads should not suppress true stop-light detection.
lead = getattr(self.starpilot_planner, "lead_one", None)
lead_distance = float(getattr(lead, "dRel", float("inf")))
lead_speed = float(getattr(lead, "vLead", float("inf")))
lead_relevant = bool(getattr(lead, "status", False)) and lead_distance < stop_threshold + self.STOP_LIGHT_LEAD_BLOCK_MARGIN
self.lead_clear_filter.update(not lead_relevant)
lead_cleared = self.lead_clear_filter.x >= THRESHOLD
handoff_to_stopped_lead = (
self.stop_light_detected and
lead_relevant and
not self.starpilot_planner.tracking_lead and
lead_speed < self.STOP_LIGHT_HANDOFF_MAX_LEAD_SPEED
)
if handoff_to_stopped_lead:
lead_cleared = True
else:
self.lead_clear_filter.update(not lead_relevant)
lead_cleared = self.lead_clear_filter.x >= THRESHOLD
self.stop_light_filter.update(model_stopping and lead_cleared)
self.stop_light_detected = bool(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared)
self.stop_light_detected = bool(
(self.stop_light_filter.x >= THRESHOLD**2 and lead_cleared) or handoff_to_stopped_lead
)
else:
self.stop_light_filter.x = 0
self.stop_light_detected = False