From 5c30a69e10dde39e9fccf5ea6169249bd4a8c232 Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Sat, 16 May 2026 08:52:00 -0500
Subject: [PATCH] final stop

---
 .../controls/lib/longitudinal_planner.py      | 58 +++++++++++++++++-
 .../tests/test_longitudinal_planner.py        | 59 +++++++++++++++++++
 2 files changed, 114 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 3617d8013..216bd1d76 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -132,10 +132,18 @@ VISION_LOW_SPEED_STOP_BUFFER_MIN_BRAKE = 1.25
 VISION_LOW_SPEED_STOP_BUFFER_BRAKE_GAIN = 0.25
 VISION_CLOSE_STOP_HOLD_MAX_EGO_SPEED = 0.75
 VISION_CLOSE_STOP_HOLD_MAX_LEAD_SPEED = 0.8
-VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 2.8
+VISION_CLOSE_STOP_HOLD_MAX_DISTANCE = 3.5
 VISION_CLOSE_STOP_HOLD_MIN_MODEL_PROB = 0.95
-VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.12
-VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.28
+VISION_CLOSE_STOP_HOLD_MIN_BRAKE = 0.20
+VISION_CLOSE_STOP_HOLD_MAX_BRAKE = 0.36
+VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED = 2.5
+VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED = 3.5
+VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE = 4.2
+VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB = 0.98
+VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA = -0.1
+VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA = 1.5
+VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE = 0.18
+VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE = 0.40
 MANUAL_STOP_RESUME_OVERRIDE_TIME = 3.0
 MANUAL_STOP_RESUME_OVERRIDE_MAX_SPEED = 2.0
 MANUAL_STOP_RESUME_OVERRIDE_MIN_ACCEL = 0.2
@@ -756,6 +764,35 @@ class LongitudinalPlanner:
     hold_brake = float(np.clip(hold_brake, VISION_CLOSE_STOP_HOLD_MIN_BRAKE, VISION_CLOSE_STOP_HOLD_MAX_BRAKE))
     return max(accel_min, -hold_brake)
 
+  def get_vision_close_release_hold_cap(self, lead, v_ego, accel_min, should_stop):
+    if should_stop or lead is None or not lead.status or bool(getattr(lead, "radar", False)):
+      return None
+
+    lead_prob = float(getattr(lead, "modelProb", 0.0))
+    if lead_prob < VISION_CLOSE_RELEASE_HOLD_MIN_MODEL_PROB:
+      return None
+
+    lead_speed = max(float(lead.vLead), 0.0)
+    lead_delta = lead_speed - float(v_ego)
+    if (
+      float(v_ego) > VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED or
+      lead_speed > VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_SPEED or
+      float(lead.dRel) > VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE or
+      lead_delta < VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA or
+      lead_delta > VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA
+    ):
+      return None
+
+    distance_factor = float(np.clip((VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE - float(lead.dRel)) /
+                                    max(VISION_CLOSE_RELEASE_HOLD_MAX_DISTANCE - 2.8, 0.1), 0.0, 1.0))
+    speed_factor = float(np.clip(float(v_ego) / max(VISION_CLOSE_RELEASE_HOLD_MAX_EGO_SPEED, 0.1), 0.0, 1.0))
+    delta_factor = float(np.clip((VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA - lead_delta) /
+                                 max(VISION_CLOSE_RELEASE_HOLD_MAX_LEAD_DELTA - VISION_CLOSE_RELEASE_HOLD_MIN_LEAD_DELTA, 0.1),
+                                 0.0, 1.0))
+    hold_brake = VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE + 0.12 * distance_factor + 0.06 * speed_factor + 0.04 * delta_factor
+    hold_brake = float(np.clip(hold_brake, VISION_CLOSE_RELEASE_HOLD_MIN_BRAKE, VISION_CLOSE_RELEASE_HOLD_MAX_BRAKE))
+    return max(accel_min, -hold_brake)
+
   def _update_manual_stop_resume_override(self, sm):
     now_t = time.monotonic()
     lead = sm["radarState"].leadOne
@@ -1618,6 +1655,17 @@ class LongitudinalPlanner:
         self.a_desired = min(self.a_desired, close_stop_hold_cap)
         output_a_target = min(output_a_target, close_stop_hold_cap)
 
+    close_release_hold_cap = None
+    if lead_control_active and not output_should_stop:
+      close_release_hold_caps = [
+        cap for cap in (
+          self.get_vision_close_release_hold_cap(self.lead_one, scene_v_ego, vision_cap_accel_min, output_should_stop),
+          self.get_vision_close_release_hold_cap(self.lead_two, scene_v_ego, vision_cap_accel_min, output_should_stop),
+        ) if cap is not None
+      ]
+      if close_release_hold_caps:
+        close_release_hold_cap = min(close_release_hold_caps)
+
     if lead_one_active:
       lead_catchup_accel_cap = self.get_lead_catchup_accel_cap(self.lead_one, scene_v_ego, effective_t_follow)
       if lead_catchup_accel_cap is not None:
@@ -1688,6 +1736,10 @@ class LongitudinalPlanner:
     output_accel_max = no_throttle_output_max if not self.allow_throttle else accel_limits_turns[1]
     output_a_target = float(np.clip(output_a_target, output_accel_min, output_accel_max))
 
+    if close_release_hold_cap is not None:
+      self.a_desired = min(self.a_desired, close_release_hold_cap)
+      output_a_target = min(output_a_target, close_release_hold_cap)
+
     force_stop_handoff = bool(
       getattr(sm['starpilotPlan'], 'forcingStop', False) and
       not lead_control_active and
diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py
index 8cadadc57..bd4293c97 100644
--- a/selfdrive/controls/tests/test_longitudinal_planner.py
+++ b/selfdrive/controls/tests/test_longitudinal_planner.py
@@ -839,6 +839,65 @@ def test_acc_mode_close_moving_vision_lead_keeps_negative_output_while_should_st
   assert all(output <= -0.02 for output in outputs[2:])
 
 
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_acc_mode_close_near_standstill_vision_lead_keeps_meaningful_brake_floor(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=0.017)
+
+  sm = make_sm(
+    0.017,
+    desired_accel=-0.06,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=2.83, v_lead=0.09, a_lead=0.10, radar=False, model_prob=0.9999),
+  )
+  sm["controlsState"].longControlState = LongCtrlState.stopping
+  sm["starpilotPlan"].vCruise = 10.0
+  sm["modelV2"].action.shouldStop = True
+
+  planner.update(sm, make_toggles(model_version))
+
+  assert planner.output_should_stop
+  assert planner.output_a_target <= -0.20
+
+
+@pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
+def test_acc_mode_close_opening_vision_lead_does_not_drop_to_zero_after_stop_release(model_version):
+  CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC)
+  planner = LongitudinalPlanner(CP, init_v=1.49)
+  toggles = make_toggles(model_version)
+
+  sm_stop = make_sm(
+    1.488,
+    desired_accel=-1.64,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=3.433, v_lead=1.469, a_lead=0.58, radar=False, model_prob=0.9996),
+  )
+  sm_stop["controlsState"].longControlState = LongCtrlState.stopping
+  sm_stop["starpilotPlan"].vCruise = 10.0
+  sm_stop["modelV2"].action.shouldStop = True
+  planner.update(sm_stop, toggles)
+
+  sm_release = make_sm(
+    1.420,
+    desired_accel=0.0,
+    min_accel=-0.5,
+    experimental_mode=False,
+    tracking_lead=True,
+    lead_one=make_lead(status=True, d_rel=3.568, v_lead=1.679, a_lead=0.66, radar=False, model_prob=0.9994),
+  )
+  sm_release["controlsState"].longControlState = LongCtrlState.pid
+  sm_release["starpilotPlan"].vCruise = 10.0
+  sm_release["modelV2"].action.shouldStop = False
+  planner.update(sm_release, toggles)
+
+  assert not planner.output_should_stop
+  assert planner.output_a_target <= -0.18
+
+
 @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14"])
 def test_acc_mode_tracked_vision_model_brake_floor_prevents_positive_output_on_slower_lead(model_version):
   v_ego = 19.1