IQ.Pilot Release Commit @ 4521b0f

iqpilot/selfdrive/controls/controlsd.py

@@ -13,7 +13,7 @@ from iqpilot.common.swaglog import cloudlog
 
 from iqdbc.car.car_helpers import interfaces
 from iqdbc.car.vehicle_model import VehicleModel
-from iqpilot.common.steer_delay import resolve_steer_delay
+from iqpilot.common.steer_delay import lateral_action_delay
 from iqpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
 from iqpilot.selfdrive.controls.lib.curvature_lookahead import get_lookahead_curvature
 from iqpilot.selfdrive.controls.lib.latcontrol import LatControl
@@ -227,15 +227,7 @@ class Controls(IQControlsLayer):
 
     lat_accel_override = bool(CS.gasPressed) or bool(self.sm['iqState'].aol.active)
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll, lat_accel_override)
-    # ALC (angle control) only: honour IQLiveSteerDelay so that with live learning off, lagd's
-    # estimate never reaches the controls loop and CP.steerActuatorDelay is used instead. lagd
-    # cross-correlates against localizer lateral accel, so it reports whole-vehicle response
-    # (~0.36 s measured on VW MQB) where the lookahead wants actuator delay (~0.10 s).
-    # Torque cars keep their existing path.
-    if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
-      lat_delay = resolve_steer_delay(self.params, self.CP.steerActuatorDelay) + LAT_SMOOTH_SECONDS
-    else:
-      lat_delay = self.sm["lateralDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lat_delay = lateral_action_delay(self.params, self.CP, self.sm["lateralDelay"].lateralDelay) + LAT_SMOOTH_SECONDS
     lookahead_curvature = None
     if not self.sm.valid['lateralManeuverPlan']:
       lookahead_curvature = get_lookahead_curvature(model_v2, CS.vEgo, lat_delay)

iqpilot/selfdrive/controls/iq_controls_layer.py

@@ -14,7 +14,6 @@ from iqdbc.car import structs
 from iqpilot.common.constants import CV
 from iqpilot.common.params import Params
 from iqpilot.common.swaglog import cloudlog
-from iqpilot.common.steer_delay import resolve_steer_delay
 from iqpilot.selfdrive.car.enhanced_stock_longitudinal_control import build_iq_control_params_from_plan
 from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
 from iqpilot.selfdrive.controls.lib.helpers.blinker_pause import IQSignalPauseController
@@ -60,7 +59,7 @@ class IQControlsLayer(InferenceStateBase):
       return
     self.blinker_pause_lateral.get_params()
     if self.CP.lateralTuning.which() == 'torque':
-      self.lat_delay = resolve_steer_delay(self.params, sm["lateralDelay"].lateralDelay)
+      self.lat_delay = sm["lateralDelay"].lateralDelay
     self._sync_set_speed = self._want_set_speed_to_limit()
     self.radar_manager.read_params()
     self._next_param_refresh = now

iqpilot/selfdrive/iqmodeld/daemon.py

@@ -32,7 +32,7 @@ from iqpilot.selfdrive.controls.lib.drive_helpers import (
 from iqpilot.selfdrive.locationd.calibration_helpers import get_calibrated_rpy
 from iqpilot.system import sentry
 
-from iqpilot.common.steer_delay import resolve_steer_delay
+from iqpilot.common.steer_delay import lateral_action_delay
 from iqpilot.selfdrive.iqmodeld.models.helpers import get_active_bundle
 from iqpilot.selfdrive.iqmodeld.models.inference_state import InferenceStateBase
 from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import get_model_runner
@@ -510,7 +510,7 @@ class InferenceDaemon:
   def _refresh_tunables(self, tick: int) -> None:
     if tick % 60 != 0:
       return
-    self._runtime.lat_delay = resolve_steer_delay(self._params, self._sub["lateralDelay"].lateralDelay)
+    self._runtime.lat_delay = lateral_action_delay(self._params, self._car_params, self._sub["lateralDelay"].lateralDelay)
     self._runtime.PLANPLUS_CONTROL = self._params.get("PlanplusControl", return_default=True)
     self._runtime.model_smoothing_max_extra_sec = _model_lat_smooth_max_sec(self._params)
     self._warps.set_offset(self._params.get("CameraOffset", return_default=True))

iqpilot/selfdrive/iqmodeld/models/runners/tinygrad/fused_runner.py

@@ -112,13 +112,26 @@ class TinygradFusedRunner(ModelRunner):
         }
         # shapes must match the captured run_policy JIT inputs
         on_shapes = self._on_meta['input_shapes']
+        captured = self._run_policy.captured
+        jit_shapes = {
+            name: tuple(int(s) for s in view.shape)
+            for name, (view, _vars, _dtype, _device) in zip(captured.expected_names, captured.expected_input_info)
+        }
+
+        def policy_input_shape(name):
+            shape = on_shapes.get(name, jit_shapes.get(name))
+            if shape is None:
+                raise ValueError(f"fused pkl declares no shape for policy input {name}")
+            return shape
+
         self._npy_buffers = {
             'desire':             np.zeros(dp[2], dtype=np.float32),
-            'traffic_convention': np.zeros(on_shapes['traffic_convention'], dtype=np.float32),
-            'action_t':           np.zeros(on_shapes['action_t'], dtype=np.float32),
+            'traffic_convention': np.zeros(policy_input_shape('traffic_convention'), dtype=np.float32),
             'tfm':                np.zeros((3, 3), dtype=np.float32),
             'big_tfm':            np.zeros((3, 3), dtype=np.float32),
         }
+        if 'action_t' in jit_shapes:
+            self._npy_buffers['action_t'] = np.zeros(policy_input_shape('action_t'), dtype=np.float32)
         self._cam_resolution = (cam_w, cam_h)
 
     def run_fused(self, bufs: dict, transforms: dict[str, np.ndarray], numpy_inputs: NumpyDict) -> NumpyDict:
@@ -134,7 +147,7 @@ class TinygradFusedRunner(ModelRunner):
             self._npy_buffers['desire'][:] = numpy_inputs[desire_key]
         if 'traffic_convention' in numpy_inputs:
             self._npy_buffers['traffic_convention'][:] = numpy_inputs['traffic_convention']
-        if 'action_t' in numpy_inputs:
+        if 'action_t' in numpy_inputs and 'action_t' in self._npy_buffers:
             self._npy_buffers['action_t'][:] = numpy_inputs['action_t']
         self._npy_buffers['tfm'][:] = transforms['img']
         self._npy_buffers['big_tfm'][:] = transforms['big_img']
@@ -149,9 +162,12 @@ class TinygradFusedRunner(ModelRunner):
         img, big_img = warp_jit(img_q=self._queues['img_q'], big_img_q=self._queues['big_img_q'],
                                 tfm=npy('tfm'), big_tfm=npy('big_tfm'), frame=frame, big_frame=big_frame)
 
-        vision_out_t, on_out_t, off_out_t = self._run_policy(
+        policy_inputs = dict(
             img=img, big_img=big_img, feat_q=self._queues['feat_q'], desire_q=self._queues['desire_q'],
-            desire=npy('desire'), traffic_convention=npy('traffic_convention'), action_t=npy('action_t'))
+            desire=npy('desire'), traffic_convention=npy('traffic_convention'))
+        if 'action_t' in self._npy_buffers:
+            policy_inputs['action_t'] = npy('action_t')
+        vision_out_t, on_out_t, off_out_t = self._run_policy(**policy_inputs)
 
         # parse each model's output on its own sliced dict; parsing a merged dict
         # would run parse_dynamic_outputs twice and double-parse plan/lead

iqpilot/selfdrive/iqmodeld/tests/test_fused_runner_guards.py

@@ -0,0 +1,128 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+from __future__ import annotations
+
+import pickle
+from dataclasses import dataclass
+
+import numpy as np
+import pytest
+
+from iqpilot.selfdrive.iqmodeld.models.runners import model_runner as model_runner_mod
+from iqpilot.selfdrive.iqmodeld.models.runners.model_runner import ModelType
+from iqpilot.selfdrive.iqmodeld.models.runners.tinygrad import fused_runner as fused_mod
+
+
+class _View:
+  def __init__(self, shape):
+    self.shape = shape
+
+
+class _Captured:
+  def __init__(self, expected_names, expected_input_info):
+    self.expected_names = expected_names
+    self.expected_input_info = expected_input_info
+
+
+class _FakeJit:
+  def __init__(self, expected_names, expected_input_info):
+    self.captured = _Captured(expected_names, expected_input_info)
+
+  def __call__(self, **kwargs):
+    raise AssertionError("policy jit should not run in this test")
+
+
+class _FakeTensor:
+  def __init__(self, arr, device=None):
+    self.shape = tuple(np.asarray(arr).shape)
+
+  def contiguous(self):
+    return self
+
+  def realize(self):
+    return self
+
+
+class _FakeDevice:
+  DEFAULT = "FAKE"
+
+
+@dataclass
+class _Type:
+  raw: int
+
+
+@dataclass
+class _Artifact:
+  fileName: str
+
+
+class _Model:
+  def __init__(self, file_name):
+    self.type = _Type(ModelType.vision)
+    self.artifact = _Artifact(file_name)
+    self.metadata = None
+
+
+class _Bundle:
+  def __init__(self, file_name):
+    self.models = [_Model(file_name)]
+    self.is20hz = True
+
+
+POLICY_INPUTS = ["action_t", "big_img", "desire", "desire_q", "feat_q", "img", "traffic_convention"]
+POLICY_SHAPES = {
+  "action_t": (1, 2), "big_img": (1, 12, 128, 256), "desire": (1, 8), "desire_q": (1, 100, 8),
+  "feat_q": (1, 99, 512), "img": (1, 12, 128, 256), "traffic_convention": (1, 2),
+}
+
+
+def _write_fused_pkl(path, policy_inputs):
+  info = [(_View(POLICY_SHAPES[n]), (), None, "NPY") for n in policy_inputs]
+  role_meta = {
+    "input_shapes": {"desire_pulse": (1, 100, 8), "traffic_convention": (1, 2), "features_buffer": (1, 99, 512)},
+    "output_slices": {},
+  }
+  blob = {
+    "metadata": {
+      "vision": {"input_shapes": {"img": (1, 12, 128, 256), "big_img": (1, 12, 128, 256)}, "output_slices": {}},
+      "on_policy": role_meta,
+      "off_policy": role_meta,
+    },
+    "run_policy": _FakeJit(policy_inputs, info),
+    "frame_skip": 4,
+    (1928, 1208): _FakeJit(["frame"], [(_View((1,)), (), None, "NPY")]),
+  }
+  with open(path, "wb") as f:
+    pickle.dump(blob, f)
+
+
+@pytest.fixture
+def fused_runner(tmp_path, monkeypatch):
+  def _build(policy_inputs):
+    name = "driving_fused_test.pkl"
+    _write_fused_pkl(tmp_path / name, policy_inputs)
+    monkeypatch.setattr(model_runner_mod, "_fetch_bundle", lambda params=None: _Bundle(name))
+    monkeypatch.setattr(fused_mod, "CUSTOM_MODEL_PATH", str(tmp_path))
+    monkeypatch.setattr(fused_mod, "_tinygrad_imports", lambda: (_FakeTensor, _FakeDevice))
+    return fused_mod.TinygradFusedRunner()
+  return _build
+
+
+def test_action_t_allocated_when_only_the_jit_declares_it(fused_runner):
+  runner = fused_runner(POLICY_INPUTS)
+  assert "action_t" not in runner._on_meta["input_shapes"]
+
+  runner._ensure_queues(1928, 1208)
+
+  assert runner._npy_buffers["action_t"].shape == POLICY_SHAPES["action_t"]
+  assert runner._npy_buffers["traffic_convention"].shape == POLICY_SHAPES["traffic_convention"]
+
+
+def test_action_t_absent_when_the_jit_does_not_take_it(fused_runner):
+  runner = fused_runner([n for n in POLICY_INPUTS if n != "action_t"])
+
+  runner._ensure_queues(1928, 1208)
+
+  assert "action_t" not in runner._npy_buffers

iqpilot/selfdrive/iqmodeld/tests/test_lat_delay_source.py

@@ -0,0 +1,72 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+"""
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.cereal import car
+from iqpilot.common.params import Params
+from iqpilot.selfdrive.iqmodeld.daemon import InferenceDaemon
+
+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)
+  p.put_bool("ModelSmoothingEnabled", False)
+  p.put("ModelLatSmoothSec", 0)
+  p.put("PlanplusControl", 1.0)
+  p.put("CameraOffset", 0.0)
+  return p
+
+
+def _daemon(params, steer_control_type):
+  car_params = car.CarParams.new_message()
+  car_params.steerControlType = steer_control_type
+  car_params.steerActuatorDelay = RACK_DELAY
+  return SimpleNamespace(
+    _params=params,
+    _car_params=car_params,
+    _sub={"lateralDelay": SimpleNamespace(lateralDelay=LIVE_DELAY)},
+    _runtime=SimpleNamespace(lat_delay=None, PLANPLUS_CONTROL=None, model_smoothing_max_extra_sec=None),
+    _warps=SimpleNamespace(set_offset=lambda _: None),
+  )
+
+
+@pytest.mark.parametrize("live_enabled, expected", [(False, RACK_DELAY + OFFSET), (True, LIVE_DELAY)])
+def test_angle_cars_honour_the_self_tuning_toggle(params, live_enabled, expected):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  daemon = _daemon(params, car.CarParams.SteerControlType.angle)
+  InferenceDaemon._refresh_tunables(daemon, 0)
+  assert daemon._runtime.lat_delay == pytest.approx(expected)
+
+
+def test_angle_cars_never_plan_against_the_live_estimate_when_disabled(params):
+  params.put_bool("IQLiveSteerDelay", False)
+  daemon = _daemon(params, car.CarParams.SteerControlType.angle)
+  InferenceDaemon._refresh_tunables(daemon, 0)
+  assert daemon._runtime.lat_delay != pytest.approx(LIVE_DELAY)
+
+
+@pytest.mark.parametrize("live_enabled", [True, False])
+def test_torque_cars_keep_the_live_estimate(params, live_enabled):
+  params.put_bool("IQLiveSteerDelay", live_enabled)
+  daemon = _daemon(params, car.CarParams.SteerControlType.torque)
+  InferenceDaemon._refresh_tunables(daemon, 0)
+  assert daemon._runtime.lat_delay == pytest.approx(LIVE_DELAY)
+
+
+def test_refresh_is_throttled_to_every_sixtieth_tick(params):
+  params.put_bool("IQLiveSteerDelay", False)
+  daemon = _daemon(params, car.CarParams.SteerControlType.angle)
+  InferenceDaemon._refresh_tunables(daemon, 1)
+  assert daemon._runtime.lat_delay is None
+  InferenceDaemon._refresh_tunables(daemon, 60)
+  assert daemon._runtime.lat_delay == pytest.approx(RACK_DELAY + OFFSET)

iqpilot/selfdrive/locationd/torqued.py

@@ -10,7 +10,6 @@ from iqpilot.common.realtime import DT_MDL
 from iqpilot.common.filter_simple import FirstOrderFilter
 from iqpilot.common.swaglog import cloudlog
 from iqpilot.selfdrive.locationd.helpers import PointBuckets, ParameterEstimator, PoseCalibrator, Pose
-from iqpilot.common.steer_delay import resolve_steer_delay
 
 HISTORY = 5  # secs
 POINTS_PER_BUCKET = 1500
@@ -97,7 +96,6 @@ class TorqueEstimator(ParameterEstimator):
 
     # try to restore cached params
     params = Params()
-    self.params = params
     params_cache = params.get("CarParamsPrevRoute")
     torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:
@@ -179,7 +177,7 @@ class TorqueEstimator(ParameterEstimator):
     elif which == "extrinsicsCalibration":
       self.calibrator.feed_live_calib(msg)
     elif which == "lateralDelay":
-      self.lag = resolve_steer_delay(self.params, msg.lateralDelay)
+      self.lag = msg.lateralDelay
     # calculate lateral accel from past steering torque
     elif which == "deviceMotion":
       if len(self.raw_points['steer_torque']) == self.hist_len: