dec

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

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)