IQ.Pilot Prebuilt Release @ 27f668a

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)))