diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 01c8f4f249..3d5c1104e6 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -224,6 +224,7 @@ struct StarPilotPlan @0xf98d843bfd7004a3 { cscOverridden @38 :Bool; # driver cancelled this curve with RES+ cscLearnedLatAccel @39 :Float32; # learned comfort at the current curvature, before margin cscBindingDistance @40 :Float32; # distance to the horizon point setting the target, m + approachStopLength @41 :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 f37ab1e5cb..d535283644 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -2196,9 +2196,14 @@ 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: force_stop_x = ( - float(sm['starpilotPlan'].forcingStopLength) + STOP_DISTANCE + - get_force_stop_distance_bias(self.CP.carFingerprint) + stop_length + STOP_DISTANCE + get_force_stop_distance_bias(self.CP.carFingerprint) ) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 0b88877712..6119ba9213 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -434,6 +434,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/starpilot/controls/lib/starpilot_vcruise.py b/starpilot/controls/lib/starpilot_vcruise.py index 80068c10b9..f583ae179b 100644 --- a/starpilot/controls/lib/starpilot_vcruise.py +++ b/starpilot/controls/lib/starpilot_vcruise.py @@ -62,6 +62,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 @@ -183,6 +185,7 @@ 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 @@ -637,6 +640,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 @@ -746,6 +752,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 af009abc2a..69478f905a 100644 --- a/starpilot/controls/starpilot_planner.py +++ b/starpilot/controls/starpilot_planner.py @@ -344,6 +344,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()