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

Keep terminal Force Stop targets closed during range refinement

Browse Source
This commit is contained in:
firestar5683
2026-11-10 16:34:06 -06:00
parent 8fa58cf8a8
commit b6d2afbc2a
2 changed files with 20 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
openpilot/starpilot/longitudinal/force_stop.py
+4 -1
View File
@@ -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))
openpilot/starpilot/longitudinal/tests/test_force_stop.py
+16 -3
View File
@@ -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()