mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-11-08 13:33:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

Force Stop Temp Hold

Browse Source
This commit is contained in:
whoisdomi
2026-11-01 22:25:07 -06:00
parent 42a1244e1b
commit 1d2d18fa2a
3 changed files with 62 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/starpilot/longitudinal/force_stop.py
+14 -3
View File
@@ -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
openpilot/starpilot/longitudinal/tests/test_force_stop.py
+38 -24
View File
@@ -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)
openpilot/starpilot/longitudinal/tests/test_force_stop_runtime.py
+10 -11
View File
@@ -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