IQ.Pilot Release Commit @ 4521b0f

iqpilot/common/steer_delay.py

@@ -6,6 +6,7 @@ live estimator learned, or the driver's fixed software delay — gated by the
 "IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
 not subscribe to lateralDelay can still read the current value.
 """
+from iqpilot.cereal import car
 from iqpilot.common.params import Params
 
 _ENABLE_KEY = "IQLiveSteerDelay"
@@ -13,13 +14,32 @@ _FIXED_KEY = "IQSoftwareSteerDelay"
 _CACHE_KEY = "IQSteerDelayCache"
 
 
+def fixed_steer_delay(params, stock_delay):
+  """The rack's own delay plus the driver's IQSoftwareSteerDelay offset, as the UI reports it."""
+  return stock_delay + float(params.get(_FIXED_KEY, return_default=True))
+
+
 def resolve_steer_delay(params, stock_delay):
-  """Learned lateral delay while live-learning is enabled, otherwise the stock delay."""
+  """Learned lateral delay while live-learning is enabled, otherwise the driver's fixed delay."""
   if not params.get_bool(_ENABLE_KEY):
-    return stock_delay
+    return fixed_steer_delay(params, stock_delay)
   return float(params.get(_CACHE_KEY, return_default=True))
 
 
+def lateral_action_delay(params, car_params, live_delay):
+  """Delay the lateral path should be planned against.
+
+  Angle cars honour the IQLiveSteerDelay toggle so that with live learning off the
+  estimate never reaches the path: lagd cross-correlates against localizer lateral
+  accel, so it reports whole-vehicle response (~0.36 s measured on VW MQB, 0.44 s on
+  Tesla) where the lookahead wants actuator delay (~0.10 s). Torque cars keep the
+  live estimate.
+  """
+  if car_params.steerControlType == car.CarParams.SteerControlType.angle:
+    return resolve_steer_delay(params, car_params.steerActuatorDelay)
+  return live_delay
+
+
 def cached_steer_delay():
   """Last value SteerDelayPublisher mirrored into the param — usable without a
   lateralDelay subscription (e.g. at process startup)."""
@@ -34,10 +54,7 @@ class SteerDelayPublisher:
     self._params = Params()
     self._actuator_delay = car_params.steerActuatorDelay
 
-  def _fixed_delay(self):
-    return self._actuator_delay + self._params.get(_FIXED_KEY, return_default=True)
-
   def update(self, lag_msg):
     live = self._params.get_bool(_ENABLE_KEY)
-    value = lag_msg.lateralDelay.lateralDelay if live else self._fixed_delay()
+    value = lag_msg.lateralDelay.lateralDelay if live else fixed_steer_delay(self._params, self._actuator_delay)
     self._params.put_nonblocking(_CACHE_KEY, value)

iqpilot/common/tests/test_steer_delay.py

@@ -0,0 +1,91 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+import time
+
+import pytest
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car
+from iqpilot.common.params import Params
+from iqpilot.common.steer_delay import (
+  SteerDelayPublisher,
+  cached_steer_delay,
+  fixed_steer_delay,
+  lateral_action_delay,
+  resolve_steer_delay,
+)
+
+ANGLE = car.CarParams.SteerControlType.angle
+TORQUE = car.CarParams.SteerControlType.torque
+
+LIVE_DELAY = 0.4387
+RACK_DELAY = 0.10
+OFFSET = 0.05
+
+
+@pytest.fixture
+def params(tmp_path, monkeypatch):
+  monkeypatch.setenv("PARAMS_ROOT", str(tmp_path))
+  p = Params()
+  p.put("IQSteerDelayCache", LIVE_DELAY)
+  p.put("IQSoftwareSteerDelay", OFFSET)
+  return p
+
+
+def _car_params(steer_control_type):
+  cp = car.CarParams.new_message()
+  cp.steerControlType = steer_control_type
+  cp.steerActuatorDelay = RACK_DELAY
+  return cp
+
+
+def _lateral_delay_msg(value):
+  msg = messaging.new_message("lateralDelay")
+  msg.lateralDelay.lateralDelay = value
+  return msg.as_reader()
+
+
+def test_params_fixture_is_isolated_from_the_real_device(params, tmp_path):
+  assert str(tmp_path) in params.get_param_path("")
+
+
+@pytest.mark.parametrize("live_enabled", [True, False])
+def test_torque_cars_always_use_live_delay(params, live_enabled):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  assert lateral_action_delay(params, _car_params(TORQUE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
+
+
+def test_angle_cars_ignore_live_delay_when_self_tuning_is_off(params):
+  params.put_bool("IQLiveSteerDelay", False)
+  delay = lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY)
+  assert delay == pytest.approx(RACK_DELAY + OFFSET)
+  assert delay != pytest.approx(LIVE_DELAY)
+
+
+def test_angle_cars_use_cached_delay_when_self_tuning_is_on(params):
+  params.put_bool("IQLiveSteerDelay", True)
+  assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(LIVE_DELAY)
+
+
+@pytest.mark.parametrize("offset", [0.05, 0.20, 0.50])
+def test_manual_offset_reaches_the_path_and_matches_what_the_ui_reports(params, offset):
+  params.put_bool("IQLiveSteerDelay", False)
+  params.put("IQSoftwareSteerDelay", offset)
+  ui_total = RACK_DELAY + offset
+  assert fixed_steer_delay(params, RACK_DELAY) == pytest.approx(ui_total)
+  assert lateral_action_delay(params, _car_params(ANGLE), LIVE_DELAY) == pytest.approx(ui_total)
+
+
+@pytest.mark.parametrize("live_enabled", [False, True])
+def test_publisher_writes_the_value_the_resolver_reads(params, live_enabled):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  params.put("IQSteerDelayCache", -1.0)
+  SteerDelayPublisher(_car_params(ANGLE)).update(_lateral_delay_msg(LIVE_DELAY))
+
+  expected = LIVE_DELAY if live_enabled else RACK_DELAY + OFFSET
+  deadline = time.monotonic() + 5.0
+  while cached_steer_delay() != pytest.approx(expected) and time.monotonic() < deadline:
+    time.sleep(0.01)
+  assert cached_steer_delay() == pytest.approx(expected)
+  assert resolve_steer_delay(params, RACK_DELAY) == pytest.approx(expected)