LongControl: fix control state oscillation (#23333)

* check if plan is to accelerate or not

* remove comment

* needed to see it

needed to see it

* test logic with Toyota TSS2

* hackily log conditions

* Revert "hackily log conditions"

This reverts commit 7a6b5691e2b982c7a1e3911d980fb7530647affd.

Revert "test logic with Toyota TSS2"

This reverts commit e583d6e3caaa5f5b896165cd7949dfc1caabfd9a.

order

* revert

* fix

selfdrive/car/interfaces.py

@@ -88,7 +88,7 @@ class CarInterfaceBase(ABC):
     ret.stopAccel = -2.0
     ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
     ret.vEgoStopping = 0.5
-    ret.vEgoStarting = 0.5  # needs to be >= vEgoStopping to avoid state transition oscillation
+    ret.vEgoStarting = 0.5
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]

selfdrive/controls/lib/longcontrol.py

@@ -12,14 +12,15 @@ ACCEL_MIN_ISO = -3.5  # m/s^2
 ACCEL_MAX_ISO = 2.0  # m/s^2
 
 
-def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future,
-                             brake_pressed, cruise_standstill):
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target,
+                             v_target_future, brake_pressed, cruise_standstill):
   """Update longitudinal control state machine"""
+  accelerating = v_target_future > v_target
   stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
                        (v_ego < CP.vEgoStopping and
-                        (v_target_future < CP.vEgoStopping or brake_pressed))
+                        ((v_target_future < CP.vEgoStopping and not accelerating) or brake_pressed))
 
-  starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
+  starting_condition = v_target_future > CP.vEgoStarting and accelerating and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -83,7 +84,7 @@ class LongControl():
     # Update state machine
     output_accel = self.last_output_accel
     self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
-                                                       v_target_future, CS.brakePressed,
+                                                       v_target, v_target_future, CS.brakePressed,
                                                        CS.cruiseState.standstill)
 
     if self.long_control_state == LongCtrlState.off or CS.gasPressed: