Doms Plan v5

2026-07-20 00:32:11 +08:00 · 2026-04-28 18:53:29 -05:00
parent d7a29e6d81
commit ef57854b81
7 changed files with 229 additions and 19 deletions
@@ -32,13 +32,16 @@ RAW_LEAD_SAFETY_TTC = 7.0
 RAW_LEAD_SAFETY_DISTANCE = 40.0
 CLOSE_LEAD_BRAKE_CAP_MAX_TTC = 25.0
 VISION_LEAD_APPROACH_MIN_CLOSING_SPEED = 2.0
-VISION_LEAD_APPROACH_TRIGGER_TIME = 4.0
+VISION_LEAD_APPROACH_TRIGGER_TIME = 4.5
 VISION_LEAD_APPROACH_FULL_TIME = 1.0
 VISION_LEAD_APPROACH_TIGHT_BUFFER = 2.0
-VISION_LEAD_APPROACH_MAX_DECEL = 0.45
-VISION_LEAD_APPROACH_MIN_DECEL = 0.12
+VISION_LEAD_APPROACH_MAX_DECEL = 0.65
+VISION_LEAD_APPROACH_MIN_DECEL = 0.15
 VISION_LEAD_APPROACH_MIN_MODEL_PROB = 0.85
 VISION_LEAD_APPROACH_FULL_MODEL_PROB = 0.98
+VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL = 1.15
+VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN = 3.0
+VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN = 0.20
 VISION_SLOW_LEAD_MAX_SPEED = 5.0
 VISION_SLOW_LEAD_MIN_CLOSING_SPEED = 1.5
 VISION_SLOW_LEAD_TRIGGER_TTC = 4.5
@@ -56,7 +59,22 @@ LEAD_APPROACH_TFOLLOW_MIN_LEAD_BRAKE = 0.2
 LEAD_APPROACH_TFOLLOW_WINDOW_MIN = 6.0
 LEAD_APPROACH_TFOLLOW_WINDOW_GAIN = 0.35
 LEAD_APPROACH_TFOLLOW_RATE_UP = 1.0
-LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.18
+LEAD_APPROACH_TFOLLOW_RATE_DOWN = 0.60
+VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA = 0.24
+VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED = 20.0
+VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN = 8.0
+VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN = 0.35
+VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED = 5.5
+VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED = 1.75
+VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB = 0.9
+VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED = 0.35
+VISION_LOW_SPEED_STOP_BUFFER_BASE = 2.8
+VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN = 0.80
+VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN = 0.25
+VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN = 0.75
+VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME = 0.6
+VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 0.9
+VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25

 # Uncertainty-based filter disable thresholds
 UNCERT_SLOPE_TRIG = 0.12  # per second
@@ -207,6 +225,7 @@ class LongitudinalPlanner:
    self._uncert_last = 0.0
    self._uncert_last_t = None
    self.effective_t_follow = None
+    self.vision_low_speed_stop_hold_until = 0.0

  @property
  def mlsim(self):
@@ -305,7 +324,17 @@ class LongitudinalPlanner:
                                (VISION_LEAD_APPROACH_TRIGGER_TIME - VISION_LEAD_APPROACH_FULL_TIME), 0.0, 1.0))
    prob_factor = float(np.clip((lead_prob - VISION_LEAD_APPROACH_MIN_MODEL_PROB) /
                                (VISION_LEAD_APPROACH_FULL_MODEL_PROB - VISION_LEAD_APPROACH_MIN_MODEL_PROB), 0.0, 1.0))
-    approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * prob_factor
+    closing_factor = float(np.clip(projected_closing_speed / (VISION_LEAD_APPROACH_MIN_CLOSING_SPEED + 2.5), 0.0, 1.0))
+    tight_follow_deficit = max(tight_follow_gap - float(lead.dRel), 0.0)
+    tight_follow_buffer = max(VISION_LEAD_APPROACH_DEFICIT_BUFFER_MIN,
+                              VISION_LEAD_APPROACH_DEFICIT_BUFFER_GAIN * float(v_ego) + 1.0)
+    deficit_factor = float(np.clip(tight_follow_deficit / tight_follow_buffer, 0.0, 1.0))
+
+    approach_decel = VISION_LEAD_APPROACH_MAX_DECEL * time_factor * (0.45 + 0.55 * prob_factor)
+    approach_decel *= 0.6 + 0.4 * closing_factor
+    deficit_decel = VISION_LEAD_APPROACH_DEFICIT_MAX_DECEL * deficit_factor * prob_factor
+    deficit_decel *= 0.5 + 0.5 * closing_factor
+    approach_decel = max(approach_decel, deficit_decel)
    if approach_decel < VISION_LEAD_APPROACH_MIN_DECEL:
      return None

@@ -371,6 +400,17 @@ class LongitudinalPlanner:
            brake_factor = float(np.clip(lead_brake / LEAD_APPROACH_TFOLLOW_MAX_LEAD_BRAKE, 0.0, 1.0))
            target_delta = LEAD_APPROACH_TFOLLOW_MAX_DELTA * np.clip(
              0.55 * time_factor + 0.25 * closing_factor + 0.20 * brake_factor, 0.0, 1.0)
+            if not bool(getattr(lead, "radar", False)):
+              gap_deficit = max(desired_gap - float(lead.dRel), 0.0)
+              gap_buffer = max(VISION_LEAD_TFOLLOW_GAP_BUFFER_MIN,
+                               VISION_LEAD_TFOLLOW_GAP_BUFFER_GAIN * float(v_ego))
+              gap_factor = float(np.clip(gap_deficit / gap_buffer, 0.0, 1.0))
+              slow_lead_factor = float(np.clip((VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED - float(lead.vLead)) /
+                                               VISION_LEAD_TFOLLOW_SLOW_LEAD_SPEED, 0.0, 1.0))
+              vision_extra = VISION_LEAD_TFOLLOW_MAX_EXTRA_DELTA * np.clip(
+                0.40 * time_factor + 0.30 * gap_factor + 0.20 * slow_lead_factor + 0.10 * closing_factor,
+                0.0, 1.0)
+              target_delta += vision_extra
            target_t_follow = base_t_follow + float(target_delta)

    if self.effective_t_follow is None:
@@ -382,6 +422,38 @@ class LongitudinalPlanner:
    self.effective_t_follow = max(base_t_follow, self.effective_t_follow)
    return self.effective_t_follow

+  def get_vision_low_speed_stop_buffer_cap(self, lead, v_ego, accel_min):
+    if lead is None or not lead.status or bool(getattr(lead, "radar", False)):
+      return None, False
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < VISION_LOW_SPEED_STOP_BUFFER_MIN_MODEL_PROB:
+      return None, False
+
+    lead_speed = max(float(lead.vLead), 0.0)
+    closing_speed = max(0.0, v_ego - lead_speed)
+    valid_context = (
+      v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and
+      lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and
+      closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED
+    )
+
+    now_t = time.monotonic()
+    entry_buffer = max(3.2, VISION_LOW_SPEED_STOP_BUFFER_BASE +
+                       VISION_LOW_SPEED_STOP_BUFFER_EGO_GAIN * float(v_ego) +
+                       VISION_LOW_SPEED_STOP_BUFFER_LEAD_GAIN * lead_speed)
+    release_buffer = entry_buffer + VISION_LOW_SPEED_STOP_BUFFER_RELEASE_MARGIN
+    if valid_context and float(lead.dRel) <= entry_buffer:
+      self.vision_low_speed_stop_hold_until = now_t + VISION_LOW_SPEED_STOP_BUFFER_HOLD_TIME
+
+    latched = now_t < self.vision_low_speed_stop_hold_until
+    active = valid_context and (float(lead.dRel) <= entry_buffer or (latched and float(lead.dRel) <= release_buffer))
+    if not active:
+      return None, False
+
+    min_stop_brake = VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE + VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN * float(v_ego)
+    return max(accel_min, -min_stop_brake), True
+
  @staticmethod
  def raw_close_lead_needs_control(lead, v_ego):
    if lead is None or not lead.status:
@@ -667,6 +739,7 @@ class LongitudinalPlanner:
    output_accel_min = get_vehicle_min_accel(self.CP, v_ego) if experimental_mlsim else accel_limits_turns[0]

    close_lead_caps = []
+    vision_low_speed_stop_active = False
    if lead_control_active:
      for lead in (self.lead_one, self.lead_two):
        cap = self.get_close_lead_brake_cap(lead, v_ego, output_accel_min)
@@ -678,6 +751,10 @@ class LongitudinalPlanner:
        approach_cap = self.get_vision_lead_approach_cap(lead, v_ego, output_accel_min, effective_t_follow)
        if approach_cap is not None:
          close_lead_caps.append(approach_cap)
+        low_speed_stop_cap, low_speed_stop_active = self.get_vision_low_speed_stop_buffer_cap(lead, v_ego, output_accel_min)
+        if low_speed_stop_cap is not None:
+          close_lead_caps.append(low_speed_stop_cap)
+        vision_low_speed_stop_active |= low_speed_stop_active
    if close_lead_caps:
      close_lead_brake_cap = min(close_lead_caps)
      self.a_desired = min(self.a_desired, close_lead_brake_cap)
@@ -699,7 +776,7 @@ class LongitudinalPlanner:
    output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))

    self.output_a_target = output_a_target
-    self.output_should_stop = bool(output_should_stop)
+    self.output_should_stop = bool(output_should_stop or vision_low_speed_stop_active)

  def publish(self, sm, pm):
    plan_send = messaging.new_message('longitudinalPlan')
@@ -213,7 +213,19 @@ def test_vision_lead_approach_cap_brakes_before_hard_cap():
  assert hard_cap == pytest.approx(-0.212, abs=1e-2)
  assert approach_cap is not None
  assert approach_cap < hard_cap
-  assert approach_cap > -0.6
+  assert approach_cap > -1.2
+
+
+def test_vision_lead_approach_cap_brakes_harder_when_inside_tight_gap():
+  v_ego = 26.18
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  lead = make_lead(status=True, d_rel=39.72, v_lead=22.46, a_lead=-0.15, radar=False, model_prob=0.97)
+
+  approach_cap = planner.get_vision_lead_approach_cap(lead, v_ego, -1.0, 1.49)
+
+  assert approach_cap is not None
+  assert approach_cap < -0.5


 def test_vision_lead_approach_cap_ignores_opening_lead_with_large_gap():
@@ -267,8 +279,8 @@ def test_dynamic_t_follow_increases_modestly_for_closing_lead(model_version):
    planner.update(sm, make_toggles(model_version))

  assert planner.effective_t_follow is not None
-  assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.05
-  assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.2
+  assert planner.effective_t_follow > sm["starpilotPlan"].tFollow + 0.15
+  assert planner.effective_t_follow < sm["starpilotPlan"].tFollow + 0.45


@pytest.mark.parametrize("model_version", ["v11", "v12"])
@@ -354,10 +366,33 @@ def test_acc_mode_vision_lead_approach_cap_smooths_before_close_brake(model_vers

  assert planner_approach.mode == "acc"
  assert planner_close.mode == "acc"
-  assert planner_approach.output_a_target < -0.3
+  assert planner_approach.output_a_target < -0.5
  assert planner_close.output_a_target < planner_approach.output_a_target - 0.25


+@pytest.mark.parametrize("model_version", ["v11", "v12"])
+def test_acc_mode_low_speed_vision_stop_buffer_sets_should_stop_before_tiny_gap(model_version):
+  v_ego = 3.8
+
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=v_ego)
+  sm = make_sm(
+    v_ego,
+    desired_accel=0.1,
+    min_accel=-3.0,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=5.75, v_lead=0.58, a_lead=-0.1, radar=False, model_prob=0.99),
+  )
+  sm["starpilotPlan"].vCruise = v_ego + 4.0
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.mode == "acc"
+  assert planner.output_should_stop
+  assert planner.output_a_target < -1.0
+
+
@pytest.mark.parametrize("model_version", ["v11", "v12"])
 def test_acc_mode_damps_far_radar_mild_lead_brake_more_than_close_brake(model_version):
  far_v_ego = 29.26
@@ -13,10 +13,16 @@ from openpilot.starpilot.controls.lib.starpilot_acceleration import (


 class FakePlanner:
-  def __init__(self, *, v_cruise=0.0, slc_target=0.0, red_light=False, forcing_stop=False, disable_throttle=False):
+  def __init__(self, *, v_cruise=0.0, slc_target=0.0, slc_offset=0.0, overridden_speed=0.0,
+               red_light=False, forcing_stop=False, disable_throttle=False):
    self.v_cruise = v_cruise
    self.starpilot_weather = SimpleNamespace(weather_id=0, reduce_acceleration=0.0)
-    self.starpilot_vcruise = SimpleNamespace(slc_target=slc_target, forcing_stop=forcing_stop)
+    self.starpilot_vcruise = SimpleNamespace(
+      slc_target=slc_target,
+      slc_offset=slc_offset,
+      forcing_stop=forcing_stop,
+      slc=SimpleNamespace(overridden_speed=overridden_speed),
+    )
    self.starpilot_cem = SimpleNamespace(stop_light_detected=red_light)
    self.starpilot_following = SimpleNamespace(disable_throttle=disable_throttle)

@@ -32,6 +38,7 @@ def make_toggles(**overrides):
    "human_acceleration": False,
    "map_acceleration": False,
    "map_deceleration": False,
+    "set_speed_limit": True,
    "set_speed_offset": 0,
    "speed_limit_controller": True,
  }
@@ -44,9 +51,9 @@ def make_lead(status=False, d_rel=150.0, v_lead=0.0, a_lead_k=0.0):


 def make_sm(*, set_speed_kph=100.0, lead_one=None, lead_two=None, standstill=False, force_decel=False,
-            eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False):
+            eco_gear=False, sport_gear=False, force_coast=False, traffic_mode=False, v_ego_cluster=0.0):
  return {
-    "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill),
+    "carState": SimpleNamespace(vCruise=set_speed_kph, standstill=standstill, vEgoCluster=v_ego_cluster),
    "controlsState": SimpleNamespace(forceDecel=force_decel),
    "radarState": SimpleNamespace(
      leadOne=lead_one or make_lead(),
@@ -81,6 +88,33 @@ def test_slc_coast_window_does_not_require_starpilot_plan_message():
  assert accel.min_accel == pytest.approx(-0.02, abs=1e-3)


+def test_slc_coast_window_uses_effective_target_with_offset_and_cluster_diff():
+  raw_target = 58.0 * CV.MPH_TO_MS
+  slc_target = 45.0 * CV.MPH_TO_MS
+  slc_offset = 3.0 * CV.MPH_TO_MS
+  v_ego = 48.0 * CV.MPH_TO_MS
+  v_ego_cluster = v_ego + 0.4
+  accel = StarPilotAcceleration(FakePlanner(v_cruise=raw_target, slc_target=slc_target, slc_offset=slc_offset))
+  sm = make_sm(set_speed_kph=100.0, v_ego_cluster=v_ego_cluster)
+
+  accel.update(v_ego, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"]))
+
+  assert accel.min_accel == pytest.approx(-0.02, abs=1e-3)
+
+
+def test_slc_coast_window_disabled_when_set_speed_limit_is_off():
+  raw_target = 58.0 * CV.MPH_TO_MS
+  slc_target = 45.0 * CV.MPH_TO_MS
+  slc_offset = 3.0 * CV.MPH_TO_MS
+  v_ego = 48.0 * CV.MPH_TO_MS
+  accel = StarPilotAcceleration(FakePlanner(v_cruise=raw_target, slc_target=slc_target, slc_offset=slc_offset))
+  sm = make_sm(set_speed_kph=100.0, v_ego_cluster=v_ego + 0.4)
+
+  accel.update(v_ego, sm, make_toggles(deceleration_profile=DECELERATION_PROFILES["ECO"], set_speed_limit=False))
+
+  assert accel.min_accel == pytest.approx(A_CRUISE_MIN_ECO)
+
+
 def test_slc_coast_window_scales_by_profile_strength():
  v_ego = 65.0 * CV.MPH_TO_MS
  v_target = 60.0 * CV.MPH_TO_MS
@@ -0,0 +1,30 @@
+import pytest
+
+from openpilot.common.constants import CV
+from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target
+
+
+def test_active_slc_control_target_requires_set_speed_limit():
+  target = get_active_slc_control_target(
+    speed_limit_controller=True,
+    set_speed_limit=False,
+    slc_target=45.0 * CV.MPH_TO_MS,
+    slc_offset=3.0 * CV.MPH_TO_MS,
+    overridden_speed=0.0,
+    v_ego_diff=0.4,
+  )
+
+  assert target == 0.0
+
+
+def test_active_slc_control_target_applies_offset_and_cluster_diff():
+  target = get_active_slc_control_target(
+    speed_limit_controller=True,
+    set_speed_limit=True,
+    slc_target=45.0 * CV.MPH_TO_MS,
+    slc_offset=3.0 * CV.MPH_TO_MS,
+    overridden_speed=0.0,
+    v_ego_diff=0.4,
+  )
+
+  assert target == pytest.approx((48.0 * CV.MPH_TO_MS) - 0.4)
@@ -945,7 +945,7 @@ class StarPilotSLCQOLLayout(StarPilotPanel):
    super().__init__()
    self.CATEGORIES = [
      {
-        "title": tr_noop("Match Speed on Engage"),
+        "title": tr_noop("Auto Match Speed Limits"),
        "type": "toggle",
        "get_state": lambda: self._params.get_bool("SetSpeedLimit"),
        "set_state": lambda s: self._params.put_bool("SetSpeedLimit", s),
@@ -15,6 +15,7 @@ from openpilot.starpilot.common.accel_profile import (
  interpolate_accel_profile,
  normalize_deceleration_profile,
 )
+from openpilot.starpilot.controls.lib.starpilot_vcruise import get_active_slc_control_target
 from openpilot.starpilot.common.starpilot_variables import CITY_SPEED_LIMIT

 def cubic_interp(x, xp, fp):
@@ -196,9 +197,23 @@ class StarPilotAcceleration:
        raw_v_cruise_kph += starpilot_toggles.set_speed_offset
      raw_v_cruise = raw_v_cruise_kph * CV.KPH_TO_MS

+      v_ego_cluster = getattr(sm["carState"], "vEgoCluster", v_ego)
+      if v_ego_cluster is None:
+        v_ego_cluster = v_ego
+      v_ego_cluster = max(v_ego_cluster, v_ego)
+      v_ego_diff = v_ego_cluster - v_ego
+      effective_slc_target = get_active_slc_control_target(
+        getattr(starpilot_toggles, "speed_limit_controller", False),
+        getattr(starpilot_toggles, "set_speed_limit", False),
+        getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0),
+        getattr(self.starpilot_planner.starpilot_vcruise, "slc_offset", 0.0),
+        getattr(getattr(self.starpilot_planner.starpilot_vcruise, "slc", None), "overridden_speed", 0.0),
+        v_ego_diff,
+      )
      v_target = float(self.starpilot_planner.v_cruise or raw_v_cruise)
-      slc_target = float(getattr(self.starpilot_planner.starpilot_vcruise, "slc_target", 0.0))
-      slc_limited = slc_target > 0.0 and abs(v_target - slc_target) <= SLC_TARGET_EPS and v_target < raw_v_cruise - SLC_TARGET_EPS
+      if effective_slc_target > 0.0:
+        v_target = min(v_target, effective_slc_target)
+      slc_limited = effective_slc_target > 0.0 and abs(v_target - effective_slc_target) <= SLC_TARGET_EPS and effective_slc_target < raw_v_cruise - SLC_TARGET_EPS
      has_relevant_lead = any(lead_is_braking_relevant(lead, v_ego) for lead in (sm["radarState"].leadOne, sm["radarState"].leadTwo))
      stop_context = (
        sm["carState"].standstill or
@@ -27,6 +27,17 @@ OFFSET_FT_MIN = -20
 OFFSET_FT_MAX = 20


+def get_active_slc_control_target(speed_limit_controller, set_speed_limit, slc_target, slc_offset, overridden_speed, v_ego_diff):
+  if not speed_limit_controller or not set_speed_limit:
+    return 0.0
+
+  base_target = max(float(overridden_speed), float(slc_target) + float(slc_offset))
+  if base_target <= 0.0:
+    return 0.0
+
+  return max(0.0, base_target - float(v_ego_diff))
+
+
 class StarPilotVCruise:
  def __init__(self, StarPilotPlanner):
    self.starpilot_planner = StarPilotPlanner
@@ -184,8 +195,16 @@ class StarPilotVCruise:
      self.tracked_model_length = self.starpilot_planner.model_length

      targets = [self.csc_target, v_cruise]
-      if starpilot_toggles.speed_limit_controller:
-        targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff)
+      slc_control_target = get_active_slc_control_target(
+        starpilot_toggles.speed_limit_controller,
+        getattr(starpilot_toggles, "set_speed_limit", False),
+        self.slc_target,
+        self.slc_offset,
+        self.slc.overridden_speed,
+        v_ego_diff,
+      )
+      if slc_control_target > 0.0:
+        targets.append(slc_control_target)
      v_cruise = min([target if target >= CSC_MIN_SPEED else v_cruise for target in targets])

    return v_cruise