comment

old-commit-hash: adf1ef88b692132c37d865fc973189fdf971464f

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)