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

test stay stopped

selfdrive/controls/lib/longcontrol.py

@@ -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:

selfdrive/controls/tests/test_longcontrol.py

@@ -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