import math import numpy as np from iqpilot.cereal import car from iqpilot.common.constants import CV from iqpilot.cereal import car, custom from iqdbc.car import structs from iqpilot.common.params import Params from iqpilot.common.realtime import DT_CTRL from 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} self.slc_set_speed_request_id = 0 self.slc_set_speed_gesture_id = 0 self.slc_set_speed_request_kph = 0.0 self._slc_accel_held = False self._slc_accel_release_frames = 0 self._slc_pcm_speed_last = None def _update_slc_accel_gesture(self, CS): self._slc_accel_release_frames = max(0, self._slc_accel_release_frames - 1) for button in CS.buttonEvents: if button.type in (ButtonType.accelCruise, ButtonType.resumeCruise): if button.pressed: self.slc_set_speed_gesture_id = (self.slc_set_speed_gesture_id + 1) % (1 << 32) self._slc_accel_release_frames = 0 else: self._slc_accel_release_frames = int(0.5 / DT_CTRL) self._slc_accel_held = button.pressed elif button.pressed: self._slc_accel_held = False self._slc_accel_release_frames = 0 if not CS.cruiseState.available: self._slc_accel_held = False self._slc_accel_release_frames = 0 def _record_slc_set_speed_increase(self, previous_kph, enabled): if enabled and (self._slc_accel_held or self._slc_accel_release_frames > 0) and \ previous_kph is not None and 0 < previous_kph < self.v_cruise_kph <= V_CRUISE_MAX: self.slc_set_speed_request_id = (self.slc_set_speed_request_id + 1) % (1 << 32) self.slc_set_speed_request_kph = float(self.v_cruise_kph) @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._update_slc_accel_gesture(CS) 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._record_slc_set_speed_increase(self.v_cruise_kph_last, _enabled) 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 self._record_slc_set_speed_increase(self._slc_pcm_speed_last, _enabled) self._slc_pcm_speed_last = self.v_cruise_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 self._slc_pcm_speed_last = None 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