IQ.Pilot Prebuilt Release @ 67fd9c2

artifacts/package_runtime/iqdbc/lvbs/car/toyota/__init__.py

@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""

artifacts/package_runtime/iqdbc/lvbs/car/toyota/gas_interceptor.py

@@ -0,0 +1,47 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+import numpy as np
+
+from iqdbc.car import structs
+from iqdbc.car.can_definitions import CanData
+from iqdbc.car.toyota.values import CAR, MIN_ACC_SPEED, PEDAL_TRANSITION
+from iqdbc.lvbs.car import create_gas_interceptor_command
+
+
+class GasInterceptorCarController:
+  def __init__(self, CP, CP_IQ):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.gas = 0.
+    self.interceptor_gas_cmd = 0.
+
+  def create_gas_command(self, CC: structs.CarControl, CS: structs.CarState, actuators: structs.CarControl.Actuators,
+                         packer, frame: int) -> list[CanData]:
+    can_sends = []
+
+    if self.CP_IQ.enableGasInterceptor and CC.longActive:
+      MAX_INTERCEPTOR_GAS = 0.3
+      # RAV4 has very sensitive gas pedal
+      if self.CP.carFingerprint in (CAR.TOYOTA_RAV4, CAR.TOYOTA_RAV4H, CAR.TOYOTA_HIGHLANDER):
+        PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
+      elif self.CP.carFingerprint in (CAR.TOYOTA_COROLLA,):
+        PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
+      else:
+        PEDAL_SCALE = np.interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
+      # offset for creep and windbrake
+      pedal_offset = np.interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
+      pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
+      self.interceptor_gas_cmd = float(np.clip(pedal_command, 0., MAX_INTERCEPTOR_GAS))
+    else:
+      self.interceptor_gas_cmd = 0.
+
+    if frame % 2 == 0 and self.CP_IQ.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
+      # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
+      # This prevents unexpected pedal range rescaling
+      can_sends.append(create_gas_interceptor_command(packer, self.interceptor_gas_cmd, frame // 2))
+      self.gas = self.interceptor_gas_cmd
+
+    return can_sends

artifacts/package_runtime/iqdbc/lvbs/car/toyota/iq_carstate.py

@@ -0,0 +1,167 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from enum import StrEnum
+
+from iqdbc.car import Bus, structs
+from iqdbc.car.carlog import carlog
+from iqdbc.can.parser import CANParser
+from iqdbc.car.common.conversions import Conversions as CV
+from iqdbc.lvbs.car.toyota.values import ToyotaFlagsIQ
+
+TRAFFIC_SIGNAL_MAP = {
+  1: "kph",
+  36: "mph",
+  65: "No overtake",
+  66: "No overtake"
+}
+
+ZSS_DIFF_THRESHOLD = 4
+ZSS_MAX_THRESHOLD = 10
+
+
+class IQCarState:
+  def __init__(self, CP, CP_IQ):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.acc_type = 1
+    self.zss_compute = False
+    self.zss_cruise_active_last = False
+    self.zss_angle_offset = 0.
+    self.zss_threshold_count = 0
+
+    """Initialize traffic signal variables"""
+    self._tsgn1 = None
+    self._spdval1 = None
+    self._splsgn1 = None
+    self._tsgn2 = None
+    self._splsgn2 = None
+    self._tsgn3 = None
+    self._splsgn3 = None
+    self._tsgn4 = None
+    self._splsgn4 = None
+
+  @staticmethod
+  def traffic_signal_description(tsgn):
+    """Get description for traffic signal code"""
+    desc = TRAFFIC_SIGNAL_MAP.get(int(tsgn))
+    return f'{tsgn}: {desc}' if desc is not None else f'{tsgn}'
+
+  def update_traffic_signals(self, cp_cam):
+    """Update traffic signals with error handling"""
+    try:
+      # Add error handling for missing RSA messages
+      tsgn1 = cp_cam.vl.get("RSA1", {}).get('TSGN1', 0)
+      spdval1 = cp_cam.vl.get("RSA1", {}).get('SPDVAL1', 0)
+      splsgn1 = cp_cam.vl.get("RSA1", {}).get('SPLSGN1', 0)
+      tsgn2 = cp_cam.vl.get("RSA1", {}).get('TSGN2', 0)
+      splsgn2 = cp_cam.vl.get("RSA1", {}).get('SPLSGN2', 0)
+      tsgn3 = cp_cam.vl.get("RSA2", {}).get('TSGN3', 0)
+      splsgn3 = cp_cam.vl.get("RSA2", {}).get('SPLSGN3', 0)
+      tsgn4 = cp_cam.vl.get("RSA2", {}).get('TSGN4', 0)
+      splsgn4 = cp_cam.vl.get("RSA2", {}).get('SPLSGN4', 0)
+    except (KeyError, AttributeError) as e:
+      # Handle case where RSA messages are not available
+      carlog.debug(f"RSA messages not available: {e}")
+      return
+
+    has_changed = tsgn1 != self._tsgn1 \
+                  or spdval1 != self._spdval1 \
+                  or splsgn1 != self._splsgn1 \
+                  or tsgn2 != self._tsgn2 \
+                  or splsgn2 != self._splsgn2 \
+                  or tsgn3 != self._tsgn3 \
+                  or splsgn3 != self._splsgn3 \
+                  or tsgn4 != self._tsgn4 \
+                  or splsgn4 != self._splsgn4
+
+    self._tsgn1 = tsgn1
+    self._spdval1 = spdval1
+    self._splsgn1 = splsgn1
+    self._tsgn2 = tsgn2
+    self._splsgn2 = splsgn2
+    self._tsgn3 = tsgn3
+    self._splsgn3 = splsgn3
+    self._tsgn4 = tsgn4
+    self._splsgn4 = splsgn4
+
+    if not has_changed:
+      return
+
+    carlog.debug('---- TRAFFIC SIGNAL UPDATE -----')
+    if tsgn1 is not None and tsgn1 != 0:
+      carlog.debug(f'TSGN1: {self.traffic_signal_description(tsgn1)}')
+    if spdval1 is not None and spdval1 != 0:
+      carlog.debug(f'SPDVAL1: {spdval1}')
+    if splsgn1 is not None and splsgn1 != 0:
+      carlog.debug(f'SPLSGN1: {splsgn1}')
+    if tsgn2 is not None and tsgn2 != 0:
+      carlog.debug(f'TSGN2: {self.traffic_signal_description(tsgn2)}')
+    if splsgn2 is not None and splsgn2 != 0:
+      carlog.debug(f'SPLSGN2: {splsgn2}')
+    if tsgn3 is not None and tsgn3 != 0:
+      carlog.debug(f'TSGN3: {self.traffic_signal_description(tsgn3)}')
+    if splsgn3 is not None and splsgn3 != 0:
+      carlog.debug(f'SPLSGN3: {splsgn3}')
+    if tsgn4 is not None and tsgn4 != 0:
+      carlog.debug(f'TSGN4: {self.traffic_signal_description(tsgn4)}')
+    if splsgn4 is not None and splsgn4 != 0:
+      carlog.debug(f'SPLSGN4: {splsgn4}')
+    carlog.debug('------------------------')
+
+  def calculate_speed_limit(self):
+    """Calculate speed limit from traffic signals with validation"""
+    # Check all traffic sign slots for speed limits, not just tsgn1
+    for tsgn, spdval in [(self._tsgn1, self._spdval1),
+                         (self._tsgn2, None),
+                         (self._tsgn3, None),
+                         (self._tsgn4, None)]:
+
+      if tsgn == 1 and spdval is not None and 0 < spdval <= 200:  # Reasonable speed range
+        return spdval * CV.KPH_TO_MS
+
+      if tsgn == 36 and spdval is not None and 0 < spdval <= 120:  # Reasonable MPH range
+        return spdval * CV.MPH_TO_MS
+
+    return 0
+
+  def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
+    cp = can_parsers[Bus.pt]
+    cp_cam = can_parsers[Bus.cam]
+
+    if self.CP_IQ.flags & ToyotaFlagsIQ.SMART_DSU or self.CP_IQ.flags & ToyotaFlagsIQ.STOP_AND_GO_HACK:
+      self.acc_type = 1
+
+    if self.CP_IQ.enableGasInterceptor:
+      gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
+      ret.gasPressed = gas > 805
+
+    # ZSS support thanks to zorrobyte, ErichMoraga, and dragonpilot
+    if self.CP_IQ.flags & ToyotaFlagsIQ.ZSS:
+      zorro_steer = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
+      control_available = ret.cruiseState.available
+
+      # Only compute ZSS offset when control is available
+      if control_available and not self.zss_cruise_active_last:
+        self.zss_threshold_count = 0
+        self.zss_compute = True  # Control was just activated, so allow offset to be recomputed
+      self.zss_cruise_active_last = control_available
+
+      # Compute ZSS offset once we have meaningful angles
+      if self.zss_compute and abs(ret.steeringAngleDeg) > 1e-3 and abs(zorro_steer) > 1e-3:
+        self.zss_compute = False
+        self.zss_angle_offset = zorro_steer - ret.steeringAngleDeg
+
+      # Sanity checks
+      steering_angle_deg = zorro_steer - self.zss_angle_offset
+      if self.zss_threshold_count <= ZSS_MAX_THRESHOLD:
+        if abs(ret.steeringAngleDeg - steering_angle_deg) > ZSS_DIFF_THRESHOLD:
+          self.zss_threshold_count += 1
+        else:
+          ret.steeringAngleDeg = steering_angle_deg
+
+    # Update traffic signals and speed limit
+    self.update_traffic_signals(cp_cam)
+    ret_iq.speedLimit = self.calculate_speed_limit()

artifacts/package_runtime/iqdbc/lvbs/car/toyota/iq_fingerprints.py

@@ -0,0 +1,6 @@
+from iqdbc.car.structs import CarParams
+
+Ecu = CarParams.Ecu
+
+FW_VERSIONS_EXT = {
+}

artifacts/package_runtime/iqdbc/lvbs/car/toyota/values.py

@@ -0,0 +1,21 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+
+from enum import IntFlag
+
+
+class ToyotaFlagsIQ(IntFlag):
+  SMART_DSU = 1
+  RADAR_CAN_FILTER = 2
+  ZSS = 4
+  STOCK_LONGITUDINAL = 8
+  STOP_AND_GO_HACK = 16
+
+
+class ToyotaSafetyFlagsIQ:
+  DEFAULT = 0
+  UNSUPPORTED_DSU = 1
+  GAS_INTERCEPTOR = 2
+  LKAS_HUD = 4
+