IQ.Pilot Prebuilt Release @ 67fd9c2

2026-09-01 20:15:16 -05:00
commit 13523543ee
2549 changed files with 678222 additions and 0 deletions
--- a/artifacts/package_runtime/iqdbc/lvbs/car/tesla/init.py
+++ b/artifacts/package_runtime/iqdbc/lvbs/car/tesla/init.py
@@ -0,0 +1,3 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
--- a/artifacts/package_runtime/iqdbc/lvbs/car/tesla/iq_carstate.py
+++ b/artifacts/package_runtime/iqdbc/lvbs/car/tesla/iq_carstate.py
@@ -0,0 +1,84 @@
+"""
+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, create_button_events, structs
+from iqdbc.can.parser import CANParser
+from iqdbc.car.common.conversions import Conversions as CV
+from iqdbc.car.tesla.values import DBC, CANBUS
+from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
+
+ButtonType = structs.CarState.ButtonEvent.Type
+
+
+class IQCarState:
+  def __init__(self, CP: structs.CarParams, CP_IQ: structs.IQCarParams):
+    self.CP = CP
+    self.CP_IQ = CP_IQ
+
+    self.infotainment_3_finger_press = 0
+    self.vehicle_bus_available = bool(CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS)
+
+  def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
+    if Bus.adas in can_parsers:
+      cp_adas = can_parsers[Bus.adas]
+
+      odometer_km = float(cp_adas.vl["ID3B6UI_odometer"].get("UI_odometer", 0.0))
+      if 0.0 < odometer_km < 4294967.296:
+        self.vehicle_bus_available = True
+        ret.odometer = odometer_km
+
+    if self.vehicle_bus_available and Bus.adas in can_parsers:
+      cp_adas = can_parsers[Bus.adas]
+
+      prev_infotainment_3_finger_press = self.infotainment_3_finger_press
+      self.infotainment_3_finger_press = int(cp_adas.vl["UI_status2"]["UI_activeTouchPoints"])
+
+      ret.buttonEvents = [*ret.buttonEvents,
+                          *create_button_events(self.infotainment_3_finger_press, prev_infotainment_3_finger_press,
+                                                {3: ButtonType.lkas})]
+
+      bms_soc_ui = float(cp_adas.vl["ID292BMS_SOC"].get("SOCUI292", 0.0))
+      ui_range_mi = float(cp_adas.vl["ID33AUI_rangeSOC"].get("UI_Range", 0.0))
+      hv_batt_voltage_v = float(cp_adas.vl["ID132HVBattAmpVolt"].get("BattVoltage132", 0.0))
+      battery_details = None
+      try:
+        battery_details = ret.batteryDetails
+      except Exception:
+        battery_details = None
+
+      soc_ui = bms_soc_ui if 0.0 <= bms_soc_ui <= 102.3 else None
+      if soc_ui is not None:
+        ret.fuelGauge = min(100.0, soc_ui) / 100.0
+        if battery_details is not None:
+          battery_details.soc = soc_ui
+          battery_details.charge = soc_ui
+      if 0.0 <= ui_range_mi <= 1023.0 and battery_details is not None:
+        battery_details.capacity = ui_range_mi
+      if 0.0 < hv_batt_voltage_v <= 800.0 and battery_details is not None:
+        battery_details.voltage = hv_batt_voltage_v
+
+    cp_party = can_parsers[Bus.party]
+    cp_ap_party = can_parsers[Bus.ap_party]
+
+    speed_units = self.can_define.dv["DI_state"]["DI_speedUnits"].get(int(cp_party.vl["DI_state"]["DI_speedUnits"]), None)
+    speed_limit = cp_ap_party.vl["DAS_status"]["DAS_fusedSpeedLimit"]
+    if self.can_define.dv["DAS_status"]["DAS_fusedSpeedLimit"].get(int(speed_limit), None) in ["NONE", "UNKNOWN_SNA"]:
+      ret_iq.speedLimit = 0
+    else:
+      if speed_units == "KPH":
+        ret_iq.speedLimit = speed_limit * CV.KPH_TO_MS
+      elif speed_units == "MPH":
+        ret_iq.speedLimit = speed_limit * CV.MPH_TO_MS
+
+  @staticmethod
+  def get_parser(CP: structs.CarParams, CP_IQ: structs.IQCarParams) -> dict[StrEnum, CANParser]:
+    messages = {}
+
+    # Only tap the vehicle bus on cars where fingerprinting saw it: an always-on
+    # bus-1 parser trips bus_timeout -> canBusMissing on harnesses without the tap.
+    if CP_IQ.flags & TeslaFlagsIQ.HAS_VEHICLE_BUS and Bus.adas in DBC[CP.carFingerprint]:
+      messages[Bus.adas] = CANParser(DBC[CP.carFingerprint][Bus.adas], [], CANBUS.vehicle)
+
+    return messages
--- a/artifacts/package_runtime/iqdbc/lvbs/car/tesla/test_torque_blend.py
+++ b/artifacts/package_runtime/iqdbc/lvbs/car/tesla/test_torque_blend.py
@@ -0,0 +1,205 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import pytest
+
+from iqdbc.car.tesla.interface import CarInterface
+from iqdbc.car.vehicle_model import VehicleModel
+from iqdbc.lvbs.car.tesla.torque_blend import (
+  DT_LAT_CTRL,
+  STEER_OVERRIDE_MAX_LAT_ACCEL,
+  STEER_OVERRIDE_MIN_TORQUE,
+  STEER_OVERRIDE_TORQUE_RANGE,
+  STEER_OVERRIDE_TORQUE_ZERO_MAX,
+  SteeringTorqueZero,
+  TorqueBlendController,
+  calc_override_angle_limited,
+  get_steer_from_lat_accel,
+)
+from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
+
+VM = VehicleModel(CarInterface.get_non_essential_params("TESLA_MODEL_Y"))
+
+COOP_ON = SimpleNamespace(flags=TeslaFlagsIQ.COOP_STEERING.value)
+COOP_OFF = SimpleNamespace(flags=0)
+
+HAND_REST_TORQUE = 0.55
+ZERO_WEIGHT_SPEED = 20.0
+FULL_WEIGHT_SPEED = 8.0
+SETTLING_SECONDS = 300.0
+
+
+def _car_state(torque, v_ego, angle=0.0):
+  return SimpleNamespace(
+    out=SimpleNamespace(steeringTorque=torque, vEgo=v_ego, vEgoRaw=v_ego, steeringAngleDeg=angle, steeringRateDeg=0.0),
+    hands_on_level=0,
+  )
+
+
+def _drive(blend, torque, v_ego, seconds, angle=0.0, cp_iq=COOP_ON, lat_active=True):
+  steps = int(seconds / DT_LAT_CTRL)
+  out = 0.0
+  for _ in range(steps):
+    out = blend.update(angle, lat_active, cp_iq, _car_state(torque, v_ego, angle), VM).steeringAngleDeg
+  return out
+
+
+def _override(blend):
+  return blend.coop_apply_angle_last - blend.debug_angle_desired_limited
+
+
+MAX_NULLABLE_PRELOAD = STEER_OVERRIDE_MIN_TORQUE + STEER_OVERRIDE_TORQUE_ZERO_MAX
+
+
+@pytest.mark.parametrize("rest", [0.3, HAND_REST_TORQUE, MAX_NULLABLE_PRELOAD])
+@pytest.mark.parametrize("sign", [1.0, -1.0])
+def test_torque_zero_pulls_a_sustained_hand_rest_inside_the_deadzone(rest, sign):
+  zero = SteeringTorqueZero()
+  for _ in range(int(SETTLING_SECONDS / DT_LAT_CTRL)):
+    corrected = zero.update(sign * rest, True)
+  assert abs(corrected) <= STEER_OVERRIDE_MIN_TORQUE
+
+
+@pytest.mark.parametrize("sign", [1.0, -1.0])
+def test_a_settled_hand_rest_at_the_nullable_limit_produces_no_override(sign):
+  blend = TorqueBlendController()
+  _drive(blend, sign * MAX_NULLABLE_PRELOAD, ZERO_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
+  assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
+
+
+def test_override_unwinds_once_the_driver_releases():
+  blend = TorqueBlendController()
+  _drive(blend, 2.0, FULL_WEIGHT_SPEED, seconds=10.0)
+  assert abs(_override(blend)) > 1.0
+  _drive(blend, 0.0, FULL_WEIGHT_SPEED, seconds=20.0)
+  assert _override(blend) == pytest.approx(0.0, abs=1e-6)
+  assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
+
+
+def test_torque_zero_passes_a_step_through_untouched():
+  zero = SteeringTorqueZero()
+  assert zero.update(2.0, True) == pytest.approx(2.0, abs=1e-3)
+
+
+def test_torque_zero_is_bounded():
+  zero = SteeringTorqueZero()
+  for _ in range(int(SETTLING_SECONDS / DT_LAT_CTRL)):
+    zero.update(10.0, True)
+  assert zero.update(0.0, False) == pytest.approx(-STEER_OVERRIDE_TORQUE_ZERO_MAX)
+
+
+def test_torque_zero_holds_while_not_learning():
+  zero = SteeringTorqueZero()
+  zero.update(1.0, False)
+  assert zero.update(1.0, False) == pytest.approx(1.0)
+
+
+def test_sustained_hand_rest_stops_steering_the_car():
+  blend = TorqueBlendController()
+  _drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=1.0)
+  early = abs(_override(blend))
+  _drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
+  assert early > 0.1
+  assert abs(_override(blend)) < 0.1 * early
+
+
+def test_a_deliberate_push_still_overrides_after_the_zero_has_settled():
+  blend = TorqueBlendController()
+  _drive(blend, HAND_REST_TORQUE, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
+  _drive(blend, HAND_REST_TORQUE + 1.5, FULL_WEIGHT_SPEED, seconds=1.0)
+  assert abs(_override(blend)) > 1.0
+
+
+def test_accumulator_cannot_charge_where_its_weight_is_zero():
+  blend = TorqueBlendController()
+  _drive(blend, 2.0, ZERO_WEIGHT_SPEED, seconds=30.0)
+  assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
+
+
+def test_accumulator_drains_once_the_car_passes_the_crossover_speed():
+  blend = TorqueBlendController()
+  _drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=10.0)
+  assert abs(blend.override_angle_accu) > 1.0
+  _drive(blend, 2.5, ZERO_WEIGHT_SPEED, seconds=5.0)
+  assert blend.override_angle_accu == pytest.approx(0.0, abs=1e-6)
+
+
+def test_low_speed_override_can_exceed_the_direct_term_authority():
+  blend = TorqueBlendController()
+  _drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=60.0)
+  authority = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, FULL_WEIGHT_SPEED, VM, STEER_OVERRIDE_MAX_LAT_ACCEL)
+  assert abs(blend.override_angle_accu) > authority
+
+
+def test_a_sustained_low_speed_push_reaches_a_large_override():
+  blend = TorqueBlendController()
+  _drive(blend, 2.5, FULL_WEIGHT_SPEED, seconds=10.0)
+  assert abs(_override(blend)) > 20.0
+
+
+@pytest.mark.parametrize("v_ego", [3.0, 5.0, FULL_WEIGHT_SPEED, 12.0])
+def test_override_stays_inside_the_max_lat_accel_envelope_plus_the_direct_term(v_ego):
+  blend = TorqueBlendController()
+  _drive(blend, 2.5, v_ego, seconds=60.0)
+  envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, v_ego, VM)
+  direct_authority = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, v_ego, VM, STEER_OVERRIDE_MAX_LAT_ACCEL)
+  assert abs(_override(blend)) <= envelope + direct_authority
+
+
+@pytest.mark.parametrize("v_ego", [3.0, 5.0, FULL_WEIGHT_SPEED, 12.0])
+def test_a_pinned_push_cannot_run_the_contribution_away(v_ego):
+  blend = TorqueBlendController()
+  _drive(blend, 2.5, v_ego, seconds=130.0)
+  envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, v_ego, VM)
+  capability = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, v_ego, VM, STEER_OVERRIDE_MAX_LAT_ACCEL) / envelope
+  assert abs(blend.override_angle_accu) * (1.0 - capability) <= envelope + 1e-6
+
+
+def test_the_envelope_opens_up_as_speed_drops():
+  low = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, 3.0, VM)
+  high = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, 12.0, VM)
+  assert low > 10 * high
+
+
+def test_relative_weight_is_zero_above_the_direct_capability_crossover():
+  capability = (calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, ZERO_WEIGHT_SPEED, VM, STEER_OVERRIDE_MAX_LAT_ACCEL) /
+                get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, ZERO_WEIGHT_SPEED, VM))
+  assert capability == pytest.approx(1.0)
+
+
+HEAVY_REST = 0.80
+
+
+@pytest.mark.parametrize("sign", [1.0, -1.0])
+def test_the_zero_nulls_a_hand_rest_heavier_than_the_deadzone(sign):
+  assert HEAVY_REST > 2 * STEER_OVERRIDE_MIN_TORQUE
+  blend = TorqueBlendController()
+  _drive(blend, sign * HEAVY_REST, FULL_WEIGHT_SPEED, seconds=SETTLING_SECONDS)
+  assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
+
+
+LIGHT_PUSH = 0.45
+
+
+@pytest.mark.parametrize("v_ego", [FULL_WEIGHT_SPEED, 17.0])
+def test_a_light_push_overrides_without_needing_the_old_half_newton(v_ego):
+  assert STEER_OVERRIDE_MIN_TORQUE < LIGHT_PUSH < 0.5
+  blend = TorqueBlendController()
+  _drive(blend, LIGHT_PUSH, v_ego, seconds=5.0)
+  assert abs(_override(blend)) > 0.2
+
+
+def test_torque_below_the_deadzone_never_overrides():
+  blend = TorqueBlendController()
+  _drive(blend, STEER_OVERRIDE_MIN_TORQUE * 0.5, FULL_WEIGHT_SPEED, seconds=5.0)
+  assert abs(_override(blend)) == pytest.approx(0.0, abs=1e-6)
+
+
+def test_coop_disabled_leaves_the_command_untouched():
+  blend = TorqueBlendController()
+  angle = 5.0
+  out = _drive(blend, 2.0, FULL_WEIGHT_SPEED, seconds=5.0, angle=angle, cp_iq=COOP_OFF)
+  assert out == pytest.approx(angle, abs=1e-6)
+  assert blend.override_angle_accu == pytest.approx(0.0)
--- a/artifacts/package_runtime/iqdbc/lvbs/car/tesla/torque_blend.py
+++ b/artifacts/package_runtime/iqdbc/lvbs/car/tesla/torque_blend.py
@@ -0,0 +1,340 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import math
+import numpy as np
+from collections import namedtuple
+from dataclasses import replace
+
+from iqdbc.car import structs, rate_limit, DT_CTRL
+from iqdbc.car.vehicle_model import VehicleModel
+from iqdbc.car.lateral import apply_steer_angle_limits_vm
+from iqdbc.car.tesla.values import CarControllerParams
+from iqdbc.lvbs.car.tesla.values import TeslaFlagsIQ
+
+
+DT_LAT_CTRL = DT_CTRL * CarControllerParams.STEER_STEP
+
+class TorqueBlendParams(CarControllerParams):
+  ANGLE_LIMITS = replace(CarControllerParams.ANGLE_LIMITS, MAX_ANGLE_RATE=5)
+
+STEERING_DEG_PHASE_LEAD_COEFF = 8.0
+
+# angle override # todo implement steering torque inertia compensation to increase gains
+STEER_OVERRIDE_MIN_TORQUE = 0.35 # Nm - only noise now, SteeringTorqueZero removes the bias this used to cover
+STEER_OVERRIDE_MAX_TORQUE = 2.5 # Nm max torque before EPS disengages, LKAS takes over at 1.8Nm
+STEER_OVERRIDE_MAX_LAT_ACCEL = 1.5 # m/s^2 - determines angle rate - speed dependent - similar to Tesla comfort steering mode
+STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT = 10 # deg/Nm stability and smoothness for angle control  # todo this could be increased after solving feedback stability
+
+# angle ramping
+STEER_OVERRIDE_MAX_LAT_JERK = 2.0 # m/s^3 - determines angle ramping rate - speed dependent
+STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK # m/s^3 -  for low speed angle ramp down
+# stability and smoothness for angle ramp control - at very low speeds this takes precedence over jerk settings
+STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT = 100 # deg/s/Nm - should be less than CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE
+STEER_OVERRIDE_TORQUE_RANGE = STEER_OVERRIDE_MAX_TORQUE - STEER_OVERRIDE_MIN_TORQUE
+
+STEER_OVERRIDE_TORQUE_ZERO_TAU = 60.0
+STEER_OVERRIDE_TORQUE_ZERO_MAX = 0.5 # Nm - sized to the observed hand rest and sensor bias, not to the deadzone
+
+# model fighting mitigation
+STEER_DESIRED_LIMITER_ALLOW_SPEED = 6.0 # m/s - below this speed the desired angle limiter is active
+STEER_DESIRED_LIMITER_ACCEL = 100 # deg/s^2 when override angle ramp is active
+STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER = 0.7 # second
+
+# limit model acceleration when engaging
+STEER_RESUME_RATE_LIMIT_RAMP_RATE = 500 # deg/s^2 - controls rate of rise of angle rate limit, not angle directly
+
+
+TorqueBlendDataIQ = namedtuple("TorqueBlendDataIQ",
+                                ["steeringAngleDeg", "lat_active", "control_type"])
+
+def get_steer_from_lat_accel(lat_accel, v_ego: float, VM: VehicleModel):
+  """Calculate the maximum steering angle based on lateral acceleration."""
+  curvature = lat_accel / (max(1, v_ego) ** 2)  # 1/m
+  return math.degrees(VM.get_steer_from_curvature(curvature, v_ego, 0))  # deg
+
+
+def apply_bounds(signal: float, limit: float) -> float:
+  """Limit input to a range."""
+  return float(np.clip(signal, -limit, limit))
+
+
+def apply_deadzone(signal: float, deadzone: float) -> float:
+  """Apply deadzone to input."""
+  return signal - apply_bounds(signal, deadzone)
+
+
+def calc_override_angle_limited(torque: float, vEgo: float, VM: VehicleModel, lat_accel) -> float:
+  """
+  Map driver torque to lateral acceleration and convert to steering angle.
+  Limit gain for stability with EPS and torque sensor interaction.
+  """
+
+  # lateral accel is linear in respect to angle so it's fine to interpolate it with torque
+  torque_to_angle = get_steer_from_lat_accel(lat_accel, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE
+
+  # limit the gain to prevent jerkiness and instability
+  gain_limit = STEER_OVERRIDE_LAT_ACCEL_GAIN_LIMIT
+  override_angle_target = torque * min(torque_to_angle, gain_limit)
+
+  return override_angle_target
+
+
+def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleModel, lat_jerk) -> float:
+  """
+  Map driver torque to lateral jerk and convert to steering speed.
+  Limit gain for stability with EPS and torque sensor interaction.
+  """
+
+  # prevents windup in carcontroller rate limiter
+  lat_jerk = min(lat_jerk, TorqueBlendParams.ANGLE_LIMITS.MAX_LATERAL_JERK)
+
+  # lateral accel is linear in respect to angle so it's fine to interpolate it with torque
+  torque_to_angle = get_steer_from_lat_accel(lat_jerk, vEgo, VM) / STEER_OVERRIDE_TORQUE_RANGE
+  # limit the gain to prevent jerkiness and instability
+  gain_limit = min(STEER_OVERRIDE_LAT_JERK_GAIN_LIMIT, CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_CTRL / STEER_OVERRIDE_TORQUE_RANGE)
+  override_angle_rate = torque * min(torque_to_angle, gain_limit)
+
+  # prevent windup in angle rate limiter
+  return apply_bounds(override_angle_rate * DT_LAT_CTRL, TorqueBlendParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
+
+
+class SteeringTorqueZero:
+  def __init__(self):
+    self._zero = 0.0
+
+  def reset(self) -> None:
+    self._zero = 0.0
+
+  def update(self, torque: float, learning: bool) -> float:
+    if learning:
+      self._zero += (torque - self._zero) * (DT_LAT_CTRL / STEER_OVERRIDE_TORQUE_ZERO_TAU)
+      self._zero = apply_bounds(self._zero, STEER_OVERRIDE_TORQUE_ZERO_MAX)
+    return torque - self._zero
+
+
+class SteerRateLimiter:
+  """Handles rate limiting of steering angle changes with a configurable rate."""
+  def __init__(self):
+    self._last = 0.0
+
+  def reset(self, angle: float) -> None:
+    """Reset the rate limiter state with the given angle."""
+    self._last = angle
+
+  def update(self, angle: float, angle_delta_lim: float) -> float:
+    angle_lim = rate_limit(angle, self._last, -angle_delta_lim, angle_delta_lim)
+    self._last = angle_lim
+    return angle_lim
+
+
+class SteerAccelLimiter:
+  """
+  Second-order limiter for steering angle:
+  - Limits angular acceleration (change in allowed angular rate).
+  - Enforces a hard max angular rate.
+  """
+  def __init__(self):
+    self.delta_rl = SteerRateLimiter()
+    self.angle_cmd = 0.0
+
+  def reset(self, angle: float) -> None:
+    self.delta_rl.reset(0)
+    self.angle_cmd = angle
+
+  def update(self, angle_target: float, max_rate: float, accel: float, decel: float, dt: float) -> float:
+    if dt <= 0.0:
+      return self.angle_cmd
+
+    # acceleration limits per update step
+    accel_delta = max(0.0, accel) * (dt * dt)
+    decel_delta = max(0.0, decel) * (dt * dt)
+
+    err = angle_target - self.angle_cmd
+    err = apply_bounds(err, max(0.0, max_rate) * dt)
+
+    # acceleration (towards target) or deceleration (away from target)
+    if err * self.delta_rl._last < 0:
+      delta = decel_delta
+    else:
+      delta = accel_delta
+
+    # Handle large decel (enabled with inf value)
+    if decel == np.inf and err * self.delta_rl._last < 0:
+      # if output crosses the target or target crosses the output
+      self.delta_rl._last = 0
+      angle_out = self.angle_cmd
+    else:
+      self.delta_rl._last = self.delta_rl.update(err, delta)
+      if decel == np.inf:
+        # if we are close to target, snap to it before we cross it
+        self.delta_rl._last = apply_bounds(self.delta_rl._last, abs(err))
+      angle_out = self.angle_cmd + self.delta_rl._last
+
+    # Integrate
+    self.angle_cmd = angle_out
+
+    return angle_out
+
+
+class TorqueBlendController:
+  def __init__(self):
+    self.coop_apply_angle_last = 0
+    self.blend_apply_angle_last_sat = 0
+    self.override_angle_accu = 0
+    self.override_active_counter = 0  # Counter for how many cycles torque is below threshold
+    self.resume_rate_limiter_delta = SteerRateLimiter()
+    self.resume_rate_limiter = SteerRateLimiter()
+    self.override_accel_rate_limiter = SteerAccelLimiter()
+    self.torque_zero = SteeringTorqueZero()
+    self.debug_angle_desired_limited = 0
+
+  def apply_override_angle_direct(self, lat_active: bool, driverTorque: float, vEgo: float, VM: VehicleModel) -> float:
+    """
+    Emulates steering springiness based on lateral acceleration exerted on the steering rack.
+    We rely on apply_override_angle_ramp to reach the max angle at low speeds.
+    At low speed lateral acceleration approaches infinity and it is not good proxy
+    for the torque to target angle conversion and needs to be limited
+
+    """
+    if not lat_active:
+      return 0.0
+
+    ## torque to position
+    # ignore torque sensor offset and disturbances
+    steering_torque_with_deadzone = apply_deadzone(driverTorque, STEER_OVERRIDE_MIN_TORQUE)
+    angle_override = calc_override_angle_limited(steering_torque_with_deadzone, vEgo, VM, STEER_OVERRIDE_MAX_LAT_ACCEL)
+    return angle_override
+
+  def apply_override_angle_relative(self, lat_active: bool, driverTorque: float, vEgo: float,
+                                    VM: VehicleModel, unwind_weight: float = 1.0) -> float:
+    """
+    Converts steering torque to steering rotation rate.
+    Physically angle rate is related to viscous damping of tires rotating on the ground.
+    Here, however, the angle rate target is obtained from lateral jerk limit
+    as a reasonable safe rate which decays quadratically with vehicle speed.
+    """
+    if not lat_active:
+      self.override_angle_accu = 0
+      return 0
+
+    # unwind accumulator toward zero if the previous loop saturated (apply_steer_angle_limits_vm)
+    unwind = (self.coop_apply_angle_last - self.blend_apply_angle_last_sat) * unwind_weight
+    if self.override_angle_accu * unwind > 0:
+      unwind = apply_bounds(unwind, abs(self.override_angle_accu))
+      self.override_angle_accu -= unwind
+
+    # torque biasing emulates the steering centering when released:
+    if self.override_angle_accu > 0 and abs(vEgo) > 0.1:
+      torque_biased = driverTorque - STEER_OVERRIDE_MIN_TORQUE
+    elif self.override_angle_accu < 0 and abs(vEgo) > 0.1:
+      torque_biased = driverTorque + STEER_OVERRIDE_MIN_TORQUE
+    else:
+      # when override_angle_accu is reset this turns off  everything
+      torque_biased = apply_deadzone(driverTorque, STEER_OVERRIDE_MIN_TORQUE)
+
+    # higher rate when centering
+    angle_override_delta = calc_override_angle_delta_limited(torque_biased, vEgo, VM,
+                          STEER_OVERRIDE_MAX_LAT_JERK if (torque_biased * self.override_angle_accu) > 0
+                          else STEER_OVERRIDE_MAX_LAT_JERK_CENTERING)
+
+    # ramp the angle
+    new_override_angle_accu = self.override_angle_accu + angle_override_delta
+    # snap to 0 if sign changes and driver torque is steering centering zone
+    if (new_override_angle_accu * self.override_angle_accu) < 0 and abs(driverTorque) < STEER_OVERRIDE_MIN_TORQUE:
+      new_override_angle_accu = 0
+
+    self.override_angle_accu = new_override_angle_accu
+
+    return self.override_angle_accu
+
+  def apply_override_angle_combined(self, lat_active: bool, driverTorque: float, vEgo: float, VM: VehicleModel) -> float:
+    """
+    Combines direct and relative override angles based on direct angle override limitations (stability and practical range depending on vehicle speed).
+    Effectively vehicle-speed based transition.
+    """
+    if not lat_active:
+      return 0
+
+    override_envelope = get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, vEgo, VM)
+    # calculate capability of direct angle override (fully active above ~36kph)
+    direct_override_capability = calc_override_angle_limited(STEER_OVERRIDE_TORQUE_RANGE, vEgo, VM,
+                                                            STEER_OVERRIDE_MAX_LAT_ACCEL) / override_envelope
+
+    angle_override_direct = self.apply_override_angle_direct(lat_active, driverTorque, vEgo, VM)
+    relative_weight = 1.0 - direct_override_capability
+    if relative_weight > 0.0:
+      self.apply_override_angle_relative(lat_active, driverTorque, vEgo, VM, unwind_weight=relative_weight)
+      self.override_angle_accu = apply_bounds(self.override_angle_accu, override_envelope / relative_weight)
+    else:
+      self.center_override_angle_accu(vEgo, VM)
+
+    return angle_override_direct * direct_override_capability + self.override_angle_accu * relative_weight
+
+  def center_override_angle_accu(self, vEgo: float, VM: VehicleModel) -> None:
+    centering_delta = calc_override_angle_delta_limited(STEER_OVERRIDE_TORQUE_RANGE, vEgo, VM,
+                                                       STEER_OVERRIDE_MAX_LAT_JERK_CENTERING)
+    self.override_angle_accu -= apply_bounds(self.override_angle_accu, centering_delta)
+
+  def overriding_steer_desired_accel_limit(self, lat_active: bool, apply_angle: float, vEgo: float, steeringTorque: float) -> float:
+    """
+    Acceleration rate limiter - limits acceleration but allows for quick deceleration (no overshoot)
+    """
+    if not lat_active:
+      self.override_accel_rate_limiter.reset(apply_angle)
+      return apply_angle
+
+    if abs(steeringTorque) >= STEER_OVERRIDE_MIN_TORQUE:
+      self.override_active_counter = 0
+    else:
+      self.override_active_counter += DT_LAT_CTRL
+      self.override_active_counter = min(self.override_active_counter, STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER)
+
+    max_angle_rate = CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE / DT_LAT_CTRL # MAX_ANGLE_RATE is per frame units so convert to real rate
+    # this ensures no acceleration limit when override is disabled:
+    max_angle_accel = max_angle_rate / DT_LAT_CTRL # ensures max deceleration
+    if vEgo < STEER_DESIRED_LIMITER_ALLOW_SPEED:
+      # Interpolate between STEER_DESIRED_LIMITER_ACCEL and max_angle_accel based on counter progress
+      max_angle_accel = np.interp(
+        self.override_active_counter,
+        [0, STEER_DESIRED_LIMITER_OVERRIDE_ACTIVE_COUNTER],
+        [STEER_DESIRED_LIMITER_ACCEL, max_angle_accel]
+      )
+    # max_angle_rate / DT_LAT_CTRL ensures max deceleration
+    return self.override_accel_rate_limiter.update(apply_angle, max_angle_rate, max_angle_accel, np.inf, DT_LAT_CTRL)
+
+  def resume_steer_desired_rate_limit(self, lat_active: bool, apply_angle: float, steering_angle: float) -> float:
+    """Limits steering wheel acceleration when resuming steering"""
+    if not lat_active:
+      # reset and bypass
+      self.resume_rate_limiter_delta.reset(0)
+      self.resume_rate_limiter.reset(steering_angle)
+      return steering_angle
+
+    angle_rate_delta_lim = self.resume_rate_limiter_delta.update(CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE,
+                                                         STEER_RESUME_RATE_LIMIT_RAMP_RATE * DT_LAT_CTRL**2)
+    apply_angle_lim = self.resume_rate_limiter.update(apply_angle, angle_rate_delta_lim)
+    return apply_angle_lim
+
+  def update(self, apply_angle, lat_active, CP_IQ: structs.IQCarParams, CS: structs.CarState, VM: VehicleModel) -> TorqueBlendDataIQ:
+    # estimate real steering angle by adding rate to the tesla filtered angle
+    steeringAngleDegPhaseLead = CS.out.steeringAngleDeg + CS.out.steeringRateDeg / STEERING_DEG_PHASE_LEAD_COEFF
+
+    angle_coop_enabled = CP_IQ.flags & TeslaFlagsIQ.COOP_STEERING.value
+
+    # avoid sudden rotation on engagement
+    apply_angle = self.resume_steer_desired_rate_limit(lat_active, apply_angle, steeringAngleDegPhaseLead)
+
+    if angle_coop_enabled:
+      # apply_angle = self.overriding_steer_desired_accel_limit(lat_active, apply_angle, CS.out.vEgo, CS.out.steeringTorque)
+      self.debug_angle_desired_limited = apply_angle #! debug
+
+      driver_torque = self.torque_zero.update(CS.out.steeringTorque, lat_active)
+      apply_angle += self.apply_override_angle_combined(lat_active, driver_torque, CS.out.vEgo, VM)
+
+    # final rate limit - matching panda safety
+    self.coop_apply_angle_last = apply_angle
+    self.blend_apply_angle_last_sat = apply_steer_angle_limits_vm(apply_angle, self.blend_apply_angle_last_sat, CS.out.vEgoRaw,
+                                                    CS.out.steeringAngleDeg, lat_active, TorqueBlendParams, VM)
+
+    return TorqueBlendDataIQ(self.blend_apply_angle_last_sat, lat_active, 1)  # 1 = angle control
--- a/artifacts/package_runtime/iqdbc/lvbs/car/tesla/values.py
+++ b/artifacts/package_runtime/iqdbc/lvbs/car/tesla/values.py
@@ -0,0 +1,15 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from enum import IntFlag
+
+
+class TeslaFlagsIQ(IntFlag):
+  HAS_VEHICLE_BUS = 1  # 3-finger infotainment press signal is present on the VEHICLE bus with the deprecated Tesla harness installed
+  COOP_STEERING = 2  # virtual torque blending
+  FSD_VISUALIZATION = 4
+
+
+class TeslaSafetyFlagsIQ:
+  HAS_VEHICLE_BUS = 1
+  FSD_VISUALIZATION = 2