From 83e26d674f7ad5ce2c6fed8a2a0362be74bfa680 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Thu, 3 Mar 2022 03:27:24 -0800 Subject: [PATCH] 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 old-commit-hash: 7254db3906eedf06da9e6e6947233848599a7862 --- selfdrive/car/interfaces.py | 2 +- selfdrive/controls/lib/longcontrol.py | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) 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: