IQ.Pilot Release Commit @ f2a861c

iqpilot/common/steer_delay.py

@@ -4,25 +4,45 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
 Chooses which steer-actuator delay the lateral controllers run with: the value the
 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 liveDelay can still read the current value.
+not subscribe to lateralDelay can still read the current value.
 """
-from openpilot.common.params import Params
+from iqpilot.cereal import car
+from iqpilot.common.params import Params
 
 _ENABLE_KEY = "IQLiveSteerDelay"
 _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
-  liveDelay subscription (e.g. at process startup)."""
+  lateralDelay subscription (e.g. at process startup)."""
   return Params().get(_CACHE_KEY, return_default=True)
 
 
@@ -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.liveDelay.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)