From 1d2d18fa2a31c8eb5b7f4031afe5c486dfaa693a Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Sun, 1 Nov 2026 22:25:07 -0600 Subject: [PATCH] Force Stop Temp Hold --- .../starpilot/longitudinal/force_stop.py | 17 ++++- .../longitudinal/tests/test_force_stop.py | 62 ++++++++++++------- .../tests/test_force_stop_runtime.py | 21 +++---- 3 files changed, 62 insertions(+), 38 deletions(-) diff --git a/openpilot/starpilot/longitudinal/force_stop.py b/openpilot/starpilot/longitudinal/force_stop.py index d4a6be77b4..6eb034f95f 100644 --- a/openpilot/starpilot/longitudinal/force_stop.py +++ b/openpilot/starpilot/longitudinal/force_stop.py @@ -5,6 +5,9 @@ import math from openpilot.common.realtime import DT_MDL +STANDSTILL_HOLD_NS = 2_000_000_000 +STANDSTILL_CLEAR_NS = 750_000_000 + @dataclass(frozen=True) class StopTune: @@ -81,6 +84,7 @@ class ForceStop: self.activation_gate = False self.stop_seen_ns = 0 self.standstill_since_ns = 0 + self.standstill_clear_ns = 0 self.standstill_reason = None self.previously_enabled = False self.distance = 0.0 @@ -88,6 +92,7 @@ class ForceStop: def _clear_standstill(self): self.standstill_since_ns = 0 + self.standstill_clear_ns = 0 self.standstill_reason = None def step(self, frame: StopFrame, tune: StopTune, offset_ft: int = 0) -> StopPlan: @@ -131,8 +136,6 @@ class ForceStop: self.from_light = False self.light_clear_ns = 0 stop_scene = available and (active or frame.raw_model_stopped) - # Reopened geometry may withdraw a moving approach; it cannot classify - # a stationary hold as a green light. go_scene = available and horizon >= 50.0 and not frame.raw_model_stopped and not frame.model_should_stop held_before = self.previously_enabled and self.standstill_reason is not None engaged_stopped = not self.previously_enabled and frame.standstill @@ -140,12 +143,20 @@ class ForceStop: self.standstill_since_ns = now self.standstill_reason = 'unclassified' self.distance = 0 + if self.standstill_reason is not None: + if not available or stop_scene: + self.standstill_clear_ns = 0 + elif not self.standstill_clear_ns: + self.standstill_clear_ns = now + hold_expired = self.standstill_reason is not None and ( + now - self.standstill_since_ns >= STANDSTILL_HOLD_NS or + self.standstill_clear_ns > 0 and now - self.standstill_clear_ns >= STANDSTILL_CLEAR_NS) if held_before and frame.standstill and frame.resume_requested: self.resume_until_ns = now + 6_000_000_000 if abs(speed) >= 2.0 or frame.lead_present or frame.tracking_lead: self.resume_until_ns = 0 driver_release = frame.pedal_override or frame.resume_requested - if self.standstill_reason is not None and driver_release: + if self.standstill_reason is not None and (driver_release or hold_expired): self.override_until_ns = now + 10_000_000_000 self._clear_standstill() held = self.standstill_reason is not None diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop.py b/openpilot/starpilot/longitudinal/tests/test_force_stop.py index ff8c4bbec2..d48373a992 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop.py @@ -109,14 +109,13 @@ class ForceStopTests(unittest.TestCase): held = self.owner.step(frame(35, speed_mps=0., standstill=True, horizon_m=None, road_curvature=None, light=False, perception_available=False), self.tune) self.assertTrue(held.should_stop) - for tick in range(36, 76): + for tick in range(36, 75): self.assertTrue(self.owner.step(frame(tick, speed_mps=-1e-44, standstill=True, horizon_m=None, road_curvature=None, light=False, perception_available=False), self.tune).should_stop) - # Reopened geometry cannot classify a held stop or authorize departure. - for tick in range(76, 92): - plan = self.owner.step(frame(tick, speed_mps=0., standstill=True, horizon_m=190., light=False), self.tune) - self.assertTrue(plan.forcing) - self.assertTrue(plan.manual_hold) + expired = self.owner.step(frame(75, speed_mps=0., standstill=True, horizon_m=None, + road_curvature=None, light=False, perception_available=False), self.tune) + self.assertFalse(expired.forcing) + self.assertFalse(expired.manual_hold) def test_missing_perception_distance_uses_actual_model_interval(self): for tick in range(15): @@ -152,34 +151,49 @@ class ForceStopTests(unittest.TestCase): self.assertFalse(plan.forcing) self.assertIsNone(plan.speed_ceiling_mps) - def test_unclassified_standstill_holds_until_driver_release(self): - for tick in range(80): + def test_standstill_hold_expires_after_two_seconds_without_rearming(self): + for tick in range(40): plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune) self.assertTrue(plan.forcing) self.assertTrue(plan.should_stop) - for tick in range(80, 96): - plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., raw_model_stopped=False, - light=False, standstill=True), self.tune) - self.assertTrue(plan.forcing) - self.assertTrue(plan.should_stop) - self.assertTrue(plan.manual_hold) - for action in ('resume_requested', 'pedal_override'): - released = self.owner.step(frame(96 if action == 'resume_requested' else 97, speed_mps=0., - horizon_m=190., light=False, standstill=True, **{action: True}), self.tune) - self.assertFalse(released.manual_hold) - self.assertFalse(released.forcing) - self.assertFalse(released.should_stop) + self.assertTrue(plan.manual_hold) + for tick in range(40, 240): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune) + self.assertFalse(plan.forcing) + self.assertFalse(plan.should_stop) + self.assertFalse(plan.manual_hold) - def test_manual_hold_survives_clear_geometry_and_roll_until_each_driver_action(self): + def test_moving_stop_hold_expires_after_two_seconds(self): + for tick in range(15): + self.owner.step(frame(tick, horizon_m=30.), self.tune) + for tick in range(15, 55): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=30., standstill=True), self.tune) + self.assertTrue(plan.manual_hold) + plan = self.owner.step(frame(55, speed_mps=0., horizon_m=30., standstill=True), self.tune) + self.assertFalse(plan.forcing) + self.assertFalse(plan.should_stop) + + def test_standstill_hold_releases_after_open_scene_dwell(self): + self.assertTrue(self.owner.step(frame(0, speed_mps=0., horizon_m=30., raw_model_stopped=True, + standstill=True), self.tune).manual_hold) + for tick in range(1, 16): + plan = self.owner.step(frame(tick, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune) + self.assertTrue(plan.manual_hold) + plan = self.owner.step(frame(16, speed_mps=0., horizon_m=190., light=False, standstill=True), self.tune) + self.assertFalse(plan.manual_hold) + self.assertFalse(plan.forcing) + self.assertFalse(plan.should_stop) + + def test_manual_hold_survives_roll_until_each_driver_action(self): for action in ('resume_requested', 'pedal_override'): self.owner.reset() held = self.owner.step(frame(0, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True), self.tune) self.assertTrue(held.manual_hold) - for tick in range(1, 401): - plan = self.owner.step(frame(tick, speed_mps=.01, horizon_m=190., light=False, standstill=False), self.tune) + for tick in range(1, 31): + plan = self.owner.step(frame(tick, speed_mps=.01, horizon_m=30., raw_model_stopped=True, standstill=False), self.tune) self.assertTrue(plan.manual_hold) self.assertEqual(plan.speed_ceiling_mps, 0.) - release = self.owner.step(frame(401, speed_mps=0., horizon_m=190., light=False, standstill=True, + release = self.owner.step(frame(31, speed_mps=0., horizon_m=30., raw_model_stopped=True, standstill=True, **{action: True}), self.tune) self.assertFalse(release.manual_hold) self.assertFalse(release.forcing) diff --git a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py index 628126cfb7..efb9ce2458 100644 --- a/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py +++ b/openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py @@ -139,15 +139,13 @@ class ForceStopRuntimeTests(unittest.TestCase): plan = self.sample(sm) self.assertTrue(plan.should_stop) self.assertEqual(plan.speed_ceiling_mps, 0.) - for tick in range(100, 116): - plan = self.sample(Frame(tick, horizon=190., stopped=True)) self.assertTrue(plan.forcing) self.assertTrue(plan.manual_hold) - now = BASE + 5_800_000_000 + 1_000_000 - for item in (button_event(577), button_event(578, [('resumeCruise', True)]), - button_event(579, [('resumeCruise', False)])): + now = BASE + 5_000_000_000 + 1_000_000 + for item in (button_event(497), button_event(498, [('resumeCruise', True)]), + button_event(499, [('resumeCruise', False)])): self.owner.resume.observe(item, now_ns=now, drive_id=DRIVE) - released = self.sample(Frame(116, horizon=190., stopped=True)) + released = self.sample(Frame(100, horizon=190., stopped=True)) self.assertFalse(released.manual_hold) self.assertFalse(released.forcing) self.assertIsNone(released.speed_ceiling_mps) @@ -219,13 +217,14 @@ class ForceStopRuntimeTests(unittest.TestCase): self.assertTrue(output.forcing) self.assertTrue(output.should_stop) self.assertEqual(output.speed_ceiling_mps, 0.) - for tick in range(41, 101): + for tick in range(41, 80): output = self.sample(Frame(tick, stopped=True, horizon=30.)) self.assertTrue(output.should_stop) - for tick in range(101, 201): - output = self.sample(Frame(tick, stopped=True, horizon=192.)) - self.assertTrue(output.forcing) - self.assertTrue(output.manual_hold) + self.assertTrue(output.manual_hold) + for tick in range(80, 200): + output = self.sample(Frame(tick, stopped=True, horizon=30.)) + self.assertFalse(output.forcing) + self.assertFalse(output.manual_hold) def test_manual_hold_publishes_dedicated_bit_and_release_withdraws_same_cycle(self): from types import SimpleNamespace