IQ.Pilot Release Commit @ 67fd9c2

iqpilot/selfdrive/controls/lib/latcontrol_torque.py

@@ -186,6 +186,8 @@ class NNTorqueModel:
 
 PLAN_SAMPLE_START = 5
 LAG_EXTRA_S = 0.0
+JERK_AHEAD_TAU_S = 0.15  # low-pass on the model-derived jerk feed-forward (kills big-model accel.y noise)
+JERK_PARAM_REFRESH = 100  # cycles (~1s at 100Hz)
 
 BASE_P = 0.8
 BASE_I = 0.15
@@ -264,6 +266,15 @@ class PilotLateralBrain:
     self.friction_look_ahead_bp = [9.0, 30.0]
     self.lat_jerk_friction_factor = 0.4
     self.lat_accel_friction_factor = 0.7
+    # The model-derived jerk term is a frame-to-frame derivative of model_v2.acceleration.y; on
+    # spatial/big models that array is slightly noisy and the raw derivative drives in-lane steering
+    # oscillation (sunny/stock has no such term). Low-pass it, and expose a live gain so it can be
+    # tuned to 0 (== stock friction ff) without a software push.
+    self._jerk_lp = FirstOrderFilter(0.0, JERK_AHEAD_TAU_S, 0.01)
+    self._jerk_gain = 1.0
+    self._jerk_param_frame = 0
+    self._jerk_param_ok = True
+    self._params = Params()
 
     self.t_diffs = np.diff(ModelConstants.T_IDXS)
     self.desired_lat_jerk_time = cp.steerActuatorDelay + LAG_EXTRA_S
@@ -294,6 +305,17 @@ class PilotLateralBrain:
     self.jerk_ahead = 0.0
 
   def update_calculations(self, car_state, vehicle_model, desired_lat_accel):
+    self._jerk_param_frame += 1
+    if self._jerk_param_ok and self._jerk_param_frame % JERK_PARAM_REFRESH == 0:
+      try:
+        raw = self._params.get("IQLatJerkGain")
+        self._jerk_gain = float(raw) if raw not in (None, b"", "") else 1.0
+      except (ValueError, TypeError):
+        self._jerk_gain = 1.0
+      except Exception:
+        # param key absent (params not rebuilt) — never let a param read touch lateral control
+        self._jerk_param_ok = False
+        self._jerk_gain = 1.0
     self._reset_jerk_estimates(car_state, vehicle_model)
     if not self.model_valid:
       return
@@ -305,6 +327,7 @@ class PilotLateralBrain:
     forecast = _pointwise_jerk(accel_y, self.t_diffs)
     window = forecast[PLAN_SAMPLE_START:self._horizon_index(car_state.vEgo)]
     self.jerk_ahead = sign_locked_min(window, desired_jerk)
+    self.jerk_ahead = self._jerk_lp.update(self.jerk_ahead) * self._jerk_gain
 
     if self.jerk_ahead == 0.0:
       self.jerk_now = 0.0

iqpilot/selfdrive/controls/tests/test_lat_jerk_lowpass.py

@@ -0,0 +1,81 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
+"""
+import numpy as np
+
+from iqpilot.cereal import car, log
+from iqdbc.car.car_helpers import interfaces
+from iqdbc.car.toyota.values import CAR as TOYOTA
+from iqdbc.car.vehicle_model import VehicleModel
+from iqpilot.common.realtime import DT_CTRL
+from iqpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
+from iqpilot.selfdrive.car.helpers import convert_to_capnp
+from iqpilot.selfdrive.car import interfaces as iqpilot_interfaces
+from iqpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N
+
+CAR_NAME = TOYOTA.TOYOTA_COROLLA_TSS2
+
+
+def _brain():
+  CI_cls = interfaces[CAR_NAME]
+  CP = CI_cls.get_non_essential_params(CAR_NAME)
+  CP_IQ = CI_cls.get_non_essential_params_iq(CP, CAR_NAME)
+  CI = CI_cls(CP, CP_IQ)
+  iqpilot_interfaces.apply_iq_car_config(CI)
+  ctrl = LatControlTorque(CP.as_reader(), convert_to_capnp(CP_IQ).as_reader(), CI, DT_CTRL)
+  return ctrl.nnff_assist, VehicleModel(CP)
+
+
+def _model(rng):
+  # same-sign accel ramp (so sign_locked_min yields a real jerk) whose slope jitters frame to
+  # frame the way a spatial big model's path does — this is what drives jerk_ahead to swing.
+  n = max(CONTROL_N, 33)
+  slope = abs(0.5 + rng.normal(0, 0.25))
+  m = log.ModelDataV2.new_message()
+  m.acceleration.y = (slope * np.arange(n) * 0.1).tolist()
+  m.orientation.x = [0.0] * n
+  return m
+
+
+def _cs():
+  cs = car.CarState.new_message()
+  cs.vEgo = 25.0
+  cs.steeringRateDeg = 0.0
+  return cs
+
+
+def _run(lp_on):
+  brain, VM = _brain()
+  rng = np.random.default_rng(7)
+  cs = _cs()
+  out = []
+  for _ in range(400):
+    brain.update_model_v2(_model(rng))
+    if not lp_on:
+      brain._jerk_lp.update = lambda x: x  # bypass low-pass == pre-fix behavior
+    brain.update_calculations(cs, VM, 0.0)
+    out.append(brain.jerk_ahead)
+  return np.array(out)
+
+
+def test_lowpass_cuts_jerk_command_swing():
+  old = _run(lp_on=False)
+  new = _run(lp_on=True)
+  # the path must actually exercise the jerk feed-forward (guard against a vacuous test)
+  assert np.abs(np.diff(old)).mean() > 0.02, "input did not exercise jerk_ahead"
+  old_swing = np.abs(np.diff(old)).mean()
+  new_swing = np.abs(np.diff(new)).mean()
+  # low-pass must cut the frame-to-frame jerk swing (the wheel oscillation) by a large margin
+  assert new_swing < 0.3 * old_swing, (old_swing, new_swing)
+
+
+def test_gain_zero_matches_stock():
+  brain, VM = _brain()
+  brain._jerk_param_ok = False
+  brain._jerk_gain = 0.0
+  rng = np.random.default_rng(1)
+  cs = _cs()
+  for _ in range(60):
+    brain.update_model_v2(_model(rng))
+    brain.update_calculations(cs, VM, 0.0)
+  assert brain.jerk_ahead == 0.0  # no model-jerk term == sunny/stock feedforward

iqpilot/selfdrive/iqmodeld/modeld_selector.py

@@ -357,6 +357,7 @@ def main() -> None:
         }))
       if run_count % 100 == 0:
         cloudlog.warning(f"modeld_selector misses: {miss_reasons}")
+        pwriter.put_bool(keys["active"], latch.active)
         cloudlog.warning(f"modeld_selector: big_used={big_used_count}/{run_count} "
                          f"last_big_peek={big_peek} target={target} active={latch.active} "
                          f"max_big_lag={BIG_MAX_LAG_FRAMES}")

iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py

@@ -13,7 +13,9 @@ import time
 os.environ.setdefault("FLOAT16", "1")
 os.environ.setdefault("JIT_BATCH_SIZE", "0")
 os.environ.setdefault("GMMU", "0")
-os.environ.setdefault("TC_OPT", "2")
+# TC_OPT=2 lets tinygrad pick tensor-core kernels; on some models a TC kernel miscompiles and biases
+# the output (documented on Metal). A parity gate below catches it and re-compiles with TC off.
+os.environ.setdefault("TC_OPT", "0" if ("--tc-off" in sys.argv or os.environ.get("IQ_EGPU_TC_OFF")) else "2")
 
 HOST = "--host" in sys.argv
 if HOST:
@@ -32,6 +34,10 @@ INPUT_SPEC = dict(MODEL_INPUT_SPEC)
 patch_tinygrad_fetch_fw()
 
 SEED = 42
+
+
+class _ParityFail(RuntimeError):
+  pass
 KERNEL_PROGRESS_SCALE = 260.0
 
 
@@ -151,6 +157,25 @@ def _policy_frame(seed: int, input_spec: dict):
   return warped
 
 
+def _tc_off_reference(onnx_path: str, meta: dict):
+  """Compile+run the model with tensor cores OFF in a child process and return the last of 3
+  policy frames. This is the trusted reference: TC-off kernels are the conservative path the
+  eMac gate also trusts. Used to catch a TC kernel miscompile that would bias steering."""
+  import subprocess
+  import tempfile
+  with tempfile.TemporaryDirectory() as td:
+    ref = os.path.join(td, "ref.npy")
+    env = {k: v for k, v in os.environ.items() if k not in ("TC_OPT", "BEAM")}
+    env["TC_OPT"] = "0"
+    env["IQ_EGPU_REFERENCE"] = ref
+    r = subprocess.run([sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model",
+                        "--model", meta["key"], "--onnx", onnx_path, "--tc-off"],
+                       env=env, capture_output=True, text=True, timeout=14400)
+    if r.returncode != 0 or not os.path.isfile(ref):
+      raise RuntimeError(f"parity reference compile failed:\n{r.stderr[-2000:]}")
+    return np.load(ref)
+
+
 def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str:
   from tinygrad.device import Device
   from tinygrad.engine.jit import TinyJit
@@ -216,6 +241,10 @@ def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str:
     flat = out.numpy().reshape(-1)
     packed.views["prev_feat"][:] = flat[meta["output_slices"]["hidden_state"]].reshape(packed.views["prev_feat"].shape)
     outs.append(flat)
+  ref_target = os.environ.get("IQ_EGPU_REFERENCE")
+  if ref_target:
+    np.save(ref_target, outs[-1])
+    return out_path
   if HOST:
     os.replace(tmp, out_path)
     return out_path
@@ -229,6 +258,13 @@ def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str:
   from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import _slice_outputs, _validate_pose_outputs
   _validate_pose_outputs(PhaseParser().parse_vision_outputs(_slice_outputs(outs[-1], meta["output_slices"])))
 
+  if os.environ.get("TC_OPT") != "0" and not os.environ.get("IQ_EGPU_SKIP_PARITY"):
+    ref = _tc_off_reference(onnx_path, meta)
+    rel = float(np.abs(outs[-1] - ref).mean() / max(1e-3, float(np.abs(ref).mean())))
+    if rel > 0.01:
+      raise _ParityFail(f"PARITY FAIL: TC kernels miscompiled {meta['key']} (rel={rel:.4f} vs TC-off); recompiling with tensor cores disabled")
+    print(f"  parity vs TC-off reference: rel={rel:.6f} OK")
+
   os.replace(tmp, out_path)
   return out_path
 
@@ -244,6 +280,7 @@ def main() -> None:
   p.add_argument("--format", type=int, default=2, choices=(1, 2))
   p.add_argument("--host", action="store_true", help="compile on a mock dock (no AMD hardware); outputs need a dock parity gate")
   p.add_argument("--arch", default=None, help="target gfx arch for --host")
+  p.add_argument("--tc-off", action="store_true", help="disable tensor-core kernels (conservative; auto-set on parity failure)")
   args = p.parse_args()
   if args.host and args.format != 2:
     raise SystemExit("--host supports format 2 only")
@@ -275,7 +312,15 @@ def main() -> None:
     sampler.start()
   try:
     build = compile_policy_model if args.format == 2 else compile_model
-    out = build(meta, onnx_path, args.output or egpu_pkl_path(meta))
+    try:
+      out = build(meta, onnx_path, args.output or egpu_pkl_path(meta))
+    except _ParityFail as e:
+      if os.environ.get("TC_OPT") == "0" or args.format != 2:
+        raise
+      print(f"{e}\nretrying compile with tensor cores disabled", flush=True)
+      os.environ["TC_OPT"] = "0"
+      os.environ["IQ_EGPU_TC_OFF"] = "1"
+      out = compile_policy_model(meta, onnx_path, args.output or egpu_pkl_path(meta))
   finally:
     if stop is not None:
       stop.set()

iqpilot/selfdrive/selfdrived/selfdrived.py

@@ -274,7 +274,10 @@ class SelfdriveD(GapButtonActions):
       dock_present = self.sm['deviceState'].egpuDockPresent
       mac_active = self.params.get_bool("MacModelActive")
       model_unavailable = big_active is True and self.sm.seen['modelV2'] and not self.sm.alive['modelV2']
-      big_failed = (big_active is False or model_unavailable
+      # an explicit False before this session's first activation is just the selector arming
+      # (it pre-clears the param on startup); alerting on it pops "big model failed" on every
+      # return to the road until the latch warms up
+      big_failed = ((self.big_model_active and big_active is False) or model_unavailable
                     or (self.big_model_active and not dock_present)) and not mac_active
       if big_failed:
         self.events.add(EventName.bigModelFailed)

iqpilot/selfdrive/ui/layouts/main.py

@@ -152,7 +152,7 @@ class MainLayout(Widget):
     self._layouts[MainState.ROUTES].set_on_play(self.open_video)
     self._layouts[MainState.VIDEO].set_on_back(self.open_routes)
     self._layouts[MainState.ONROAD].set_click_callback(self._on_onroad_clicked)
-    device.add_interactive_timeout_callback(self._set_mode_for_state)
+    device.add_interactive_timeout_callback(self._on_interactive_timeout)
 
   def _update_layout_rects(self):
     self._sidebar_rect = rl.Rectangle(self._rect.x, self._rect.y, SIDEBAR_WIDTH, self._rect.height)
@@ -166,6 +166,20 @@ class MainLayout(Widget):
 
       self._set_mode_for_state()
 
+  def _car_stationary(self) -> bool:
+    if not ui_state.sm.valid["carState"]:
+      return False
+    return ui_state.sm["carState"].vEgo < 0.1
+
+  def _on_interactive_timeout(self):
+    # The idle timeout normally returns the UI to the road view. Don't yank the user out of Settings
+    # while the car is stationary - e.g. a hybrid parked with the engine running to charge reads as
+    # onroad (ignition tracks the ICE), so this would otherwise make Settings unusable while parked.
+    # A moving car still returns to the road view.
+    if self._current_mode == MainState.SETTINGS and ui_state.started and self._car_stationary():
+      return
+    self._set_mode_for_state()
+
   def _set_mode_for_state(self):
     if ui_state.started:
       # Don't hide sidebar from interactive timeout

iqpilot/selfdrive/ui/tests/test_settings_interactive_timeout.py

@@ -0,0 +1,56 @@
+from types import SimpleNamespace
+
+import pytest
+
+from iqpilot.selfdrive.ui.layouts.main import MainLayout, MainState
+
+
+def make_layout(current_mode):
+  layout = object.__new__(MainLayout)
+  layout._current_mode = current_mode
+  layout._set_mode_calls = []
+  layout._set_mode_for_state = lambda: layout._set_mode_calls.append(current_mode)
+  return layout
+
+
+class FakeSm:
+  def __init__(self, v_ego, carstate_valid):
+    self.valid = {"carState": carstate_valid}
+    self._v_ego = v_ego
+
+  def __getitem__(self, key):
+    return SimpleNamespace(vEgo=self._v_ego)
+
+
+class TestSettingsInteractiveTimeout:
+  def _run(self, current_mode, started, v_ego, carstate_valid=True):
+    layout = make_layout(current_mode)
+    fake = SimpleNamespace(started=started, sm=FakeSm(v_ego, carstate_valid))
+    monkeypatch = pytest.MonkeyPatch()
+    monkeypatch.setattr("iqpilot.selfdrive.ui.layouts.main.ui_state", fake)
+    try:
+      layout._on_interactive_timeout()
+    finally:
+      monkeypatch.undo()
+    return layout._set_mode_calls
+
+  def test_stationary_in_settings_stays(self):
+    # parked/charging hybrid reads onroad; the timeout must not eject from Settings
+    assert self._run(MainState.SETTINGS, started=True, v_ego=0.0) == []
+
+  def test_moving_in_settings_returns_to_road(self):
+    assert self._run(MainState.SETTINGS, started=True, v_ego=5.0) == [MainState.SETTINGS]
+
+  def test_onroad_layout_always_handled(self):
+    assert self._run(MainState.ONROAD, started=True, v_ego=0.0) == [MainState.ONROAD]
+
+  def test_home_layout_always_handled(self):
+    assert self._run(MainState.HOME, started=True, v_ego=0.0) == [MainState.HOME]
+
+  def test_offroad_in_settings_handled(self):
+    # car off (offroad): existing behavior is unchanged
+    assert self._run(MainState.SETTINGS, started=False, v_ego=0.0) == [MainState.SETTINGS]
+
+  def test_invalid_carstate_treated_as_moving(self):
+    # if speed is unknown, fail safe to the road view rather than trapping in settings
+    assert self._run(MainState.SETTINGS, started=True, v_ego=0.0, carstate_valid=False) == [MainState.SETTINGS]