IQ.Pilot Release Commit @ 0798119

2026-08-22 23:42:42 -05:00
commit b42569dbca
4529 changed files with 1132125 additions and 0 deletions
--- a/selfdrive/car/cruise.py
+++ b/selfdrive/car/cruise.py
@@ -0,0 +1,352 @@
+import math
+import numpy as np
+
+from cereal import car
+from openpilot.common.constants import CV
+from cereal import car, custom
+from iqdbc.car import structs
+from openpilot.common.params import Params
+from openpilot.iqpilot.selfdrive.car.long_increments import LongIncrementConfig, read_long_increment_config, resolve_button_step
+
+
+# ===== VCruiseHelperIQ (dissolved from iqpilot vcruise_helper_iq) =====
+
+ButtonType = car.CarState.ButtonEvent.Type
+SpeedLimitAssistState = custom.IQPlan.SpeedLimit.AssistState
+SPEED_LIMIT_CONTROL_ACTIVE_STATES = (SpeedLimitAssistState.active, SpeedLimitAssistState.adapting)
+
+
+def compare_cluster_target(v_cruise_cluster: float, target_set_speed: float, is_metric: bool) -> tuple[bool, bool]:
+  """Whether the cluster set-speed needs +/- presses to reach the target, in display units."""
+  to_shown = CV.MS_TO_KPH if is_metric else CV.MS_TO_MPH
+  now = round(v_cruise_cluster * to_shown)
+  goal = round(target_set_speed * to_shown)
+  return now < goal, now > goal
+
+
+CRUISE_BUTTON_TIMER = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0,
+                       ButtonType.setCruise: 0, ButtonType.resumeCruise: 0,
+                       ButtonType.cancel: 0, ButtonType.mainCruise: 0}
+
+V_CRUISE_MIN = 8
+V_CRUISE_MAX = 200  # ~ 124 mph
+V_CRUISE_UNSET = 255
+IQ_SET_SPEED_MODE_OFF = 0
+IQ_SET_SPEED_MODE_FIXED = 1
+IQ_SET_SPEED_MPH_DEFAULT = 65
+IQ_SET_SPEED_MPH_MIN = 20
+IQ_SET_SPEED_MPH_MAX = 120
+
+
+def get_minimum_set_speed_kph(_is_metric: bool) -> float:
+  # IQ minimum set speed floor, expressed in kph for VCruiseHelper integration.
+  return float(V_CRUISE_MIN)
+
+
+def update_manual_button_timers(CS: car.CarState, button_timers: dict[car.CarState.ButtonEvent.Type, int]) -> None:
+  # age any button that's currently held (nonzero timer)
+  for btn, held_frames in button_timers.items():
+    if held_frames > 0:
+      button_timers[btn] = held_frames + 1
+
+  # a press/release edge (re)starts the timer at 1 or clears it to 0
+  for event in CS.buttonEvents:
+    raw = event.type.raw
+    if raw in button_timers:
+      button_timers[raw] = 1 if event.pressed else 0
+
+
+class VCruiseHelperIQ:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams) -> None:
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+    self.v_cruise_kph = V_CRUISE_UNSET
+    self.v_cruise_cluster_kph = V_CRUISE_UNSET
+    self.params = Params()
+    self.v_cruise_min = 0
+    self.enabled_prev = False
+
+    self.long_increment_config: LongIncrementConfig = read_long_increment_config(self.params)
+    self.set_speed_to_limit = self._read_set_speed_to_limit()
+    self.iq_set_speed_mode = self._read_iq_set_speed_mode()
+    self.iq_set_speed_use_current = self._read_iq_set_speed_use_current()
+    self.iq_set_speed_mph = self._read_iq_set_speed_mph()
+
+    self.enable_button_timers = CRUISE_BUTTON_TIMER
+
+    # Speed Limit Assist
+    self.speed_limit_state = SpeedLimitAssistState.disabled
+    self.prev_speed_limit_state = SpeedLimitAssistState.disabled
+    self.has_speed_limit = False
+    self.speed_limit_final_last = 0.
+    self.speed_limit_final_last_kph = 0.
+    self.prev_speed_limit_final_last_kph = 0.
+    self.req_plus = False
+    self.req_minus = False
+
+  def _read_set_speed_to_limit(self) -> bool:
+    try:
+      return self.params.get_bool("SLCSetSpeedToLimit")
+    except Exception:
+      return False
+
+  def _read_iq_set_speed_mode(self) -> int:
+    try:
+      return int(self.params.get("IQE2ESetSpeedMode", return_default=True) or IQ_SET_SPEED_MODE_OFF)
+    except Exception:
+      return IQ_SET_SPEED_MODE_OFF
+
+  def _read_iq_set_speed_use_current(self) -> bool:
+    try:
+      return bool(self.params.get_bool("IQE2ESetSpeedUseCurrent"))
+    except Exception:
+      return False
+
+  def _read_iq_set_speed_mph(self) -> int:
+    try:
+      value = int(self.params.get("IQE2ESetSpeedMph", return_default=True) or IQ_SET_SPEED_MPH_DEFAULT)
+    except Exception:
+      value = IQ_SET_SPEED_MPH_DEFAULT
+    return int(np.clip(value, IQ_SET_SPEED_MPH_MIN, IQ_SET_SPEED_MPH_MAX))
+
+  def read_custom_set_speed_params(self) -> None:
+    self.long_increment_config = read_long_increment_config(self.params)
+    self.set_speed_to_limit = self._read_set_speed_to_limit()
+    self.iq_set_speed_mode = self._read_iq_set_speed_mode()
+    self.iq_set_speed_use_current = self._read_iq_set_speed_use_current()
+    self.iq_set_speed_mph = self._read_iq_set_speed_mph()
+
+  def get_iq_mode_initial_set_speed_kph(self, current_speed_kph: float, fallback_kph: float) -> float:
+    if self.iq_set_speed_mode != IQ_SET_SPEED_MODE_FIXED:
+      return fallback_kph
+
+    if self.iq_set_speed_use_current:
+      return float(np.clip(round(current_speed_kph, 1), self.v_cruise_min, V_CRUISE_MAX))
+
+    fixed_kph = float(self.iq_set_speed_mph) * CV.MPH_TO_KPH
+    return float(np.clip(round(fixed_kph, 1), self.v_cruise_min, V_CRUISE_MAX))
+
+  def update_v_cruise_delta(self, long_press: bool, v_cruise_delta: float) -> tuple[bool, float]:
+    return resolve_button_step(self.long_increment_config, long_press, v_cruise_delta)
+
+  def get_minimum_set_speed(self, is_metric: bool) -> None:
+    if self.CP_IQ.pcmCruiseSpeed:
+      self.v_cruise_min = V_CRUISE_MIN
+      return
+
+    self.v_cruise_min = get_minimum_set_speed_kph(is_metric)
+
+  def update_enabled_state(self, CS: car.CarState, enabled: bool) -> bool:
+    # pcmCruiseSpeed cars keep the stock enabled flag; others gate engagement on button release
+    if self.CP_IQ.pcmCruiseSpeed:
+      return enabled
+
+    update_manual_button_timers(CS, self.enable_button_timers)
+    button_pressed = any(t > 0 for t in self.enable_button_timers.values())
+
+    if enabled and not self.enabled_prev:
+      # first engage frame while the button is still down: hold off until it's let go
+      self.enabled_prev = not button_pressed
+      return False
+    if not enabled:
+      self.enabled_prev = False
+
+    return enabled and self.enabled_prev
+
+  def update_speed_limit_assist(self, is_metric, LP_IQ: custom.IQPlan) -> None:
+    resolver = LP_IQ.speedLimit.resolver
+    self.has_speed_limit = resolver.speedLimitValid or resolver.speedLimitLastValid
+    self.speed_limit_final_last = LP_IQ.speedLimit.resolver.speedLimitFinalLast
+    self.speed_limit_final_last_kph = self.speed_limit_final_last * CV.MS_TO_KPH
+    self.speed_limit_state = LP_IQ.speedLimit.assist.state
+    self.req_plus, self.req_minus = compare_cluster_target(self.v_cruise_cluster_kph * CV.KPH_TO_MS,
+                                                           self.speed_limit_final_last, is_metric)
+
+  @property
+  def update_speed_limit_final_last_changed(self) -> bool:
+    if not self.has_speed_limit:
+      return False
+    return self.speed_limit_final_last_kph != self.prev_speed_limit_final_last_kph
+
+  def update_speed_limit_assist_v_cruise_non_pcm(self) -> None:
+    if self.set_speed_to_limit and \
+       self.speed_limit_state in SPEED_LIMIT_CONTROL_ACTIVE_STATES and \
+       (self.prev_speed_limit_state not in SPEED_LIMIT_CONTROL_ACTIVE_STATES or self.update_speed_limit_final_last_changed):
+      self.v_cruise_kph = np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
+
+    self.prev_speed_limit_state = self.speed_limit_state
+    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
+
+  def update_speed_limit_assist_v_cruise_op_long(self) -> None:
+    if not self.CP.openpilotLongitudinalControl or not self.set_speed_to_limit:
+      return
+
+    if self.speed_limit_state == SpeedLimitAssistState.disabled:
+      self.prev_speed_limit_state = self.speed_limit_state
+      self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
+      return
+
+    if not self.has_speed_limit or self.speed_limit_final_last_kph <= 0:
+      self.prev_speed_limit_state = self.speed_limit_state
+      self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
+      return
+
+    target_kph = float(np.clip(round(self.speed_limit_final_last_kph, 1), self.v_cruise_min, V_CRUISE_MAX))
+    # OP Long uses planner min(v_cruise, slc target) to enforce limits.
+    # Do not clamp the user's max (v_cruise) here, or they cannot raise/lower it.
+    # Only sync on initial activation or when the resolved limit changes.
+    if (self.v_cruise_kph == V_CRUISE_UNSET and self.speed_limit_state in SPEED_LIMIT_CONTROL_ACTIVE_STATES) or \
+       self.update_speed_limit_final_last_changed:
+      self.v_cruise_kph = target_kph
+      self.v_cruise_cluster_kph = target_kph
+
+    self.prev_speed_limit_state = self.speed_limit_state
+    self.prev_speed_limit_final_last_kph = self.speed_limit_final_last_kph
+
+
+
+# WARNING: this value was determined based on the model's training distribution,
+#          model predictions above this speed can be unpredictable
+# V_CRUISE's are in kph
+V_CRUISE_MIN = 8
+V_CRUISE_MAX = 200  # ~ 124 mph
+V_CRUISE_UNSET = 255
+V_CRUISE_INITIAL = 40
+V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
+IMPERIAL_INCREMENT = round(CV.MPH_TO_KPH, 1)  # round here to avoid rounding errors incrementing set speed
+
+ButtonEvent = car.CarState.ButtonEvent
+ButtonType = car.CarState.ButtonEvent.Type
+CRUISE_LONG_PRESS = 50
+CRUISE_NEAREST_FUNC = {
+  ButtonType.accelCruise: math.ceil,
+  ButtonType.decelCruise: math.floor,
+}
+CRUISE_INTERVAL_SIGN = {
+  ButtonType.accelCruise: +1,
+  ButtonType.decelCruise: -1,
+}
+
+
+class VCruiseHelper(VCruiseHelperIQ):
+  def __init__(self, CP, CP_IQ):
+    VCruiseHelperIQ.__init__(self, CP, CP_IQ)
+    self.CP = CP
+    self.v_cruise_kph = V_CRUISE_UNSET
+    self.v_cruise_cluster_kph = V_CRUISE_UNSET
+    self.v_cruise_kph_last = 0
+    self.button_timers = {ButtonType.decelCruise: 0, ButtonType.accelCruise: 0}
+    self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers}
+
+  @property
+  def v_cruise_initialized(self):
+    return self.v_cruise_kph != V_CRUISE_UNSET
+
+  @property
+  def volkswagen_standby_set_speed(self) -> bool:
+    return self.CP.brand == "volkswagen" and self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
+
+  def update_v_cruise(self, CS, enabled, is_metric):
+    self.v_cruise_kph_last = self.v_cruise_kph
+
+    self.get_minimum_set_speed(is_metric)
+
+    if CS.cruiseState.available:
+      _enabled = self.update_enabled_state(CS, enabled)
+      if not self.CP.pcmCruise or (not self.CP_IQ.pcmCruiseSpeed and _enabled):
+        # if stock cruise is completely disabled, then we can use our own set speed logic
+        self._update_v_cruise_non_pcm(CS, _enabled, is_metric, self.volkswagen_standby_set_speed)
+        self.update_speed_limit_assist_v_cruise_non_pcm()
+        self.v_cruise_cluster_kph = self.v_cruise_kph
+        self.update_button_timers(CS, enabled)
+      else:
+        self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
+        self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH
+        if CS.cruiseState.speed == 0:
+          self.v_cruise_kph = V_CRUISE_UNSET
+          self.v_cruise_cluster_kph = V_CRUISE_UNSET
+        elif CS.cruiseState.speed == -1:
+          self.v_cruise_kph = -1
+          self.v_cruise_cluster_kph = -1
+        else:
+          self.update_speed_limit_assist_v_cruise_op_long()
+    else:
+      self.v_cruise_kph = V_CRUISE_UNSET
+      self.v_cruise_cluster_kph = V_CRUISE_UNSET
+
+  def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, allow_standby_adjustment=False):
+    # handle button presses. TODO: this should be in state_control, but a decelCruise press
+    # would have the effect of both enabling and changing speed is checked after the state transition
+    if not enabled and not allow_standby_adjustment:
+      return
+
+    long_press = False
+    button_type = None
+
+    v_cruise_delta = 1. if is_metric else IMPERIAL_INCREMENT
+
+    for b in CS.buttonEvents:
+      if b.type.raw in self.button_timers and not b.pressed:
+        if self.button_timers[b.type.raw] > CRUISE_LONG_PRESS:
+          return  # end long press
+        button_type = b.type.raw
+        break
+    else:
+      for k, timer in self.button_timers.items():
+        if timer and timer % CRUISE_LONG_PRESS == 0:
+          button_type = k
+          long_press = True
+          break
+
+    if button_type is None:
+      return
+
+    # Don't adjust speed when pressing resume to exit standstill
+    cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill
+    if button_type == ButtonType.accelCruise and cruise_standstill:
+      return
+
+    # Don't adjust speed if we've enabled since the button was depressed (some ports enable on rising edge)
+    if enabled and not self.button_change_states[button_type]["enabled"]:
+      return
+
+    long_press, v_cruise_delta = VCruiseHelperIQ.update_v_cruise_delta(self, long_press, v_cruise_delta)
+    if long_press and self.v_cruise_kph % v_cruise_delta != 0:  # partial interval
+      self.v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](self.v_cruise_kph / v_cruise_delta) * v_cruise_delta
+    else:
+      self.v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+
+    # If set is pressed while overriding, clip cruise speed to minimum of vEgo
+    if enabled and CS.gasPressed and button_type in (ButtonType.decelCruise, ButtonType.setCruise):
+      self.v_cruise_kph = max(self.v_cruise_kph, CS.vEgo * CV.MS_TO_KPH)
+
+    self.v_cruise_kph = np.clip(round(self.v_cruise_kph, 1), self.v_cruise_min, V_CRUISE_MAX)
+
+  def update_button_timers(self, CS, enabled):
+    # increment timer for buttons still pressed
+    for k in self.button_timers:
+      if self.button_timers[k] > 0:
+        self.button_timers[k] += 1
+
+    for b in CS.buttonEvents:
+      if b.type.raw in self.button_timers:
+        # Start/end timer and store current state on change of button pressed
+        self.button_timers[b.type.raw] = 1 if b.pressed else 0
+        self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled}
+
+  def initialize_v_cruise(self, CS, experimental_mode: bool, iq_dynamic_mode: bool) -> None:
+    # initializing is handled by the PCM
+    if self.CP.pcmCruise or self.v_cruise_initialized:
+      return
+
+    initial_experimental_mode = experimental_mode and not iq_dynamic_mode
+    initial = V_CRUISE_INITIAL_EXPERIMENTAL_MODE if initial_experimental_mode else V_CRUISE_INITIAL
+    if initial_experimental_mode:
+      initial = self.get_iq_mode_initial_set_speed_kph(CS.vEgo * CV.MS_TO_KPH, initial)
+
+    if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_initialized:
+      self.v_cruise_kph = self.v_cruise_kph_last
+    else:
+      self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX)))
+
+    self.v_cruise_cluster_kph = self.v_cruise_kph