IQ.Pilot Release Commit @ b6534c0

iqpilot/selfdrive/controls/lib/iq_dynamic/engine.py

@@ -0,0 +1,249 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from iqpilot.cereal import messaging
+from numpy import interp
+from iqdbc.car import structs
+from iqpilot.common.params import Params
+from iqpilot.common.realtime import DT_MDL
+from iqpilot.selfdrive.controls.lib.iq_dynamic.imahelper import (
+  IQConstants,
+  IQFilterEngine,
+  IQModeEngine,
+  IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM,
+  IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM,
+  IQ_DYNAMIC_MODE_PARAM,
+  IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM,
+  IQ_DYNAMIC_MODEL_STOP_TIME_PARAM,
+  IQ_FORCE_STOPS_PARAM,
+  compute_slowdown_need,
+)
+
+S_Y = 33
+
+
+class IQDynamicController:
+  def __init__(self, CP: structs.CarParams, mpc, params=None):
+    self.IQS = CP
+    self._mpc = mpc
+    self.IQParams = params or Params()
+
+    self.IQDynamicStatus = False
+    self.IQDynamicA = False
+    self.IQDynamicF = 0
+    self.IQDynamicU = 0.0
+    self.IQEngineManager = IQModeEngine()
+
+    self.IQFilterL = IQFilterEngine(measurement_noise=0.17, process_noise=0.04, process_decay=1.03, smoothing_floor=0.9)
+    self.IQFilterSDL = IQFilterEngine(measurement_noise=0.12, process_noise=0.098, process_decay=1.01, smoothing_floor=0.8)
+    self.IQFilterSFL = IQFilterEngine(measurement_noise=0.11, process_noise=0.06, process_decay=1.000, smoothing_floor=0.90)
+    self.IQFilterFCW = IQFilterEngine(measurement_noise=0.19, process_noise=0.11, process_decay=1.11, smoothing_floor=0.4)
+    self.IQFilterSlowLead = IQFilterEngine(measurement_noise=0.15, process_noise=0.08, process_decay=1.02, smoothing_floor=0.75)
+    self.IQFilterModelStop = IQFilterEngine(measurement_noise=0.15, process_noise=0.06, process_decay=1.01, smoothing_floor=0.7)
+
+    self.hasIQFilterLED = False
+    self.hasIQSDL = False
+    self.hasIQSFL = False
+    self.hasIQL = False
+    self.curve_detected = False
+    self.slow_lead_detected = False
+    self.stop_light_detected = False
+    self.low_speed_detected = False
+    self.low_speed_lead_detected = False
+    self.model_stopped = False
+    self.tracking_lead = False
+    self.force_stops_enabled = True
+    self.slc_experimental_mode = False
+
+    self.kph = 0.0
+    self.cruise_kph = 0.0
+    self.aeb = 0
+    self.aeb_c = 0
+    self.ss_c = 0
+    self.e_x = float('inf')
+    self.e_d = 0.0
+    self.model_length = 0.0
+    self.lead_speed = 0.0
+
+    self.conditional_curves = True
+    self.conditional_slower_lead = True
+    self.conditional_stopped_lead = True
+    self.conditional_model_stops = True
+    self.conditional_slc_fallback = True
+    self.conditional_speed = IQConstants.CONDITIONAL_SPEED_DEFAULT
+    self.conditional_lead_speed = IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT
+    self.model_stop_time = IQConstants.MODEL_STOP_TIME_DEFAULT
+    self.minimum_force_stop_length = IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT
+
+  def _read_bool(self, key: str, default: bool) -> bool:
+    value = self.IQParams.get_bool(key)
+    return default if value is None else bool(value)
+
+  def _read_float(self, key: str, default: float) -> float:
+    value = self.IQParams.get(key)
+    if value is None:
+      return default
+    if isinstance(value, bytes):
+      value = value.decode('utf-8')
+    try:
+      return float(value)
+    except (TypeError, ValueError):
+      return default
+
+  def _readIQParams(self) -> None:
+    if self.IQDynamicF % int(1. / DT_MDL) != 0:
+      return
+    self.IQDynamicStatus = self._read_bool(IQ_DYNAMIC_MODE_PARAM, False)
+    self.conditional_curves = self._read_bool(IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM, True)
+    self.conditional_slower_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM, True)
+    self.conditional_stopped_lead = self._read_bool(IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM, True)
+    self.conditional_model_stops = self._read_bool(IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM, True)
+    self.conditional_slc_fallback = self._read_bool(IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM, True)
+    self.conditional_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM, IQConstants.CONDITIONAL_SPEED_DEFAULT)
+    self.conditional_lead_speed = self._read_float(IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM, IQConstants.CONDITIONAL_LEAD_SPEED_DEFAULT)
+    self.model_stop_time = self._read_float(IQ_DYNAMIC_MODEL_STOP_TIME_PARAM, IQConstants.MODEL_STOP_TIME_DEFAULT)
+    self.minimum_force_stop_length = self._read_float(IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM, IQConstants.MINIMUM_FORCE_STOP_LENGTH_DEFAULT)
+    self.force_stops_enabled = self._read_bool(IQ_FORCE_STOPS_PARAM, True)
+
+  def set_slc_experimental_mode(self, active: bool) -> None:
+    self.slc_experimental_mode = bool(active)
+
+  def mode(self) -> str:
+    return self.IQEngineManager.get_mode()
+
+  def enabled(self) -> bool:
+    return self.IQDynamicStatus
+
+  def active(self) -> bool:
+    return self.IQDynamicA
+
+  def force_stop_requested(self) -> bool:
+    return bool(self.force_stops_enabled and self.stop_light_detected and self.model_stopped and not self.tracking_lead)
+
+  def setaeb(self) -> None:
+    self.aeb = self.aeb_c
+
+  def IQDynamicEngine(self, sm: messaging.SubMaster) -> None:
+    car_state = sm['carState']
+    radar_state = sm['radarState']
+    model = sm['modelV2']
+
+    self.kph = car_state.vEgo * 3.6
+    self.cruise_kph = car_state.vCruise
+    self.ss_c = min(20, self.ss_c + 1) if car_state.standstill else max(0, self.ss_c - 1)
+
+    lead_status = float(getattr(radar_state.leadOne, "status", False))
+    self.IQFilterL.push(lead_status)
+    self.hasIQFilterLED = (self.IQFilterL.value() or 0.0) > IQConstants.LEAD_LOCK_GATE
+    self.tracking_lead = self.hasIQFilterLED
+    self.lead_speed = float(getattr(radar_state.leadOne, "vLead", 0.0))
+
+    prev_fcw = self.IQFilterFCW.value() or 0.0
+    self.IQFilterFCW.push(float(self.aeb > 0))
+    self.hasIQL = prev_fcw > 0.5
+
+    valid_model = len(model.position.x) == S_Y and len(model.orientation.x) == S_Y
+    if valid_model:
+      self.model_length = float(model.position.x[S_Y - 1])
+      self.e_x = self.model_length
+      self.e_d = interp(self.kph, IQConstants.BRAKE_CURVE_SPEED_AXIS, IQConstants.BRAKE_CURVE_DISTANCE_AXIS)
+      need = compute_slowdown_need(self.kph, self.model_length, self.e_d)
+    else:
+      self.model_length = 0.0
+      self.e_x = float('inf')
+      self.e_d = 0.0
+      need = 0.3 if self.kph > 20.0 else 0.0
+    self.IQFilterSDL.push(need)
+    self.IQDynamicU = self.IQFilterSDL.value() or 0.0
+    self.hasIQSDL = self.IQDynamicU > (IQConstants.BRAKE_CURVE_GATE * 0.8)
+    self.curve_detected = self.hasIQSDL
+
+    if self.ss_c <= 5 and not self.hasIQSDL:
+      slowness_observed = float(self.kph <= (self.cruise_kph * IQConstants.CRUISE_LAG_RATIO_GATE))
+      self.IQFilterSFL.push(slowness_observed)
+      threshold = IQConstants.CRUISE_LAG_GATE * (0.8 if self.hasIQSFL else 1.1)
+      self.hasIQSFL = (self.IQFilterSFL.value() or 0.0) > threshold
+
+    v_ego = float(car_state.vEgo)
+    self.low_speed_detected = not self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_speed
+    self.low_speed_lead_detected = self.tracking_lead and IQConstants.CRUISING_SPEED <= v_ego < self.conditional_lead_speed
+
+    if self.tracking_lead:
+      slower_lead = (v_ego - self.lead_speed) > IQConstants.CRUISING_SPEED and self.conditional_slower_lead
+      stopped_lead = self.lead_speed < 1.0 and self.conditional_stopped_lead
+      self.IQFilterSlowLead.push(float(slower_lead or stopped_lead))
+      self.slow_lead_detected = (self.IQFilterSlowLead.value() or 0.0) >= IQConstants.SLOW_LEAD_THRESHOLD
+    else:
+      self.IQFilterSlowLead.reset()
+      self.slow_lead_detected = False
+
+    should_stop = bool(getattr(getattr(model, "action", None), "shouldStop", False))
+    model_stopping = self.model_length > 0.0 and self.model_length < max(v_ego * self.model_stop_time, IQConstants.CRUISING_SPEED)
+    self.model_stopped = bool(should_stop or model_stopping)
+    self.IQFilterModelStop.push(float(self.model_stopped and not self.tracking_lead))
+    self.stop_light_detected = (self.IQFilterModelStop.value() or 0.0) >= IQConstants.MODEL_STOP_THRESHOLD
+
+  def _request_blended(self, urgency: float = 1.0, emergency: bool = False) -> None:
+    self.IQEngineManager.request('blended', urgency=urgency, emergency=emergency)
+
+  def _request_acc(self, urgency: float = 0.8) -> None:
+    self.IQEngineManager.request('acc', urgency=urgency)
+
+  def IQStateEngine(self) -> None:
+    if self.hasIQL:
+      self._request_blended(1.0, True)
+    elif self.stop_light_detected and self.conditional_model_stops:
+      self._request_blended(1.0, self.model_stopped)
+    elif self.low_speed_detected or self.low_speed_lead_detected:
+      self._request_blended(0.95)
+    elif self.slow_lead_detected:
+      self._request_blended(0.9)
+    elif self.conditional_curves and self.hasIQSDL:
+      self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.5)))
+    elif self.conditional_slc_fallback and self.slc_experimental_mode:
+      self._request_blended(0.8)
+    elif self.ss_c > 3:
+      self._request_blended(0.9)
+    elif self.hasIQSFL and not self.hasIQSDL:
+      self._request_acc(0.8)
+    else:
+      self._request_acc(0.7)
+
+  def IQStateEngine_R(self) -> None:
+    if self.hasIQL:
+      self._request_blended(1.0, True)
+    elif self.stop_light_detected and self.conditional_model_stops:
+      self._request_blended(1.0, self.model_stopped)
+    elif self.low_speed_detected or self.low_speed_lead_detected:
+      self._request_blended(0.95)
+    elif self.slow_lead_detected:
+      self._request_blended(0.9)
+    elif self.conditional_curves and self.hasIQSDL:
+      self._request_blended(max(0.8, min(1.0, self.IQDynamicU * 1.3)))
+    elif self.conditional_slc_fallback and self.slc_experimental_mode:
+      self._request_blended(0.8)
+    elif self.hasIQFilterLED and not (self.ss_c > 3):
+      self._request_acc(1.0)
+    elif self.ss_c > 3:
+      self._request_blended(0.9)
+    elif self.hasIQSFL and not self.hasIQSDL:
+      self._request_acc(0.8)
+    else:
+      self._request_acc(0.7)
+
+  def update(self, sm: messaging.SubMaster) -> None:
+    self._readIQParams()
+    self.setaeb()
+    self.IQDynamicEngine(sm)
+    if self.IQS.radarUnavailable:
+      self.IQStateEngine()
+    else:
+      self.IQStateEngine_R()
+    self.IQEngineManager.update()
+    self.IQDynamicA = sm['selfdriveState'].experimentalMode and self.IQDynamicStatus
+    self.IQDynamicF += 1

iqpilot/selfdrive/controls/lib/iq_dynamic/imahelper.py

@@ -0,0 +1,148 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from typing import Literal
+
+ModeType = Literal['acc', 'blended']
+IQ_DYNAMIC_MODE_PARAM = "IQDynamicMode"
+IQ_DYNAMIC_CONDITIONAL_CURVES_PARAM = "IQDynamicConditionalCurves"
+IQ_DYNAMIC_CONDITIONAL_SLOWER_LEAD_PARAM = "IQDynamicConditionalSlowerLead"
+IQ_DYNAMIC_CONDITIONAL_STOPPED_LEAD_PARAM = "IQDynamicConditionalStoppedLead"
+IQ_DYNAMIC_CONDITIONAL_MODEL_STOPS_PARAM = "IQDynamicConditionalModelStops"
+IQ_DYNAMIC_CONDITIONAL_SLC_FALLBACK_PARAM = "IQDynamicConditionalSLCFallback"
+IQ_DYNAMIC_CONDITIONAL_SPEED_PARAM = "IQDynamicConditionalSpeed"
+IQ_DYNAMIC_CONDITIONAL_LEAD_SPEED_PARAM = "IQDynamicConditionalLeadSpeed"
+IQ_DYNAMIC_MODEL_STOP_TIME_PARAM = "IQDynamicModelStopTime"
+IQ_DYNAMIC_MINIMUM_FORCE_STOP_LENGTH_PARAM = "IQDynamicMinimumForceStopLength"
+IQ_FORCE_STOPS_PARAM = "IQForceStops"
+
+
+class IQConstants:
+  CRUISING_SPEED = 3.0
+  SIGNAL_QUEUE_DEPTH = 6
+  LEAD_LOCK_GATE = 0.45
+  BRAKE_CURVE_QUEUE_DEPTH = 5
+  BRAKE_CURVE_GATE = 0.3
+  BRAKE_CURVE_SPEED_AXIS = [0., 10., 20., 30., 40., 50., 55., 60.]
+  BRAKE_CURVE_DISTANCE_AXIS = [32., 46., 64., 86., 108., 130., 145., 165.]
+  CRUISE_LAG_QUEUE_DEPTH = 10
+  CRUISE_LAG_GATE = 0.55
+  CRUISE_LAG_RATIO_GATE = 1.025
+  CONDITIONAL_SPEED_DEFAULT = 18.0
+  CONDITIONAL_LEAD_SPEED_DEFAULT = 24.0
+  MODEL_STOP_TIME_DEFAULT = 3.0
+  MODEL_STOP_THRESHOLD = 0.55
+  SLOW_LEAD_THRESHOLD = 0.55
+  FORCE_STOP_PLANNER_TIME = 3.0
+  MINIMUM_FORCE_STOP_LENGTH_DEFAULT = 0.0
+
+
+def compute_slowdown_need(kph: float, horizon_dist: float, desired_dist: float) -> float:
+  if horizon_dist >= desired_dist or desired_dist <= 0.0:
+    return 0.0
+
+  shortage = desired_dist - horizon_dist
+  shortage_ratio = shortage / desired_dist
+  need = min(1.0, shortage_ratio * 2.0)
+
+  if horizon_dist < desired_dist * 0.3:
+    need = min(1.0, need * 2.0)
+
+  if kph > 25.0:
+    need = min(1.0, need * (1.0 + (kph - 25.0) / 80.0))
+
+  return need
+
+
+class IQFilterEngine:
+  def __init__(self, initial=0.0, measurement_noise=0.1, process_noise=0.01, process_decay=1.0, smoothing_floor=0.85):
+    self._value = initial
+    self._variance = 1.0
+    self._measurement_noise = measurement_noise
+    self._process_noise = process_noise
+    self._process_decay = process_decay
+    self._smoothing_floor = smoothing_floor
+    self._initialized = False
+    self._samples = []
+    self._sample_limit = 10
+    self._confidence = 0.0
+
+  def push(self, measurement: float) -> None:
+    if len(self._samples) >= self._sample_limit:
+      self._samples.pop(0)
+    self._samples.append(measurement)
+
+    if not self._initialized:
+      self._value = measurement
+      self._initialized = True
+      self._confidence = 0.1
+      return
+
+    self._variance = self._process_decay * self._variance + self._process_noise
+    gain = self._variance / (self._variance + self._measurement_noise)
+    effective_gain = gain * (1.0 - self._smoothing_floor) + self._smoothing_floor * 0.1
+
+    innovation = measurement - self._value
+    self._value = self._value + effective_gain * innovation
+    self._variance = (1.0 - effective_gain) * self._variance
+
+    if abs(innovation) < 0.1:
+      self._confidence = min(1.0, self._confidence + 0.05)
+    else:
+      self._confidence = max(0.1, self._confidence - 0.02)
+
+  def value(self):
+    return self._value if self._initialized else None
+
+  def confidence(self) -> float:
+    return self._confidence
+
+  def reset(self) -> None:
+    self._initialized = False
+    self._samples = []
+    self._confidence = 0.0
+
+
+class IQModeEngine:
+  def __init__(self):
+    self._state: ModeType = 'acc'
+    self._scores = {'acc': 1.0, 'blended': 0.0}
+    self._switching_timer = 0
+    self._mode_age = 0
+    self._forced_takeover = False
+
+  def request(self, mode: ModeType, urgency: float = 1.0, emergency: bool = False) -> None:
+    if emergency:
+      self._forced_takeover = True
+      self._state = mode
+      self._switching_timer = 15
+      self._mode_age = 0
+      return
+
+    self._scores[mode] = min(1.0, self._scores[mode] + 0.1 * urgency)
+    for key in self._scores:
+      if key != mode:
+        self._scores[key] = max(0.0, self._scores[key] - 0.05)
+
+    if self._mode_age < 10 and not self._forced_takeover:
+      return
+
+    threshold = 0.6 if mode != self._state else 0.3
+    if self._scores[mode] > threshold and mode != self._state and self._switching_timer == 0:
+      self._switching_timer = 15
+      self._state = mode
+      self._mode_age = 0
+
+  def update(self) -> None:
+    if self._switching_timer > 0:
+      self._switching_timer -= 1
+
+    self._mode_age += 1
+    if self._forced_takeover and self._mode_age > 20:
+      self._forced_takeover = False
+
+    for key in self._scores:
+      self._scores[key] *= 0.98
+
+  def get_mode(self) -> ModeType:
+    return self._state

iqpilot/selfdrive/controls/lib/iq_dynamic/radar_manager.py

@@ -0,0 +1,68 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+
+RadarManager — IQ.Dynamics side of "Blend IQ.Pilot + Stock ACC Radar" (VW PQ only).
+
+Decides the high-level intent for the stock ACC radar and writes it onto iqCarControl for the iqdbc
+PQRadarHandler to execute on CAN. It does NOT touch CAN or read radar feedback directly: the handler
+(car process) owns the bus and the failure latch, and the carcontroller gates radar-accel passthrough
+on the live ACS_Sta_ADR. That keeps the desync guard automatic — if chill is requested but the radar
+isn't active, the carcontroller simply uses the planner's VoACC accel (standard IQ long).
+
+Intent produced:
+  radarBlendActive  feature enabled + PQ + alpha long available
+  radarEngageReq    want the radar's cruise engaged (engage same time long control engages)
+  radarCancelReq    cancel now (1 kph stop / driver brake / long disengaged / teardown)
+  useRadarAccel     chill (acc) mode + engaged -> carcontroller passes radar ACS_Sollbeschl through
+  radarSetSpeedKph  OP set speed to sync the radar's ACA_V_Wunsch toward
+  radarGapBars      OP follow-distance bars to mirror to the radar
+"""
+from iqpilot.common.constants import CV
+
+CANCEL_CEIL_MS = 1.0 * CV.KPH_TO_MS  # cancel the radar at/below 1 kph (it can still see speed -> would fault)
+
+
+class RadarManager:
+  def __init__(self, CP, params):
+    self.CP = CP
+    self.params = params
+    self.is_pq = self._detect_pq(CP)
+    self.enabled_param = False
+
+  @staticmethod
+  def _detect_pq(CP) -> bool:
+    if getattr(CP, "brand", "") != "volkswagen":
+      return False
+    try:
+      from iqdbc.car.volkswagen.values import VolkswagenFlags
+      return bool(CP.flags & VolkswagenFlags.PQ)
+    except Exception:
+      return False
+
+  def read_params(self) -> None:
+    if self.is_pq:
+      self.enabled_param = self.params.get_bool("IQDynamicBlendStockRadar")
+
+  def update(self, CC_IQ, sm, set_speed_kph: float) -> None:
+    blend = bool(self.is_pq and self.enabled_param and self.CP.openpilotLongitudinalControl)
+    CC_IQ.radarBlendActive = blend
+    if not blend:
+      return
+
+    ss = sm['selfdriveState']
+    cs = sm['carState']
+    iq = sm['iqPlan'].iqDynamic
+
+    long_engaged = bool(ss.enabled) and self.CP.openpilotLongitudinalControl
+    iq_engaged = long_engaged and bool(iq.enabled)
+    chill = iq_engaged and bool(iq.active) and (iq.state == 'acc')
+    brake = bool(cs.brakePressed)
+    v_ego = float(cs.vEgo)
+
+    # Engage the radar whenever IQ.Dynamics long is engaged; cancel on stop/brake/teardown.
+    # The handler resolves priority (cancel wins) and applies the 1->2 kph engage hysteresis.
+    CC_IQ.radarEngageReq = iq_engaged and not brake
+    CC_IQ.radarCancelReq = (not iq_engaged) or brake or (v_ego <= CANCEL_CEIL_MS)
+    CC_IQ.useRadarAccel = chill
+    CC_IQ.radarSetSpeedKph = float(max(set_speed_kph, 0.0))
+    CC_IQ.radarGapBars = int(min(3, max(1, ss.personality.raw + 1)))