From 9b7f1222e80ebd7719a4fbd668d347cb9c166570 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Mon, 3 Aug 2026 13:46:33 -0500 Subject: [PATCH] dec --- .../controls/lib/longitudinal_planner.py | 25 +++++++++++++ .../tests/test_longitudinal_planner.py | 35 +++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 0709403e8..6ef7be35b 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -42,6 +42,10 @@ MODEL_STOP_MIN_END_SPEED = 2.0 MODEL_FIRST_AUTHORITY = 0.75 MODEL_FIRST_OPEN_ROAD_AUTHORITY = 0.35 MODEL_FIRST_CRUISE_ERROR = 1.5 +MODEL_SLOWDOWN_MIN_ACCEL = 0.15 +MODEL_SLOWDOWN_MIN_SPEED_DROP = 0.75 +MODEL_SLOWDOWN_FULL_SPEED_DROP = 3.0 +MODEL_SLOWDOWN_MAX_AUTHORITY = 0.65 STOP_ENTER_AUTHORITY = 0.72 STOP_RELEASE_TIME = 0.50 @@ -184,6 +188,25 @@ class LongitudinalPlanner: speed_score = 1.0 - _smoothstep(end_speed, 0.5, MODEL_STOP_MIN_END_SPEED) return float(time_score * speed_score) + @staticmethod + def _model_slowdown_authority(model, v_ego): + """Preview sustained model braking before it becomes a full stop request.""" + if not len(model.position.x) or not len(model.velocity.x): + return 0.0 + + desired_accel = float(getattr(model.action, "desiredAcceleration", 0.0)) + if not np.isfinite(desired_accel) or desired_accel >= -MODEL_SLOWDOWN_MIN_ACCEL: + return 0.0 + + end_speed = float(model.velocity.x[-1]) + speed_drop = float(v_ego) - end_speed + if not np.isfinite(end_speed) or speed_drop < MODEL_SLOWDOWN_MIN_SPEED_DROP: + return 0.0 + + accel_score = _smoothstep(-desired_accel, MODEL_SLOWDOWN_MIN_ACCEL, 0.8) + speed_score = _smoothstep(speed_drop, MODEL_SLOWDOWN_MIN_SPEED_DROP, MODEL_SLOWDOWN_FULL_SPEED_DROP) + return MODEL_SLOWDOWN_MAX_AUTHORITY * max(accel_score, speed_score) + @staticmethod def _curve_authority(sm, v_ego): curvature = abs(float(getattr(sm["starpilotPlan"], "roadCurvature", 0.0))) @@ -403,9 +426,11 @@ class LongitudinalPlanner: has_lead = any(bool(getattr(lead, "status", False)) for lead in leads) model_first = bool(getattr(starpilot_toggles, "longitudinal_model_preference", False)) stop_authority = self._model_stop_authority(sm["modelV2"], scene_v_ego) + slowdown_authority = self._model_slowdown_authority(sm["modelV2"], scene_v_ego) self.model_authority = self._get_model_authority( sm, scene_v_ego, v_cruise, has_lead, model_first, stop_authority, ) + self.model_authority = max(self.model_authority, slowdown_authority) model_limited_target = min(mpc_target, model_target) target = mpc_target + self.model_authority * (model_limited_target - mpc_target) self.plan_source = "e2e" if model_limited_target < mpc_target - 1e-3 and self.model_authority > 0.05 else self.mpc.source diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index bf94fd806..24bf5e673 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -77,6 +77,14 @@ def set_model_stop(model, distance=5.0): model.action.desiredAcceleration = -1.0 +def set_model_slowdown(model, end_speed, desired_accel=-0.4): + times = np.asarray(ModelConstants.T_IDXS) + v_ego = float(model.velocity.x[0]) + model.position.x = np.linspace(0.0, max(v_ego * times[-1] * 0.8, 1.0), len(times)).tolist() + model.velocity.x = np.linspace(v_ego, end_speed, len(times)).tolist() + model.action.desiredAcceleration = desired_accel + + def make_sm(v_ego=20.0, *, v_cruise=None, model_accel=0.0, lead_one=None, lead_two=None): v_cruise = v_ego + 5.0 if v_cruise is None else v_cruise return { @@ -177,6 +185,33 @@ def test_model_stop_intent_brakes_without_binary_mode_switch(): assert planner.plan_source == "e2e" +def test_set_speed_first_previews_sustained_model_slowdown(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + use_fixed_mpc(planner, 0.5) + sm = make_sm(v_ego=20.0, model_accel=-0.4) + set_model_slowdown(sm["modelV2"], end_speed=16.0) + + planner.update(sm, make_toggles(model_first=False)) + + assert 0.0 < planner.model_authority < 1.0 + assert planner.output_a_target < 0.0 + assert planner.state == PlanState.moving + + +def test_small_model_speed_dip_does_not_override_set_speed(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=20.0) + use_fixed_mpc(planner, 0.5) + sm = make_sm(v_ego=20.0, model_accel=-0.2) + set_model_slowdown(sm["modelV2"], end_speed=19.5, desired_accel=-0.2) + + planner.update(sm, make_toggles(model_first=False)) + + assert planner.model_authority == 0.0 + assert planner.output_a_target > 0.0 + + def test_force_stop_is_a_hard_stop(): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) planner = LongitudinalPlanner(CP, init_v=0.0)