mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-19 01:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Prevent premature stop-hold release

Browse Source
This commit is contained in:
rav4kumar
2026-07-18 00:02:57 -07:00
parent 41cfac46d7
commit a90286b4a5
2 changed files with 29 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py
+2 -1
View File
@@ -77,6 +77,7 @@ STOP_HOLD_PRECONDITION_SPEED = 0.05
STOP_HOLD_PRECONDITION_ACCEL_MAX = -0.25
STOP_HOLD_PRECONDITION_EXTRA = 0.20
STOPPED_LEAD_SPEED = 0.30
STOP_HOLD_EXIT_CAP = 0.80
STOP_HOLD_EXIT_FRAMES = 4
MAX_LEAD_ACCEL_TAU = 10.0
VEGO_NOISE_TOLERANCE = 0.10
@@ -379,7 +380,7 @@ class AccelController:
path.pace = max(planner_speed, 0.0)
if path.state == AccelControllerState.stopHold:
moving_lead = envelope.departure_lead_speed > STOPPED_LEAD_SPEED and math.isfinite(raw_cap)
moving_lead = raw_cap > STOP_HOLD_EXIT_CAP and filtered_cap > STOP_HOLD_EXIT_CAP
confirmed_lead_loss = not math.isfinite(raw_cap) and not math.isfinite(filtered_cap)
path.departure_frames = path.departure_frames + 1 if moving_lead or confirmed_lead_loss else 0
if path.departure_frames < STOP_HOLD_EXIT_FRAMES:
sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py
+27 -9
View File
@@ -21,7 +21,10 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
MOVING_LEAD_DECEL_ACCEL_SLEW_RATE,
MOVING_LEAD_DECEL_RELEASE_RATE,
RELIEF_CONFIRM_FRAMES,
STOP_HOLD_EXIT_CAP,
STOP_HOLD_EXIT_FRAMES,
STOP_HOLD_PRECONDITION_ACCEL_MAX,
STOPPED_LEAD_SPEED,
AccelController,
AccelControllerState,
AccelProfile,
@@ -655,7 +658,8 @@ class TestPaceGovernor:
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
held = update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6)
waiting = [update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) for _ in range(13)]
confirmations = [update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) for _ in range(STOP_HOLD_EXIT_FRAMES)]
confirmation_frames = CAP_FILTER_FRAMES // 2 + STOP_HOLD_EXIT_FRAMES
confirmations = [update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6) for _ in range(confirmation_frames)]
released = update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1, stock_accel_max=1.6)
assert held.state == AccelControllerState.stopHold
@@ -675,18 +679,32 @@ class TestPaceGovernor:
assert released.target_speed >= confirmations[-1].target_speed
assert released.launching
def test_slow_queue_creep_exits_after_four_frames(self):
def test_slow_queue_creep_waits_for_safe_exit_cap(self):
controller = make_controller()
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
creeping = make_radar(make_lead(status=True, d_rel=6.1, v_lead_k=0.5))
update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1)
results = [update(controller, creeping, base_speed=8.0, v_ego=0.1, planner_speed=0.1) for _ in range(STOP_HOLD_EXIT_FRAMES)]
released = update(controller, creeping, base_speed=8.0, v_ego=0.1, planner_speed=0.1)
assert all(result.state == AccelControllerState.stopHold for result in results[:-1])
assert results[-1].state == AccelControllerState.release
assert results[-1].target_speed > 0.0
assert released.target_speed > 0.0
assert all(result.raw_energy_cap <= STOP_HOLD_EXIT_CAP for result in results)
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 for result in results)
def test_decelerating_departure_below_exit_cap_does_not_gas_then_brake(self):
controller = make_controller()
stopped = make_radar(make_lead(status=True, d_rel=2.6, v_lead_k=0.0))
decelerating = make_radar(make_lead(status=True, d_rel=2.6, v_lead_k=0.675, a_lead_k=-1.8))
update(controller, stopped, profile=AccelProfile.eco, base_speed=8.0, v_ego=0.0, planner_speed=0.0)
results = [
update(controller, decelerating, profile=AccelProfile.eco, base_speed=8.0, v_ego=0.0, planner_speed=0.0)
for _ in range(STOP_HOLD_EXIT_FRAMES + 2)
]
assert all(STOPPED_LEAD_SPEED < result.raw_energy_cap < STOP_HOLD_EXIT_CAP for result in results)
assert all(result.state == AccelControllerState.stopHold for result in results)
assert all(result.target_speed == 0.0 for result in results)
assert all(result.effective_accel_max == STOP_HOLD_PRECONDITION_ACCEL_MAX for result in results)
assert all(not result.launching for result in results)
def test_far_stopped_lead_does_not_enter_stop_hold(self):
far_stopped = make_radar(make_lead(status=True, d_rel=100.0, v_lead_k=0.0))
@@ -701,7 +719,7 @@ class TestPaceGovernor:
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1)
for _ in range(STOP_HOLD_EXIT_FRAMES - 1):
for _ in range(CAP_FILTER_FRAMES // 2 + STOP_HOLD_EXIT_FRAMES - 1):
update(controller, moving, base_speed=8.0, v_ego=0.1, planner_speed=0.1)
interrupted = update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1)
@@ -713,7 +731,7 @@ class TestPaceGovernor:
stopped = make_radar(make_lead(status=True, d_rel=6.0, v_lead_k=0.0))
moving = make_radar(make_lead(status=True, d_rel=20.0, v_lead_k=5.0))
update(controller, stopped, base_speed=8.0, v_ego=0.1, planner_speed=0.1, previous_should_stop=True)
for _ in range(STOP_HOLD_EXIT_FRAMES):
for _ in range(CAP_FILTER_FRAMES // 2 + STOP_HOLD_EXIT_FRAMES):
result = update(
controller,
moving,