From 2e6e977c934aa81995945461667ad7ece6fbc434 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 16 Sep 2024 16:06:02 -0700 Subject: [PATCH] Longitudinal planner: wait for valid cruise speed (#33568) * Revert "only vCruise changes here" This reverts commit 4f5659b5d53e9bac3357781bce262a29d3b1d14e. * less nonsense --- selfdrive/controls/lib/longitudinal_planner.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 103d3f7047..0b65119383 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -13,7 +13,7 @@ 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.car.cruise import V_CRUISE_MAX +from openpilot.selfdrive.car.cruise import V_CRUISE_MAX, V_CRUISE_UNSET from openpilot.common.swaglog import cloudlog LON_MPC_STEP = 0.2 # first step is 0.2s @@ -101,12 +101,15 @@ 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 + v_cruise_initialized = sm['carState'].vCruise != V_CRUISE_UNSET long_control_off = sm['controlsState'].longControlState == LongCtrlState.off force_slow_decel = sm['controlsState'].forceDecel # Reset current state when not engaged, or user is controlling the speed reset_state = long_control_off if self.CP.openpilotLongitudinalControl else not sm['selfdriveState'].enabled + # PCM cruise speed may be updated a few cycles later, check if initialized + reset_state = reset_state or not v_cruise_initialized # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill)