Skip to content

Longcontrol: fix control state oscillation #23333

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 8 commits into from
Mar 3, 2022
Merged

Longcontrol: fix control state oscillation #23333

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
cbd8fa7
check if plan is to accelerate or not
sshane Dec 28, 2021
aaa57db
remove comment
sshane Dec 28, 2021
07d1336
needed to see it
sshane Dec 28, 2021
c371e19
test logic with Toyota TSS2
sshane Dec 28, 2021
04717f3
hackily log conditions
sshane Dec 28, 2021
a2da6dd
Revert "hackily log conditions"
sshane Dec 29, 2021
13f0fac
revert
sshane Mar 1, 2022
e6b39fe
fix
sshane Mar 1, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion selfdrive/car/interfaces.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ def get_std_params(candidate, fingerprint):
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.]
Expand Down
11 changes: 6 additions & 5 deletions selfdrive/controls/lib/longcontrol.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,15 @@
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
Expand Down Expand Up @@ -83,7 +84,7 @@ def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
# 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:
Expand Down