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

longcontrol: remove starting state (#38340)

Browse Source
This commit is contained in:
Adeeb Shihadeh
2026-07-20 16:30:42 -07:00
committed by GitHub
parent b1f2e638de
commit 031b1ad0a3
3 changed files with 27 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo
+1 -1
Submodule opendbc_repo updated: 78a1c9e73d...938043e24a
openpilot/selfdrive/controls/lib/longcontrol.py
+6 -17
View File
@@ -10,8 +10,7 @@ CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N]
LongCtrlState = car.CarControl.Actuators.LongControlState
def long_control_state_trans(CP, active, long_control_state, v_ego,
should_stop, brake_pressed, cruise_standstill):
def long_control_state_trans(active, long_control_state, should_stop, brake_pressed, cruise_standstill):
starting_condition = (not should_stop and
not cruise_standstill and
not brake_pressed)
@@ -23,22 +22,17 @@ def long_control_state_trans(CP, active, long_control_state, v_ego,
if long_control_state == LongCtrlState.off:
if not starting_condition:
long_control_state = LongCtrlState.stopping
elif 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:
if starting_condition:
long_control_state = LongCtrlState.pid
elif long_control_state in [LongCtrlState.starting, LongCtrlState.pid]:
elif long_control_state == LongCtrlState.pid:
if should_stop:
long_control_state = LongCtrlState.stopping
elif v_ego > CP.vEgoStarting:
long_control_state = LongCtrlState.pid
return long_control_state
class LongControl:
@@ -58,9 +52,8 @@ class LongControl:
self.pid.neg_limit = accel_limits[0]
self.pid.pos_limit = accel_limits[1]
self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo,
should_stop, CS.brakePressed,
CS.cruiseState.standstill)
self.long_control_state = long_control_state_trans(active, self.long_control_state, should_stop,
CS.brakePressed, CS.cruiseState.standstill)
if self.long_control_state == LongCtrlState.off:
self.reset()
output_accel = 0.
@@ -72,10 +65,6 @@ class LongControl:
output_accel -= self.CP.stoppingDecelRate * DT_CTRL
self.reset()
elif self.long_control_state == LongCtrlState.starting:
output_accel = self.CP.startAccel
self.reset()
else: # LongCtrlState.pid
error = a_target - CS.aEgo
output_accel = self.pid.update(error, speed=CS.vEgo,
openpilot/selfdrive/controls/tests/test_longcontrol.py
+20 -36
View File
@@ -1,56 +1,40 @@
from opendbc.car.structs 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,
next_state = long_control_state_trans(active, current_state,
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,
next_state = long_control_state_trans(active, current_state,
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,
next_state = long_control_state_trans(active, current_state,
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,
next_state = long_control_state_trans(active, current_state,
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,
next_state = long_control_state_trans(active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.off
def test_engage(self):
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(self):
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
def test_engage():
active = True
current_state = LongCtrlState.off
next_state = long_control_state_trans(active, current_state,
should_stop=True, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(active, current_state,
should_stop=False, brake_pressed=True, cruise_standstill=False)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=True)
assert next_state == LongCtrlState.stopping
next_state = long_control_state_trans(active, current_state,
should_stop=False, brake_pressed=False, cruise_standstill=False)
assert next_state == LongCtrlState.pid