From da14c6d7708799543e53c8e326fa50c5a1e6a10f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Tue, 3 Sep 2024 19:44:14 -0700 Subject: [PATCH] comment old-commit-hash: adf1ef88b692132c37d865fc973189fdf971464f --- selfdrive/controls/lib/longitudinal_planner.py | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d805d6e252..5441b686a8 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -12,7 +12,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC -from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error +from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N, get_speed_error, V_CRUISE_UNSET from openpilot.selfdrive.car.cruise import V_CRUISE_MAX from openpilot.common.swaglog import cloudlog @@ -69,7 +69,6 @@ class LongitudinalPlanner: self.mpc = LongitudinalMpc(dt=dt) self.fcw = False self.dt = dt - self.enable_frames = 0 self.a_desired = init_a self.v_desired_filter = FirstOrderFilter(init_v, 2.0, self.dt) @@ -102,17 +101,14 @@ class LongitudinalPlanner: v_ego = sm['carState'].vEgo v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX) v_cruise = v_cruise_kph * CV.KPH_TO_MS + # some PCM messages may be updated a few cycles later, check if initialized + v_cruise_initialized = abs(sm['carState'].vCruise - V_CRUISE_UNSET) < 1e-3 long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel - if not long_control_off: - self.enable_frames += 1 - else: - self.enable_frames = 0 - # Reset current state when not engaged, or user is controlling the speed - reset_state = (long_control_off or self.enable_frames < 10) if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled + reset_state = (long_control_off or not v_cruise_initialized) if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill)