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

Force Stop Tweaks

Browse Source
This commit is contained in:
whoisdomi
2026-08-01 12:55:23 -05:00
committed by firestar5683
parent 8436e41731
commit 32e2f0bccc
6 changed files with 202 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
starpilot/controls/lib/starpilot_vcruise.py
+82 -6
View File
@@ -37,12 +37,24 @@ NAV_TURN_TARGET_SPEEDS = {
# Smaller values pull speed down earlier on approach.
FORCE_STOP_MODEL_APPROACH_DECEL = 0.65
FORCE_STOP_DASH_APPROACH_DECEL = 1.0
ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this
ACTIVATION_M = 75.0 # m — CEM/model path activates when model_length < this.
# Don't raise: forcing_stop latches until standstill, so a brief
# red-light blip at longer range commits to a stop we can't release.
ACTIVATION_HYSTERESIS_M = 8.0 # m — release margin; absorbs model_length jitter at the gate
LEAD_VETO_M = 75.0 # m — lead proximity that vetoes Force Stop (kept off ACTIVATION_M
# so raising activation can't silently widen the veto)
MPC_HANDOFF_M = 6.0 # m — below this, command 0 and let MPC finish the stop
FORCE_STOP_APPROACH_DECEL = 0.75 # m/s^2 — speed ceiling before commit. LOWER = more early
# braking. Must stay above FORCE_STOP_MODEL_APPROACH_DECEL or the
# pre-commit ceiling is stricter than the stop itself.
ADAS_MAX_MS = 17.88 # 40 mph — cross-street ADAS guard
DASH_SEED_M = 27.0 # ~88 ft — typical ADAS detection distance, used to snap
# tracked length closer when dashboard confirms a sign
DASH_MODEL_AGREE_M = 50.0 # m — dash arm/snap needs model_length under this; a lone dash bit
# against a long model path is a phantom stop
FT_TO_M = 0.3048
ADJACENT_STOP_MIN_USE_M = 10.0 # m — inside this the MPC already owns the stop; a
# late-arriving hint could only jerk it
FORCE_STOP_TURN_VETO_MAX_SPEED = 18.0 * CV.MPH_TO_MS
# Real-turn steering angle. A stop-then-turn is still ~straight on approach, so a low
# threshold caused legit stops to be skipped when the blinker came on early. Only suppress
@@ -136,6 +148,7 @@ class StarPilotVCruise:
self.override_force_stop_timer = 0
self.force_stop_timer = 0.0
self.activation_gate_active = False
self.standstill_force_stop_hold = False
self.standstill_force_stop_clear_since = 0.0
self.standstill_force_stop_started_at = None
@@ -190,6 +203,26 @@ class StarPilotVCruise:
self.standstill_force_stop_started_at = None
self.standstill_force_stop_reason = None
@staticmethod
def _get_adjacent_stop_distance(sm):
"""dRel of a vehicle that decelerated to a stop in an adjacent lane, or None.
The model's own distance runs long on a clear-lane approach; a car stopped alongside
is physically at (or just behind) the stop bar. Radar-only, so it holds for any
driving model.
"""
try:
radar_state = sm["starpilotRadarState"]
except (KeyError, IndexError, TypeError, AttributeError):
return None
adjacent = getattr(radar_state, "adjacentStopped", None)
if adjacent is None or not getattr(adjacent, "status", False):
return None
d_rel = float(getattr(adjacent, "dRel", 0.0))
return d_rel if d_rel > ADJACENT_STOP_MIN_USE_M else None
@staticmethod
def _nav_maneuver_target_speed(maneuver_type, maneuver_modifier):
maneuver_type = str(maneuver_type or "").strip().lower()
@@ -296,7 +329,7 @@ class StarPilotVCruise:
# during the filter's settling window and stay committed for the whole stop.
lead = self.starpilot_planner.lead_one
lead_present = (bool(getattr(lead, "status", False))
and float(getattr(lead, "dRel", float("inf"))) < ACTIVATION_M
and float(getattr(lead, "dRel", float("inf"))) < LEAD_VETO_M
and float(getattr(lead, "vLead", float("inf"))) < v_ego + 2.0)
curved_approach_scene = (
abs(float(getattr(self.starpilot_planner, "road_curvature", 0.0))) >= FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE
@@ -307,9 +340,19 @@ class StarPilotVCruise:
# Exclude when a lead is present (raw or filtered) — the handoff_to_stopped_lead path
# in CEM can set stop_light_detected even with a lead present, which would incorrectly
# activate Force Stop and stop the car far behind the lead instead of letting ACC handle it.
cem_path = (self.starpilot_planner.starpilot_cem.stop_light_detected
# Schmitt trigger: model_length jitters around ACTIVATION_M and keeps resetting
# force_stop_timer's ramp. Scoped to a detected stop so the wider release threshold
# can't leak into ordinary slow driving.
stop_light_detected = self.starpilot_planner.starpilot_cem.stop_light_detected
if self.activation_gate_active and stop_light_detected:
model_length_active = self.starpilot_planner.model_length < ACTIVATION_M + ACTIVATION_HYSTERESIS_M
else:
model_length_active = self.starpilot_planner.model_length < ACTIVATION_M
self.activation_gate_active = model_length_active and stop_light_detected
cem_path = (stop_light_detected
and controls_enabled and starpilot_toggles.force_stops
and self.starpilot_planner.model_length < ACTIVATION_M
and model_length_active
and self.override_force_stop_timer <= 0
and not self.starpilot_planner.driving_in_curve
and not curved_approach_scene
@@ -323,6 +366,7 @@ class StarPilotVCruise:
dash_active = dash_value > 0
dash_path = (dash_active and controls_enabled and starpilot_toggles.force_stops
and v_ego < ADAS_MAX_MS
and self.starpilot_planner.model_length < DASH_MODEL_AGREE_M
and self.override_force_stop_timer <= 0
and not self.starpilot_planner.driving_in_curve
and not turn_scene_active
@@ -494,11 +538,22 @@ class StarPilotVCruise:
# Kinematic distance estimator (also published as forcingStopLength).
# Decay one-to-one with motion, clamp by current model_length so we adopt
# the model's view when it regains sight, and snap closer to DASH_SEED_M
# whenever the dashboard signal is active.
# when the dashboard signal is active and the model agrees a stop is near.
self.tracked_model_length = max(self.tracked_model_length - (v_ego * DT_MDL), 0.0)
self.tracked_model_length = min(self.tracked_model_length, self.starpilot_planner.model_length)
if dash_active:
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
if self.starpilot_planner.model_length < DASH_MODEL_AGREE_M:
self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M)
# inside the seed the model range is the better line estimate; letting it pull
# tracked back up is what keeps an early snap from parking us short of the sign
if self.starpilot_planner.model_length < DASH_SEED_M:
self.tracked_model_length = self.starpilot_planner.model_length
# A car stopped in the next lane marks the stop bar better than the model does.
# Shortening clamp only — it can pull the stop in, never push it out.
adjacent_stop_d = self._get_adjacent_stop_distance(sm)
if adjacent_stop_d is not None:
self.tracked_model_length = min(self.tracked_model_length, adjacent_stop_d)
# Kinematic profile with user offset. Positive offset shifts the perceived
# line further down the road -> car rolls further before commanding 0.
@@ -544,6 +599,27 @@ class StarPilotVCruise:
targets.append(slc_control_target)
if self.nav_turn_target > 0.0:
targets.append(self.nav_turn_target)
# Far-approach envelope: bleed speed off before commit so the car isn't still at
# cruise when the kinematic curve takes over. Same vetoes as the activation paths;
# no latch, recomputed each frame, releases on green.
if (stop_light_detected
and controls_enabled and starpilot_toggles.force_stops
and self.override_force_stop_timer <= 0
and not self.starpilot_planner.driving_in_curve
and not curved_approach_scene
and not turn_scene_active
and not self.starpilot_planner.tracking_lead
and not lead_present):
# adjacent-stopped hint caps the model distance; shorten-only, self-clearing
approach_d = self.starpilot_planner.model_length
adjacent_stop_d = self._get_adjacent_stop_distance(sm)
if adjacent_stop_d is not None:
approach_d = min(approach_d, adjacent_stop_d)
approach_d += offset_m
if approach_d > MPC_HANDOFF_M:
targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - MPC_HANDOFF_M)))
v_cruise = min(targets)
self.controls_enabled_previously = controls_enabled
starpilot/controls/starpilot_planner.py
+5 -1
View File
@@ -30,6 +30,7 @@ from openpilot.starpilot.controls.lib.starpilot_vcruise import StarPilotVCruise
from openpilot.starpilot.controls.lib.weather_checker import WeatherChecker
RADARLESS_TRACK_HOLD_TIME = 0.45
FORCE_STOP_JERK_SCALE = 0.32 # accel-change cost multiplier while forcing_stop (125 -> ~40)
def _sanitize_json_value(value):
@@ -282,7 +283,10 @@ class StarPilotPlanner:
starpilot_plan_send.valid = sm.all_checks(service_list=["carState", "controlsState", "selfdriveState", "radarState"])
starpilotPlan = starpilot_plan_send.starpilotPlan
starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk)
# While committed to a Force Stop, cut the MPC's accel-change penalty so terminal
# braking can ramp faster. 0.32 lands near 40, what long_mpc uses in blended mode.
jerk_scale = FORCE_STOP_JERK_SCALE if self.starpilot_vcruise.forcing_stop else 1.0
starpilotPlan.accelerationJerk = float(A_CHANGE_COST * self.starpilot_following.acceleration_jerk * jerk_scale)
starpilotPlan.dangerFactor = float(self.starpilot_following.danger_factor)
starpilotPlan.dangerJerk = float(DANGER_ZONE_COST * self.starpilot_following.danger_jerk)
starpilotPlan.speedJerk = float(J_EGO_COST * self.starpilot_following.speed_jerk)