diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 2de44b4784..e6f546acc3 100644 --- a/selfdrive/car/interfaces.py +++ b/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.] diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 3ba50fd0cf..21682d8263 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/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: