diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 5f2a11dc1..1f14636bd 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -221,6 +221,11 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { trackingLead @36 :Bool; stopSignConfirmed @37 :Bool; pulseGlideCoasting @38 :Bool; # developer-only P&G phase for on-road status UI + # Curve Speed Controller diagnostics, for tuning and rollout validation + cscOverridden @39 :Bool; # driver cancelled this curve with RES+ + cscLearnedLatAccel @40 :Float32; # learned comfort at the current curvature, before margin + cscBindingDistance @41 :Float32; # distance to the horizon point setting the target, m + approachStopLength @42 :Float32; # pre-commit distance to a detected stop, m; 0 when off } struct StarPilotRadarState @0xb86e6369214c01c8 { diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 01b544cf1..7fd1b6301 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -9,6 +9,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.starpilot.common.model_versions import is_tinygrad_model_version +from openpilot.starpilot.controls.lib.starpilot_vcruise import FT_TO_M, OFFSET_FT_MAX, OFFSET_FT_MIN 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 @@ -381,6 +382,11 @@ def get_vehicle_min_accel(CP, v_ego): # Restored planner constants retained by CEM, stop, and departure paths. A_CRUISE_MIN = -1.0 +# The stop distance runs ~9 m long through the mid-approach, which leaves the obstacle slack +# so it stays silent and deceleration sags. Multiplicative so the trim scales with what is +# left. Note the car parks where the obstacle sits, so this is also a placement bias — 0.85 +# stopped ~4.6 m short, 0.93 ~1.6 m. +FORCE_STOP_OBSTACLE_TRIM = 0.93 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_SPEED = 0.25 STANDSTILL_LEAD_CREEP_RELEASE_MIN_LEAD_ACCEL = 0.08 STANDSTILL_LEAD_CREEP_RELEASE_MIN_GAP_MARGIN = 0.1 @@ -2217,8 +2223,17 @@ class LongitudinalPlanner: force_stop_x = None force_stop_handoff_m = get_force_stop_handoff_distance(self.CP.carFingerprint) if sm['starpilotPlan'].forcingStop and sm['starpilotPlan'].forcingStopLength > force_stop_handoff_m: + stop_length = float(sm['starpilotPlan'].forcingStopLength) + else: + # pre-commit the envelope is only a speed ceiling, which the solver tracks with a lag; + # getattr so a stale cereal build degrades to the old behaviour instead of raising + stop_length = float(getattr(sm['starpilotPlan'], 'approachStopLength', 0.0)) + if stop_length > force_stop_handoff_m: + # ForceStopDistanceOffset shifts the perceived line for the v_cruise ceiling, so it has + # to shift the obstacle too or the slider barely moves anything now that stop_x leads. + offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, int(getattr(starpilot_toggles, 'force_stop_distance_offset', 0) or 0))) force_stop_x = ( - float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE + + stop_length * FORCE_STOP_OBSTACLE_TRIM + offset_ft * FT_TO_M + STOP_DISTANCE + get_force_stop_distance_bias(self.CP.carFingerprint) ) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a7f057101..a5ae074fc 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -53,6 +53,8 @@ ADJACENT_STOP_REST_FRAMES = 15 ADJACENT_STOP_MIN_Y = 1.8 # m — inside this is our own lane ADJACENT_STOP_MAX_Y = 7.5 # m — beyond this is roadside, not an adjacent lane ADJACENT_STOP_MAX_D = 110.0 # m +ADJACENT_STOP_QUEUE_GAP_M = 5.0 # m — anything stopped beyond the furthest qualifier means + # the bar is past it too, so the hint would stop us short class KalmanParams: @@ -179,6 +181,10 @@ class Track: if self.leadTrackID == self.identifier: return False + return self.in_adjacent_lane(model_data) + + def in_adjacent_lane(self, model_data: capnp._DynamicStructReader): + """Lane geometry only, no deceleration history — also used to spot a queue ahead.""" if not (ADJACENT_STOP_MIN_Y < abs(self.yRel) < ADJACENT_STOP_MAX_Y): return False @@ -398,9 +404,10 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStructReader) -> dict[str, Any]: """Stop-line hint: a vehicle that decelerated to a stop in a neighbouring lane. - Takes the FARTHEST qualifying vehicle: in a queue the front car sits at the bar and the - rest are closer to us, so the nearest one underestimates the distance. The consumer only - shortens with this, so underestimating is the harmful direction. + Takes the FARTHEST qualifying vehicle, then drops the hint entirely if a queue reaches + past it. Cars already stopped when we acquire them never show the moving -> stopped + transition, so the qualifying set is biased toward the back of a line; without this the + hint marks a mid-queue bumper and stops us short of the bar. """ if len(model_data.laneLines) < 4: return {'status': False} @@ -410,6 +417,11 @@ def get_adjacent_stopped(tracks: dict[int, Track], model_data: capnp._DynamicStr return {'status': False} furthest = max(candidates, key=lambda c: c.dRel) + for c in tracks.values(): + if (c.dRel > furthest.dRel + ADJACENT_STOP_QUEUE_GAP_M and + abs(c.vLead) < ADJACENT_STOP_REST_V and + c.in_adjacent_lane(model_data)): + return {'status': False} return { 'status': True, 'dRel': float(furthest.dRel), diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index a78967023..830edfdfd 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -526,6 +526,7 @@ def make_sm(v_ego: float, desired_accel: float, min_accel: float, *, experimenta forcingStop=False, redLight=False, forcingStopLength=2, + approachStopLength=0.0, ), } diff --git a/selfdrive/controls/tests/test_starpilot_vcruise.py b/selfdrive/controls/tests/test_starpilot_vcruise.py index afabe58df..dd0248bd1 100644 --- a/selfdrive/controls/tests/test_starpilot_vcruise.py +++ b/selfdrive/controls/tests/test_starpilot_vcruise.py @@ -7,6 +7,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.starpilot.common.starpilot_variables import PLANNER_TIME from openpilot.starpilot.controls.lib.curve_speed_controller import CSC_MAX_DECEL_RATE, CurveSpeedController from openpilot.starpilot.controls.lib.starpilot_vcruise import ( + FORCE_STOP_CAP_SLACK_M, FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME, STANDSTILL_FORCE_STOP_LIGHT_HOLD_TIME, StarPilotVCruise, @@ -58,6 +59,8 @@ def make_vcruise(*, red_light=False, raw_model_stopped=False, forcing_stop=False vcruise.forcing_stop = forcing_stop vcruise.force_stop_timer = 1.0 if forcing_stop else 0.0 vcruise.tracked_model_length = 0.0 if forcing_stop else planner.model_length + # what the not-committed branch would have left behind on the frame before commit + vcruise.force_stop_distance_cap = planner.model_length return planner, vcruise @@ -524,6 +527,7 @@ def test_force_stop_reanchors_when_model_reopens_path_without_stop_action(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 90.0 vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 90.0 sm = make_sm(standstill=False) sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) @@ -573,6 +577,37 @@ def test_force_stop_does_not_reanchor_inside_reanchor_floor(): assert vcruise.tracked_model_length < 25.0 +def test_force_stop_reanchor_bounded_by_distance_driven(): + # The line can't recede: a ballooning horizon may not push the stop past where it was at + # commit minus the distance driven since. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 70.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=10.0) + + assert vcruise.tracked_model_length <= 70.0 + FORCE_STOP_CAP_SLACK_M + assert vcruise.tracked_model_length < 100.0 # nowhere near the 200 m the horizon claimed + + +def test_force_stop_cap_slack_tapers_near_the_line(): + # Slack protects against an under-read at commit; held near the line it would just aim the + # solver that far past the stop bar. + planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) + planner.model_length = 200.0 + vcruise.tracked_model_length = 60.0 + vcruise.force_stop_distance_cap = 12.0 + sm = make_sm(standstill=False) + sm["modelV2"] = SimpleNamespace(action=SimpleNamespace(shouldStop=False)) + + update_vcruise(vcruise, sm, make_toggles(), now=0.0, v_ego=5.0) + + assert vcruise.tracked_model_length < 12.0 + FORCE_STOP_CAP_SLACK_M / 2.0 + + def test_force_stop_does_not_reanchor_committed_model_stop(): planner, vcruise = make_vcruise(red_light=False, raw_model_stopped=False, forcing_stop=True) planner.model_length = 40.0 diff --git a/starpilot/common/accel_profile.py b/starpilot/common/accel_profile.py index 8165919ce..48d81e1c0 100644 --- a/starpilot/common/accel_profile.py +++ b/starpilot/common/accel_profile.py @@ -3,8 +3,6 @@ from __future__ import annotations import math -from openpilot.selfdrive.controls.lib.longitudinal_planner import get_max_accel - ACCELERATION_PROFILES = { "STANDARD": 0, "ECO": 1, diff --git a/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 0aa713a37..283830b84 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -59,6 +59,8 @@ LEAD_VETO_M_OVERRIDES = { } FORCE_STOP_APPROACH_DECEL = 0.65 # m/s^2 — speed ceiling before commit. LOWER = more early # braking; don't go under FORCE_STOP_MODEL_APPROACH_DECEL +# approachStopLength is published RAW: model_length converges from above, so rate-limiting +# it inward freezes it far out and the constraint never binds. Tried, measured, don't re-add. 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 @@ -76,7 +78,14 @@ FORCE_STOP_TURN_VETO_STEERING_ANGLE = 25.0 FORCE_STOP_CURVE_VETO_MAX_ROAD_CURVATURE = 0.003 FORCE_STOP_TURN_VETO_STOP_SEEN_HOLD_TIME = 4.0 FORCE_STOP_DISTANCE_REANCHOR_MIN_GAP = 3.0 # m — ignore small model-horizon noise -FORCE_STOP_REANCHOR_MIN_M = 40.0 +FORCE_STOP_REANCHOR_MIN_M = 40.0 # m — inside this only ratchet down; shouldStop doesn't + # assert until ~10 m, so horizon jitter would release the stop +FORCE_STOP_CAP_SLACK_M = 15.0 # m — the line can't move away, so tracked can never exceed + # what it was at commit minus distance driven. Slack covers an + # under-read at commit; without it that would stop us short. +FORCE_STOP_CAP_TAPER_M = 60.0 # m — slack fades to 0 as the cap closes. The solver aims at + # tracked, so slack held near the line is braking for a stop bar + # that far past the real one. # Knob bounds (mirror of UI slider; defense in depth) OFFSET_FT_MIN = -20 @@ -180,9 +189,11 @@ class StarPilotVCruise: self.force_stop_from_light = False self.force_stop_light_clear_since = None self.controls_enabled_previously = False + self.approach_stop_length = 0.0 # published as starpilotPlan.approachStopLength # Kinematic distance estimator. Same attribute also published as # starpilotPlan.forcingStopLength, so the existing reader keeps working. self.tracked_model_length = 0.0 + self.force_stop_distance_cap = 0.0 # odometry ceiling, re-seeded until commit self.stop_sign_confirmed = False self.stop_seen_on_approach_at = None @@ -606,6 +617,9 @@ class StarPilotVCruise: offset_ft = max(OFFSET_FT_MIN, min(OFFSET_FT_MAX, offset_ft_raw)) offset_m = offset_ft * FT_TO_M + # cleared on every path; only the far-approach envelope below republishes it + self.approach_stop_length = 0.0 + if force_standstill_enabled and not self.override_force_standstill: self.forcing_stop = True self.tracked_model_length = 0.0 @@ -645,6 +659,12 @@ class StarPilotVCruise: self.tracked_model_length = model_length else: self.tracked_model_length = min(self.tracked_model_length, model_length) + # Odometry ceiling: the line can't recede, so a re-anchor may never exceed what we + # had at commit minus what we've driven. Bounds a ballooning horizon (seen +95 m) + # that the REANCHOR_MIN floor can't catch, since that floor trusts the estimate. + self.force_stop_distance_cap = max(self.force_stop_distance_cap - (v_ego * DT_MDL), 0.0) + cap_slack = FORCE_STOP_CAP_SLACK_M * min(self.force_stop_distance_cap / FORCE_STOP_CAP_TAPER_M, 1.0) + self.tracked_model_length = min(self.tracked_model_length, self.force_stop_distance_cap + cap_slack) if dash_active: if model_length < DASH_MODEL_AGREE_M: self.tracked_model_length = min(self.tracked_model_length, DASH_SEED_M) @@ -678,6 +698,7 @@ class StarPilotVCruise: self.stop_sign_confirmed = False self.tracked_model_length = self.starpilot_planner.model_length + self.force_stop_distance_cap = self.tracked_model_length targets = [v_cruise] if self.csc_target >= CSC_MIN_SPEED: @@ -723,6 +744,8 @@ class StarPilotVCruise: adjacent_stop_d = self._get_adjacent_stop_distance(sm) if adjacent_stop_d is not None: approach_d = min(approach_d, adjacent_stop_d) + # pre-offset, so it hands off to forcingStopLength at commit without a step + self.approach_stop_length = max(approach_d, 0.0) approach_d += offset_m + force_stop_distance_bias_m if approach_d > force_stop_handoff_m: targets.append(math.sqrt(2.0 * FORCE_STOP_APPROACH_DECEL * (approach_d - force_stop_handoff_m))) diff --git a/starpilot/controls/starpilot_planner.py b/starpilot/controls/starpilot_planner.py index 650bdecb1..4cc96b91f 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -31,7 +31,10 @@ 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) +FORCE_STOP_JERK_SCALE = 0.20 # accel-change cost multiplier for the whole stop approach, + # envelope included (125 -> 25). Lower = reaches the braking + # target sooner; it does not make the target deeper. Response + # is super-linear here, so raise it if onset feels like a step. FORCE_STOP_JERK_SCALE_OVERRIDES = { # The Elantra's current force-stop ramp is smooth, but it waits too long # before building decel and then arrives at the initial brake too abruptly. @@ -308,7 +311,9 @@ class StarPilotPlanner: except (KeyError, IndexError, TypeError, AttributeError): car_params = None - if self.starpilot_vcruise.forcing_stop: + # Also while the far-approach envelope is running: at onset the ramp reaches only + # ~-0.5 m/s^2 after a second, so the first seconds of a detected red are mostly lost. + if self.starpilot_vcruise.forcing_stop or self.starpilot_vcruise.approach_stop_length > 0.0: jerk_scale = get_force_stop_jerk_scale(car_params) elif self.tracking_lead: # Elantra vision leads can hand off from cruise to lead0 while closing @@ -346,6 +351,7 @@ class StarPilotPlanner: starpilotPlan.forcingStop = self.starpilot_vcruise.forcing_stop starpilotPlan.forcingStopLength = self.starpilot_vcruise.tracked_model_length + starpilotPlan.approachStopLength = float(self.starpilot_vcruise.approach_stop_length) starpilotPlan.stopSignConfirmed = self.starpilot_vcruise.stop_sign_confirmed starpilotPlan.starpilotEvents = self.starpilot_events.events.to_msg()