From b6d2afbc2a55d7ecba10cdc22e1eabaffca902be Mon Sep 17 00:00:00 2001
From: firestar5683 <168790843+firestar5683@users.noreply.github.com>
Date: Tue, 10 Nov 2026 16:34:06 -0600
Subject: [PATCH] Keep terminal Force Stop targets closed during range
 refinement

---
 .../starpilot/longitudinal/force_stop.py      |  5 ++++-
 .../longitudinal/tests/test_force_stop.py     | 19 ++++++++++++++++---
 2 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py
index e969f710ed..dfbb2ff244 100644
--- a/openpilot/starpilot/longitudinal/force_stop.py
+++ b/openpilot/starpilot/longitudinal/force_stop.py
@@ -314,10 +314,13 @@ class ForceStop:
             not self.sign_confirmed and not frame.model_should_stop):
           # A moving model preview is not a fixed world endpoint to count down.
           self.distance = self.distance_cap = horizon
+        terminal_distance = self.distance
         if sign_estimate is not None and (self.sign_confirmed or frame.sign and available and horizon < SIGN_MODEL_AGREE_M):
           self.distance = min(sign_estimate, cap)
         if self.queue_line is not None and self.sign_confirmed:
           self.distance = self.queue_line
+        if tune.distance_control and self.stop_requested:
+          self.distance = min(self.distance, terminal_distance)
         effective = self.distance + offset + tune.distance_bias_m
         decel = SIGN_APPROACH_DECEL if frame.sign else MODEL_APPROACH_DECEL
         ceiling = math.sqrt(2 * decel * max(0, effective - tune.handoff_m))
@@ -346,7 +349,7 @@ class ForceStop:
     # The MPC obstacle includes its standing-distance term; adding it at the
     # native boundary avoids moving the actual stop line by that buffer.
     obstacle = (stop_length * .93 + offset + tune.distance_bias_m
-                if stop_length > 0 and stop_length + offset > tune.handoff_m else None)
+                if stop_length > 0 and stop_length + max(0.0, offset) > tune.handoff_m else None)
     if tune.distance_control and self.forcing:
       obstacle = max(0.0,min(self.distance * .93 + offset + tune.distance_bias_m,
                              (self.distance + offset + tune.distance_bias_m) * .93))
diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py
index 0a55491b67..6c87028c1a 100644
--- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py
+++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py
@@ -528,15 +528,28 @@ class MovingPreviewTests(unittest.TestCase):
     after = owner.step(preview_frame(15, horizon_m=None, road_curvature=None, perception_available=False, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
     self.assertAlmostEqual(after.tracked_distance_m, before.tracked_distance_m - 0.2)
 
-  def test_confirmed_stop_and_sign_cannot_extend(self):
+  def test_confirmed_stop_and_sign_cannot_resume_preview_tracking(self):
     for kw in [{'model_should_stop': True}, {'sign': True}]:
       owner = ForceStop()
       owner.last_model_ns = BASE
       owner.distance = owner.distance_cap = 8.0
       owner.forcing = True
       owner.timer = 1.0
-      after = owner.step(preview_frame(1, horizon_m=35.0, moving_preview=True, **kw), vehicle_tune('HYUNDAI_IONIQ_6'))
-      self.assertLess(after.tracked_distance_m, 8.0)
+      owner.entry_speed = 4.0
+      owner.preview_tracking = True
+      fixed = owner.step(preview_frame(1, horizon_m=35.0, moving_preview=True, **kw), vehicle_tune('HYUNDAI_IONIQ_6'))
+      after = owner.step(preview_frame(2, horizon_m=50.0, moving_preview=True), vehicle_tune('HYUNDAI_IONIQ_6'))
+      self.assertFalse(owner.preview_tracking)
+      self.assertLess(after.tracked_distance_m, fixed.tracked_distance_m)
+
+  def test_terminal_stop_distance_cannot_reopen_on_new_sign_range(self):
+    owner = ForceStop()
+    for tick in range(15):
+      before = owner.step(preview_frame(tick, horizon_m=5.0), vehicle_tune('HYUNDAI_IONIQ_6'))
+    self.assertTrue(before.should_stop)
+    after = owner.step(preview_frame(15, horizon_m=35.0, sign=True, vision_distance_m=30.), vehicle_tune('HYUNDAI_IONIQ_6'))
+    self.assertTrue(after.should_stop)
+    self.assertLess(after.tracked_distance_m, before.tracked_distance_m)
 
   def test_once_stationary_endpoint_seen_it_cannot_become_a_moving_preview_again(self):
     owner = ForceStop()