mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 20:03:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Longcontrol: engage into stopped state when needed (#33042)

Browse Source 
test stay stopped
This commit is contained in:
Harald Schäfer
2024-07-23 13:55:30 -07:00
committed by GitHub
parent fbbd4ee692
commit 3bc1b173d2
2 changed files with 65 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
selfdrive/controls/lib/longcontrol.py
View File

@@ -22,22 +22,26 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
long_control_state = LongCtrlState.off
else:
if long_control_state in (LongCtrlState.off, LongCtrlState.pid):
long_control_state = LongCtrlState.pid
if stopping_condition:
if long_control_state == LongCtrlState.off:
if not starting_condition:
long_control_state = LongCtrlState.stopping
else:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
else:
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.stopping:
if starting_condition and CP.startingState:
long_control_state = LongCtrlState.starting
elif starting_condition:
long_control_state = LongCtrlState.pid
elif long_control_state == LongCtrlState.starting:
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
if stopping_condition:
long_control_state = LongCtrlState.stopping
elif started_condition:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:

56
selfdrive/controls/tests/test_longcontrol.py Normal file
View File

@@ -0,0 +1,56 @@
from cereal import car
from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState, long_control_state_trans
class TestLongControlStateTransition:
def test_stay_stopped(self):
CP = car.CarParams.new_message()
active = True
current_state = LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
active = False
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.off
def test_engage():
CP = car.CarParams.new_message()
active = True
current_state = LongCtrlState.off
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid
def test_starting():
CP = car.CarParams.new_message(startingState=True, vEgoStarting=0.5)
active = True
current_state = LongCtrlState.starting
next_state = long_control_state_trans(CP, active, current_state, v_ego=0.1,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.starting
next_state = long_control_state_trans(CP, active, current_state, v_ego=1.0,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid