IQ.Pilot Release Commit @ 4521b0f

2026-08-22 10:38:43 -05:00
parent e142a0001a
commit 11cefcb266
29 changed files with 822 additions and 111 deletions
--- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py
+++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/carcontroller.py
@@ -259,8 +259,8 @@ class CarController(CarControllerBase):
    self._pq_patch_checked = not bool(CP.flags & VolkswagenFlags.PQ) or self.eps_timer_workaround
    self.hca_frame_timer_resetting = 0
    self.hca_frame_low_torque = 0
-    self.long_override_counter = 0
-    self.long_disabled_counter = 0
+    self.acc_hold_type_last = mebcan.ACC_HMS_NO_REQUEST
+    self.acc_hold_ramp_counter = 0
    self.standstill_manager = MQBStandstillManager(CP.mass, self.CCP.ACCEL_MIN) if self.CCS == mqbcan else None
    self.radar_handler = PQRadarHandler(self.CAN) if self.CCS is pqcan else None
    self.blend_stock_radar = False
@@ -474,11 +474,7 @@ class CarController(CarControllerBase):
        accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.enabled else 0)

        long_override = CC.cruiseControl.override or CS.out.gasPressed
-        self.long_override_counter = min(self.long_override_counter + 1, 5) if long_override else 0
-        long_override_begin = long_override and self.long_override_counter < 5

-        self.long_disabled_counter = min(self.long_disabled_counter + 1, 5) if not CC.enabled else 0
-        long_disabling = not CC.enabled and self.long_disabled_counter < 5

        critical_state = hud_control.visualAlert == VisualAlert.fcw
        if CC.longComfortMode and self.long_jerk_control is not None and self.long_limit_control is not None:
@@ -486,8 +482,10 @@ class CarController(CarControllerBase):
          self.long_limit_control.update(CC.enabled, CS.out.vEgoRaw, hud_control.setSpeed, hud_control.leadDistance, hud_control.leadVisible, critical_state)

        acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, long_override)
-        acc_hold_type = self.CCS.acc_hold_type(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
-                                               CS.esp_hold_confirmation, long_override, long_override_begin, long_disabling)
+        acc_hold_type, self.acc_hold_ramp_counter = self.CCS.acc_hold_type(
+          CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
+          CS.esp_hold_confirmation, long_override, self.acc_hold_type_last, self.acc_hold_ramp_counter)
+        self.acc_hold_type_last = acc_hold_type
        can_sends.extend(self.CCS.create_acc_accel_control(
          self.packer_pt, self.CAN.pt, self.CP, CS.acc_type, CC.enabled,
          self.long_jerk_control.get_jerk_up() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,
--- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py
+++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/mebcan.py
@@ -16,6 +16,7 @@ ACC_HMS_RAMP_RELEASE = 5
 ACC_HMS_RELEASE      = 4
 ACC_HMS_HOLD         = 1
 ACC_HMS_NO_REQUEST   = 0
+ACC_HMS_RAMP_FRAMES  = 5

 ACC_HUD_ERROR    = 6
 ACC_HUD_OVERRIDE = 4
@@ -160,21 +161,16 @@ def acc_control_value(main_switch_on, acc_faulted, long_active, override):
  return acc_control


-def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping, esp_hold, override, override_begin, long_disabling):
+def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping, esp_hold, override,
+                  prev_acc_hold_type, ramp_counter):
  # warning: car is reacting to hold mechanic even with long control off

  if acc_faulted:
    acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
  elif not long_active:
-    if long_disabling:
-      acc_hold_type = ACC_HMS_RAMP_RELEASE # ramp release of requests right after disabling long control (prevents car error with EPB at low speed)
-    else:
-      acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
+    acc_hold_type = ACC_HMS_NO_REQUEST # no hold request
  elif override:
-    if override_begin:
-      acc_hold_type = ACC_HMS_RAMP_RELEASE # ramp release of requests at the beginning of override (prevents car error with EPB at low speed)
-    else:
-      acc_hold_type = ACC_HMS_NO_REQUEST # overriding / no request
+    acc_hold_type = ACC_HMS_NO_REQUEST # overriding / no request
  elif starting:
    acc_hold_type = ACC_HMS_RELEASE # release request and startup
  elif stopping or esp_hold:
@@ -182,7 +178,18 @@ def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping,
  else:
    acc_hold_type = ACC_HMS_NO_REQUEST # no hold request

-  return acc_hold_type
+  if acc_hold_type == ACC_HMS_HOLD:
+    ramp_counter = 0
+  elif prev_acc_hold_type == ACC_HMS_HOLD and acc_hold_type == ACC_HMS_NO_REQUEST:
+    acc_hold_type = ACC_HMS_RAMP_RELEASE
+    ramp_counter = ACC_HMS_RAMP_FRAMES
+  elif ramp_counter > 0:
+    acc_hold_type = ACC_HMS_RAMP_RELEASE
+    ramp_counter -= 1
+  else:
+    pass
+
+  return acc_hold_type, ramp_counter


 def create_acc_accel_control(packer, bus, CP, acc_type, acc_enabled, upper_jerk, lower_jerk, upper_control_limit, lower_control_limit,
--- a/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/pqcan.py
+++ b/artifacts/package_sources/iqdbc/iqdbc/car/volkswagen/pqcan.py
@@ -139,7 +139,7 @@ def create_blinker_control(packer, bus, leftBlinker, rightBlinker):


 def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, unavailable, decel, d_unresponsive):
-  priodisp = 0 if fcw_alert else 1 if (acc_hud_status == 4 or decel or leadVisible) else 2 if (acc_hud_status in (3, 2)) else 0
+  priodisp = 0 if fcw_alert else 0 if (acc_hud_status == 4 or decel) else 2 if (acc_hud_status in (3, 2) or leadVisible) else 0
  leadDistanceBars = distanceBars + 1 if distanceBars in (1, 2, 3) else 2
  values = {
    "ACA_StaACC": acc_hud_status,
@@ -149,7 +149,6 @@ def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance,
    "ACA_gemZeitl": min(15, max(1, int(round(leadDistance)))) if leadVisible else 0,
    "ACA_PrioDisp": priodisp,
    "ACA_Akustik1": d_unresponsive,
-    # "ACA_Fahrerhinw": unavailable,
    "ACA_Akustik2": fcw_alert,
    "ACA_ACC_Verz": decel,
  }
--- a/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/test_torque_blend.py
+++ b/artifacts/package_sources/iqdbc/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_sources/iqdbc/iqdbc/lvbs/car/tesla/torque_blend.py
+++ b/artifacts/package_sources/iqdbc/iqdbc/lvbs/car/tesla/torque_blend.py
@@ -21,7 +21,7 @@ class TorqueBlendParams(CarControllerParams):
 STEERING_DEG_PHASE_LEAD_COEFF = 8.0

 # angle override # todo implement steering torque inertia compensation to increase gains
-STEER_OVERRIDE_MIN_TORQUE = 0.5 # Nm - based on typical steering bias + noise
+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
@@ -33,6 +33,9 @@ STEER_OVERRIDE_MAX_LAT_JERK_CENTERING = TorqueBlendParams.ANGLE_LIMITS.MAX_LATER
 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
@@ -96,6 +99,20 @@ def calc_override_angle_delta_limited(torque: float, vEgo: float, VM: VehicleMod
  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):
@@ -169,6 +186,7 @@ class TorqueBlendController:
    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:
@@ -238,16 +256,25 @@ class TorqueBlendController:
    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) /
-                   get_steer_from_lat_accel(STEER_OVERRIDE_MAX_LAT_ACCEL, vEgo, VM))
+    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
-    angle_override_relative = self.apply_override_angle_relative(lat_active, driverTorque, vEgo, VM,
-                                                                 unwind_weight=relative_weight)
+    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 + angle_override_relative * relative_weight
+    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:
    """
@@ -302,7 +329,8 @@ class TorqueBlendController:
      # 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

-      apply_angle += self.apply_override_angle_combined(lat_active, CS.out.steeringTorque, CS.out.vEgo, VM)
+      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
--- a/artifacts/package_sources/panda/board/drivers/can_common.h
+++ b/artifacts/package_sources/panda/board/drivers/can_common.h
@@ -191,6 +191,10 @@ void ignition_can_hook(CANPacket_t *msg) {
    static int toyota_gear = TOYOTA_GEAR_P;
    static int toyota_hybrid_gear = TOYOTA_HYBRID_GEAR_P;
    static bool toyota_hybrid_gear_seen = false;
+    static bool vw_meb_seen = false;
+    static bool vw_meb_getriebe_out_of_p = false;
+    static bool vw_meb_gateway_out_of_p = false;
+    static int prev_counter_vw_meb = -1;

    // GM exception
    if ((msg->addr == 0x1F1U) && (len == 8)) {
@@ -293,6 +297,30 @@ void ignition_can_hook(CANPacket_t *msg) {
      }
    }

+    if ((msg->addr == 0xADU) && (len == 8)) {
+      int fahrstufe = (msg->data[5] >> 2) & 0xFU;
+      vw_meb_getriebe_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14);
+    }
+
+    if ((msg->addr == 0x3DCU) && (len == 8)) {
+      int fahrstufe = msg->data[5] & 0xFU;
+      vw_meb_gateway_out_of_p = (fahrstufe >= 6) && (fahrstufe <= 14);
+    }
+
+    if ((msg->addr == 0x13DU) && (len == 32)) {
+      vw_meb_seen = true;
+    }
+
+    if ((msg->addr == 0x3C0U) && (len == 4) && vw_meb_seen) {
+      int counter = msg->data[1] & 0xFU;
+      if ((counter == ((prev_counter_vw_meb + 1) % 16)) && (prev_counter_vw_meb != -1)) {
+        bool vw_meb_out_of_park = vw_meb_getriebe_out_of_p || vw_meb_gateway_out_of_p;
+        ignition_can = (((msg->data[2] >> 1) & 1U) != 0U) && vw_meb_out_of_park;
+        ignition_can_cnt = 0U;
+        ignition_can_gates_line = true;
+      }
+      prev_counter_vw_meb = counter;
+    }

  }
 }