IQ.Pilot Release Commit @ 7550fa9

iqpilot/selfdrive/dmonitoringmodeld/models/prebuilt_check.json

@@ -2,8 +2,8 @@
   "dmonitoring_model": {
     "outputs": {
       "dmonitoring_model_metadata.pkl": "31a86ab7a92dc0af088b15787a440dd3b210aa662e445a15145900e559a1b5c3",
-      "dmonitoring_model_tinygrad.pkl": "de991722fc93036a09595ac72f944f52afa15b097f182b3d5aa30a33d93d2d20"
+      "dmonitoring_model_tinygrad.pkl": "72757faf4828b7b574b9090c299fcad3e80d56ecfb4f5afa30d2355e556fb9c1"
     },
-    "signature": "696bc8453926631500feeb8f6d7baa9e8abeac06ee581e439b8edde299b24eb4"
+    "signature": "02f2a397f74dad0300afbbf9d4f68f253c9793c52d1464b56acad2a3c7a6d364"
   }
 }

iqpilot/selfdrive/iqlocd/atlas_loc_core.cc

@@ -48,6 +48,18 @@ static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE
   return res;
 }
 
+static bool finite_vector3(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
+  if (floatlist.size() != 3) {
+    return false;
+  }
+  for (int i = 0; i < 3; i++) {
+    if (!std::isfinite(floatlist[i])) {
+      return false;
+    }
+  }
+  return true;
+}
+
 static Vector4d quat2vector(const Quaterniond& quat) {
   return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
 }
@@ -257,6 +269,10 @@ void AtlasLocator::consume_sensor_frame(double current_time, const cereal::Senso
   // Gyro Uncalibrated
   if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
     auto v = log.getGyroUncalibrated().getV();
+    if (!finite_vector3(v)) {
+      this->observation_values_invalid["gyroscope"] += 1.0;
+      return;
+    }
     auto meas = Vector3d(-v[2], -v[1], -v[0]);
 
     VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
@@ -275,6 +291,10 @@ void AtlasLocator::consume_sensor_frame(double current_time, const cereal::Senso
   // Accelerometer
   if (log.getSensor() == SENSOR_ACCELEROMETER && log.getType() == SENSOR_TYPE_ACCELEROMETER) {
     auto v = log.getAcceleration().getV();
+    if (!finite_vector3(v)) {
+      this->observation_values_invalid["accelerometer"] += 1.0;
+      return;
+    }
 
     // TODO: reduce false positives and re-enable this check
     // check if device fell, estimate 10 for g
@@ -295,6 +315,10 @@ void AtlasLocator::seed_fake_gps_observations(double current_time) {
   // This is done to make sure that the error estimate of the position does not blow up
   // when the filter is in no-gps mode
   // Steps : first predict -> observe current obs with reasonable STD
+  double filter_time = this->kf->get_filter_time();
+  if (!std::isnan(filter_time) && current_time < filter_time) {
+    return;
+  }
   this->kf->predict(current_time);
 
   VectorXd current_x = this->kf->get_x();
@@ -308,12 +332,16 @@ void AtlasLocator::seed_fake_gps_observations(double current_time) {
 }
 
 void AtlasLocator::consume_gps_frame(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
+  bool gps_malformed = !finite_vector3(log.getVNED()) ||
+                       !std::isfinite(log.getLatitude()) || !std::isfinite(log.getLongitude()) || !std::isfinite(log.getAltitude()) ||
+                       !std::isfinite(log.getHorizontalAccuracy()) || !std::isfinite(log.getVerticalAccuracy()) ||
+                       !std::isfinite(log.getSpeedAccuracy()) || !std::isfinite(log.getBearingAccuracyDeg()) || !std::isfinite(log.getBearingDeg());
   bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
   bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
   bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
-  bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
+  bool gps_vel_insane = gps_malformed || (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
 
-  if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
+  if (!log.getHasFix() || gps_malformed || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
     //this->gps_valid = false;
     this->refresh_gps_mode(current_time);
     return;
@@ -450,6 +478,12 @@ void AtlasLocator::consume_car_state_frame(double current_time, const cereal::Ca
 }
 
 void AtlasLocator::consume_camera_odometry(double current_time, const cereal::CameraOdometry::Reader& log) {
+  if (!finite_vector3(log.getRot()) || !finite_vector3(log.getTrans()) ||
+      !finite_vector3(log.getRotStd()) || !finite_vector3(log.getTransStd())) {
+    this->observation_values_invalid["cameraOdometry"] += 1.0;
+    return;
+  }
+
   VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
   VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
 
@@ -499,6 +533,10 @@ void AtlasLocator::consume_live_calibration(double current_time, const cereal::E
   }
 
   if (log.getRpyCalib().size() > 0) {
+    if (!finite_vector3(log.getRpyCalib())) {
+      this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
+      return;
+    }
     auto live_calib = floatlist2vector(log.getRpyCalib());
     if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
       this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
@@ -520,7 +558,7 @@ void AtlasLocator::reset_kalman(double current_time) {
 }
 
 void AtlasLocator::run_finite_guard(double current_time) {
-  bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
+  bool all_finite = this->kf->get_x().array().isFinite().all() && this->kf->get_P().array().isFinite().all();
   if (!all_finite) {
     LOGE("Non-finite values detected, kalman reset");
     this->reset_kalman(current_time);
@@ -590,24 +628,29 @@ void AtlasLocator::consume_bytes(const char *data, const size_t size) {
 void AtlasLocator::consume_event(const cereal::Event::Reader& log) {
   double t = log.getLogMonoTime() * 1e-9;
   this->run_time_guard(t);
-  if (log.isAccelerometer()) {
-    this->consume_sensor_frame(t, log.getAccelerometer());
-  } else if (log.isGyroscope()) {
-    this->consume_sensor_frame(t, log.getGyroscope());
-  } else if (log.isGpsLocation()) {
-    this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
-  } else if (log.isGpsLocationExternal()) {
-    this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
-  //} else if (log.isGnssMeasurements()) {
-  //  this->consume_gnss_frame(t, log.getGnssMeasurements());
-  } else if (log.isCarState()) {
-    this->consume_car_state_frame(t, log.getCarState());
-  } else if (log.isCameraOdometry()) {
-    this->consume_camera_odometry(t, log.getCameraOdometry());
-  } else if (log.isExtrinsicsCalibration()) {
-    this->consume_live_calibration(t, log.getExtrinsicsCalibration());
+  try {
+    if (log.isAccelerometer()) {
+      this->consume_sensor_frame(t, log.getAccelerometer());
+    } else if (log.isGyroscope()) {
+      this->consume_sensor_frame(t, log.getGyroscope());
+    } else if (log.isGpsLocation()) {
+      this->consume_gps_frame(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
+    } else if (log.isGpsLocationExternal()) {
+      this->consume_gps_frame(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
+    //} else if (log.isGnssMeasurements()) {
+    //  this->consume_gnss_frame(t, log.getGnssMeasurements());
+    } else if (log.isCarState()) {
+      this->consume_car_state_frame(t, log.getCarState());
+    } else if (log.isCameraOdometry()) {
+      this->consume_camera_odometry(t, log.getCameraOdometry());
+    } else if (log.isExtrinsicsCalibration()) {
+      this->consume_live_calibration(t, log.getExtrinsicsCalibration());
+    }
+  } catch (const std::exception &e) {
+    LOGE("Estimator rejected an observation (%s), kalman reset", e.what());
+    this->reset_kalman(t);
   }
-  this->run_finite_guard();
+  this->run_finite_guard(t);
   this->cool_reset_tracker();
 }
 

iqpilot/selfdrive/iqlocd/tests/test_iqlocd.py

@@ -112,3 +112,61 @@ class TestIQLocdProc:
     assert lastGPS['latitude'] == pytest.approx(self.lat, abs=0.001)
     assert lastGPS['longitude'] == pytest.approx(self.lon, abs=0.001)
     assert lastGPS['altitude'] == pytest.approx(self.alt, abs=0.2)
+
+  def _well_formed_burst(self, t0, frames=60):
+    published = 0
+    for i in range(frames):
+      t = t0 + i * 50_000_000
+      for name in self.LLD_MSGS:
+        self.pm.send(name, self.get_msg(name, t))
+      self.pm.wait_for_readers_to_update("cameraOdometry", 0.1, dt=0.005)
+      self.sm.update(0)
+      published += int(self.sm.updated["iqLiveLocation"])
+    return t0 + frames * 50_000_000, published
+
+  def _malformed(self, case, t):
+    if case in ("odometry_short", "odometry_empty", "odometry_nan", "odometry_nan_std"):
+      msg = messaging.new_message("cameraOdometry")
+      odo = msg.cameraOdometry
+      if case == "odometry_short":
+        odo.rot = [0.0] * 6; odo.trans = [0.0] * 6; odo.rotStd = [0.01] * 6; odo.transStd = [0.01] * 6
+      elif case == "odometry_nan":
+        odo.rot = [float("nan"), 0.0, 0.0]; odo.trans = [0.0] * 3; odo.rotStd = [0.01] * 3; odo.transStd = [0.01] * 3
+      elif case == "odometry_nan_std":
+        odo.rot = [0.0] * 3; odo.trans = [0.0] * 3; odo.rotStd = [float("nan"), 0.01, 0.01]; odo.transStd = [0.01] * 3
+    elif case in ("calibration_short", "calibration_nan"):
+      msg = messaging.new_message("extrinsicsCalibration")
+      msg.extrinsicsCalibration.calStatus = "calibrated"
+      msg.extrinsicsCalibration.rpyCalib = [0.0, 0.0] if case == "calibration_short" else [float("nan"), 0.0, 0.0]
+    elif case in ("gps_short", "gps_nan"):
+      msg = self.get_msg("gpsLocationExternal", t)
+      msg.gpsLocationExternal.vNED = [0.0, 0.0] if case == "gps_short" else [float("nan"), 0.0, 0.0]
+    elif case == "gyro_nan":
+      msg = self.get_msg("gyroscope", t)
+      msg.gyroscope.gyroUncalibrated.v = [float("nan"), 0.0, 0.0]
+    elif case == "accel_short":
+      msg = self.get_msg("accelerometer", t)
+      msg.accelerometer.acceleration.v = [0.0, 0.0]
+    msg.logMonoTime = t
+    msg.valid = True
+    return msg
+
+  @pytest.mark.parametrize("case", (
+    "odometry_short", "odometry_empty", "odometry_nan", "odometry_nan_std",
+    "calibration_short", "calibration_nan", "gps_short", "gps_nan", "gyro_nan", "accel_short",
+  ))
+  def test_malformed_input_does_not_kill_the_process(self, case):
+    random.seed(1)
+    self.x, self.y, self.z = -2710700.0, -4280600.0, 3850300.0
+    self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
+
+    t, _ = self._well_formed_burst(int(1e9))
+    assert self.proc.poll() is None
+
+    msg = self._malformed(case, t)
+    self.pm.send(msg.which(), msg)
+    time.sleep(0.05)
+    _, published = self._well_formed_burst(t + 50_000_000)
+
+    assert self.proc.poll() is None, f"iqlocd died on {case} with {self.proc.returncode}"
+    assert published >= 30

iqpilot/selfdrive/iqmodeld/egpu_helpers.py

@@ -71,6 +71,11 @@ def egpu_oob_pkl_path(meta: dict) -> str:
   return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_policy_oob.pkl")
 
 
+def egpu_model_oob_pkl_path(meta: dict) -> str:
+  from iqpilot.system.hardware.hw import Paths
+  return os.path.join(Paths.model_root(), f"egpu_{meta['key']}_{meta['sha256'][:8]}_amd_model_oob.pkl")
+
+
 def onnx_cache_path(meta: dict) -> str:
   from iqpilot.system.hardware.hw import Paths
   return os.path.join(Paths.model_root(), f"{meta['model_name']}_{meta['sha256'][:8]}.onnx")
@@ -156,13 +161,21 @@ def download_onnx(meta: dict, progress_cb=None) -> str:
   return path
 
 
-def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False) -> str | None:
-  field = "egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact"
+ARTIFACT_PATHS = {
+  "egpu_model_oob_artifact": egpu_model_oob_pkl_path,
+  "egpu_oob_artifact": egpu_oob_pkl_path,
+  "egpu_policy_artifact": egpu_policy_pkl_path,
+  "egpu_artifact": egpu_pkl_path,
+}
+
+
+def download_precompiled(meta: dict, progress_cb=None, policy: bool = False, oob: bool = False, field: str | None = None) -> str | None:
+  field = field or ("egpu_oob_artifact" if oob else "egpu_policy_artifact" if policy else "egpu_artifact")
   art = meta.get(field)
   if not art or not (art.get("objects") or art.get("hf_path")):
     return None
   from iqpilot.selfdrive.iqmodeld.model_bundle_downloader import download_hf_file, download_lfs_bundle
-  dest = egpu_oob_pkl_path(meta) if oob else egpu_policy_pkl_path(meta) if policy else egpu_pkl_path(meta)
+  dest = ARTIFACT_PATHS[field](meta)
   if art.get("hf_path"):
     try:
       return download_hf_file(art["hf_path"], dest, art["sha256"], int(art.get("size", 0)), progress_cb=progress_cb)

iqpilot/selfdrive/iqmodeld/egpu_pipeline.py

@@ -5,7 +5,7 @@ from __future__ import annotations
 
 import numpy as np
 
-from iqpilot.selfdrive.iqmodeld.egpu_policy import PolicyRunner
+from iqpilot.selfdrive.iqmodeld.egpu_policy import ModelRunner, PolicyRunner
 from iqpilot.selfdrive.iqmodeld.temporal_state import MODEL_INPUT_SPEC, TemporalInputState, spec_from_meta
 
 
@@ -13,6 +13,10 @@ class EgpuPipelineError(RuntimeError):
   pass
 
 
+class EgpuOutputInvalid(EgpuPipelineError):
+  pass
+
+
 class EgpuPipeline:
 
   def __init__(self, meta: dict, infer_fn):
@@ -31,14 +35,26 @@ class EgpuPipeline:
     else:
       inputs = self.state.push_and_materialize(warped, desire_vec, traffic_convention, action_t)
       out = np.asarray(self.infer_fn(inputs), dtype=np.float32).reshape(-1)
-    if out.shape[0] != self.output_len:
-      raise EgpuPipelineError(f"eGPU output length {out.shape[0]} != {self.output_len}")
-    if not np.isfinite(out).all():
-      raise EgpuPipelineError("eGPU output contains non-finite values")
+    self._check(out)
     if not isinstance(self.infer_fn, PolicyRunner):
       self.state.note_hidden_state(out, self.hidden_slice)
     return out
 
+  def run_frames(self, main_frame, extra_frame, tfm: np.ndarray, big_tfm: np.ndarray, desire_vec: np.ndarray,
+                 traffic_convention: np.ndarray, action_t: np.ndarray) -> np.ndarray:
+    if not isinstance(self.infer_fn, ModelRunner):
+      raise EgpuPipelineError("run_frames needs a format-3 (warp-on-dock) artifact")
+    out = np.asarray(self.infer_fn.run(main_frame, extra_frame, tfm, big_tfm, desire_vec, traffic_convention, action_t),
+                     dtype=np.float32).reshape(-1)
+    self._check(out)
+    return out
+
+  def _check(self, out: np.ndarray) -> None:
+    if out.shape[0] != self.output_len:
+      raise EgpuPipelineError(f"eGPU output length {out.shape[0]} != {self.output_len}")
+    if not np.isfinite(out).all():
+      raise EgpuOutputInvalid("eGPU output contains non-finite values")
+
 
 def make_big_channel_payload(frame_id: int, live_calib_seen: bool, execution_time: float,
                              egpu_exec_ms: float, msgs: dict[str, bytes]) -> dict:

iqpilot/selfdrive/iqmodeld/egpu_policy.py

@@ -14,9 +14,11 @@ import tempfile
 import numpy as np
 
 POLICY_FORMAT = 2
+MODEL_FORMAT = 3
 OOB_MAGIC = b"IQEGPUOOB1"
 QUEUE_NAMES = ("img_q", "big_img_q", "feat_q", "desire_q")
 PACKED_ORDER = ("desire", "traffic_convention", "action_t", "prev_feat")
+MODELD_INPUTS = (*QUEUE_NAMES, "packed_npy_inputs")
 
 
 def packed_layout(input_spec: dict) -> tuple[dict[str, tuple[int, ...]], list[int]]:
@@ -56,7 +58,8 @@ class PackedInputs:
     from tinygrad.tensor import Tensor
     self.shapes, self.sizes = packed_layout(input_spec)
     self.array = np.zeros(sum(self.sizes), dtype=np.float32)
-    self.views = dict(zip(self.shapes, [v.reshape(s) for s, v in zip(self.shapes.values(), np.split(self.array, np.cumsum(self.sizes[:-1])))], strict=True))
+    parts = np.split(self.array, np.cumsum(self.sizes[:-1]))
+    self.views = {name: part.reshape(shape) for (name, shape), part in zip(self.shapes.items(), parts, strict=True)}
     self.tensor = Tensor(self.array, device="NPY").realize()
 
 
@@ -124,6 +127,173 @@ class PolicyRunner:
     return flat
 
 
+def nv12_copy_size(stride: int, y_height: int, uv_height: int) -> int:
+  return stride * (y_height + uv_height)
+
+
+def frame_layout(input_spec: dict) -> tuple[dict[str, tuple[int, ...]], list[int], int]:
+  policy_shapes, _ = packed_layout(input_spec)
+  shapes = {"tfm": (3, 3), "big_tfm": (3, 3)} | policy_shapes
+  sizes = [math.prod(s) for s in shapes.values()]
+  return shapes, sizes, sum(sizes) * np.dtype(np.float32).itemsize
+
+
+def model_size(input_spec: dict) -> tuple[int, int]:
+  img = input_spec["img"][0]
+  return img[3] * 2, img[2] * 2
+
+
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
+  from tinygrad.tensor import Tensor
+  w_dst, h_dst = dst_shape
+  h_src, w_src = src_shape
+
+  x = Tensor.arange(w_dst).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
+
+  src_x = M_inv[0, 0] * x + M_inv[0, 1] * y + M_inv[0, 2]
+  src_y = M_inv[1, 0] * x + M_inv[1, 1] * y + M_inv[1, 2]
+  src_w = M_inv[2, 0] * x + M_inv[2, 1] * y + M_inv[2, 2]
+
+  src_x = src_x / src_w
+  src_y = src_y / src_w
+
+  x_round = Tensor.round(src_x)
+  y_round = Tensor.round(src_y)
+  x_nn_clipped = x_round.clip(0, w_src - 1).cast("int")
+  y_nn_clipped = y_round.clip(0, h_src - 1).cast("int")
+  idx = y_nn_clipped * (w_src + stride_pad) + x_nn_clipped
+  sampled = src_flat[idx]
+
+  if border_fill_val is None:
+    return sampled
+
+  in_bounds = ((x_round >= 0) & (x_round <= w_src - 1) &
+               (y_round >= 0) & (y_round <= h_src - 1)).cast(sampled.dtype)
+  return sampled * in_bounds + Tensor(border_fill_val, dtype=sampled.dtype) * (1 - in_bounds)
+
+
+def frames_to_tensor(frames):
+  from tinygrad.tensor import Tensor
+  H = (frames.shape[0] * 2) // 3
+  W = frames.shape[1]
+  in_img1 = Tensor.cat(frames[0:H:2, 0::2],
+                       frames[1:H:2, 0::2],
+                       frames[0:H:2, 1::2],
+                       frames[1:H:2, 1::2],
+                       frames[H:H + H // 4].reshape((H // 2, W // 2)),
+                       frames[H + H // 4:H + H // 2].reshape((H // 2, W // 2)), dim=0).reshape((6, H // 2, W // 2))
+  return in_img1
+
+
+def make_frame_prepare(nv12: tuple[int, int, int, int, int], model_w: int, model_h: int, device: str):
+  from tinygrad.helpers import Context
+  from tinygrad.tensor import Tensor
+  cam_w, cam_h, stride, y_height, uv_height = nv12
+  uv_offset = stride * y_height
+  stride_pad = stride - cam_w
+
+  def frame_prepare_tinygrad(input_frame, M_inv):
+    M_inv_uv = M_inv * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=device)
+    uv = input_frame[uv_offset:uv_offset + uv_height * stride].reshape(uv_height, stride)
+    with Context(SPLIT_REDUCEOP=0):
+      y = warp_perspective_tinygrad(input_frame[:cam_h * stride],
+                                    M_inv, (model_w, model_h),
+                                    (cam_h, cam_w), stride_pad).realize()
+      u = warp_perspective_tinygrad(uv[:cam_h // 2, :cam_w:2].flatten(),
+                                    M_inv_uv, (model_w // 2, model_h // 2),
+                                    (cam_h // 2, cam_w // 2), 0).realize()
+      v = warp_perspective_tinygrad(uv[:cam_h // 2, 1:cam_w:2].flatten(),
+                                    M_inv_uv, (model_w // 2, model_h // 2),
+                                    (cam_h // 2, cam_w // 2), 0).realize()
+    yuv = y.cat(u).cat(v).reshape((model_h * 3 // 2, model_w))
+    return frames_to_tensor(yuv)
+  return frame_prepare_tinygrad
+
+
+def make_warp(nv12: tuple[int, int, int, int, int], model_w: int, model_h: int, device: str):
+  from tinygrad.tensor import Tensor
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h, device)
+
+  def warp(tfm, big_tfm, frame, big_frame):
+    tfm = tfm.to(device)
+    big_tfm = big_tfm.to(device)
+    frame = frame.to(device)
+    big_frame = big_frame.to(device)
+    Tensor.realize(tfm, big_tfm, frame, big_frame)
+
+    warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
+    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
+    return Tensor.cat(warped_frame, warped_big_frame)
+
+  return warp
+
+
+def make_run_model(warp, run_policy, input_spec: dict, frame_copy_size: int, device: str):
+  from tinygrad.tensor import Tensor
+  _, policy_sizes = packed_layout(input_spec)
+  _, _, packed_npy_size = frame_layout(input_spec)
+
+  def run_model(img_q, big_img_q, feat_q, desire_q, packed_npy_inputs):
+    packed_input = packed_npy_inputs.to(device)
+    Tensor.realize(packed_input)
+    packed_npy_inputs = packed_input[:packed_npy_size].bitcast("float32")
+    frame = packed_input[packed_npy_size:packed_npy_size + frame_copy_size]
+    big_frame = packed_input[packed_npy_size + frame_copy_size:]
+    tfm, big_tfm, policy_inputs = packed_npy_inputs.split([9, 9, sum(policy_sizes)])
+    warped = warp(tfm.reshape(3, 3), big_tfm.reshape(3, 3), frame, big_frame)
+    return run_policy(warped, img_q, big_img_q, feat_q, desire_q, policy_inputs)
+
+  return run_model
+
+
+class PackedFrames:
+  def __init__(self, input_spec: dict, frame_copy_size: int):
+    from tinygrad.tensor import Tensor
+    self.shapes, self.sizes, npy_bytes = frame_layout(input_spec)
+    self.frame_copy_size = frame_copy_size
+    self.array = np.zeros(npy_bytes + 2 * frame_copy_size, dtype=np.uint8)
+    npy = self.array[:npy_bytes].view(np.float32)
+    self.views = dict(zip(self.shapes, [v.reshape(s) for s, v in zip(self.shapes.values(), np.split(npy, np.cumsum(self.sizes[:-1])), strict=True)],
+                          strict=True))
+    frames = self.array[npy_bytes:]
+    self.frames = {"img": frames[:frame_copy_size], "big_img": frames[frame_copy_size:]}
+    self.tensor = Tensor(self.array, device="NPY").realize()
+
+
+def make_model_queues(input_spec: dict, frame_skip: int, device: str, frame_copy_size: int) -> tuple[dict, PackedFrames]:
+  packed = PackedFrames(input_spec, frame_copy_size)
+  return {**make_queues(input_spec, frame_skip, device), "packed_npy_inputs": packed.tensor}, packed
+
+
+class ModelRunner:
+  def __init__(self, jit, input_spec: dict, frame_skip: int, hidden_slice: slice, device: str, frame_copy_size: int):
+    self._jit = jit
+    self._queues, self._packed = make_model_queues(input_spec, frame_skip, device, frame_copy_size)
+    self._hidden = hidden_slice
+    self._prev_desire = np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32)
+    self.frame_copy_size = frame_copy_size
+
+  def run(self, main_frame, extra_frame, tfm: np.ndarray, big_tfm: np.ndarray, desire_pulse: np.ndarray,
+          traffic_convention: np.ndarray, action_t: np.ndarray) -> np.ndarray:
+    n = self.frame_copy_size
+    v = self._packed.views
+    f = self._packed.frames
+    np.copyto(f["img"], np.frombuffer(main_frame, dtype=np.uint8, count=n))
+    np.copyto(f["big_img"], np.frombuffer(extra_frame, dtype=np.uint8, count=n))
+    v["tfm"][:, :] = tfm
+    v["big_tfm"][:, :] = big_tfm
+    cur = desire_pulse.astype(np.float32, copy=False)
+    v["desire"][:] = np.where(cur - self._prev_desire > 0.99, cur, 0)
+    self._prev_desire[:] = cur
+    v["traffic_convention"][:] = np.asarray(traffic_convention, dtype=np.float32).reshape(v["traffic_convention"].shape)
+    v["action_t"][:] = np.asarray(action_t, dtype=np.float32).reshape(v["action_t"].shape)
+    out, = self._jit(**self._queues)
+    flat = out.numpy().reshape(-1)
+    v["prev_feat"][:] = flat[self._hidden].reshape(v["prev_feat"].shape)
+    return flat
+
+
 def dump_oob(obj, f) -> None:
   # Out-of-band pickle buffers keep the host peak at one tensor while the weights stream to the
   # dock; a plain pickle keeps every weight referenced in the memo until load() returns (~1.7GB).

iqpilot/selfdrive/iqmodeld/iqegpumodeld.py

@@ -39,16 +39,17 @@ from iqpilot.selfdrive.iqmodeld.driving_action import (
   DESIRE_LEN, LAT_SMOOTH_SECONDS, LONG_SMOOTH_SECONDS, get_action_from_model,
 )
 from iqpilot.selfdrive.iqmodeld.egpu_helpers import (
-  download_onnx, download_precompiled, egpu_oob_pkl_path, egpu_pkl_path, egpu_policy_pkl_path, egpu_present_consented, egpu_selected, local_onnx,
+  download_onnx, download_precompiled, egpu_model_oob_pkl_path, egpu_oob_pkl_path, egpu_pkl_path, egpu_policy_pkl_path, egpu_present_consented,
+  egpu_selected, local_onnx,
   patch_tinygrad_fetch_fw, quarantine_artifact, resolve_backend, usbgpu_present,
 )
 from iqpilot.selfdrive.iqmodeld.egpu_model import resolve_egpu_model
-from iqpilot.selfdrive.iqmodeld.egpu_pipeline import EgpuPipeline, EgpuPipelineError, make_big_channel_payload
+from iqpilot.selfdrive.iqmodeld.egpu_pipeline import EgpuOutputInvalid, EgpuPipeline, EgpuPipelineError, make_big_channel_payload
 from iqpilot.selfdrive.iqmodeld.egpu_telemetry import EgpuDockTelemetry
 from iqpilot.selfdrive.iqmodeld.messaging import DrivePacketMemory, populate_drive_messages, populate_odometry_message
 from iqpilot.selfdrive.iqmodeld.metadata import Meta20hz
 from iqpilot.selfdrive.iqmodeld.model_channel import BIG_CHANNEL, ModelChannel
-from iqpilot.selfdrive.iqmodeld.egpu_policy import POLICY_FORMAT, PolicyRunner, load_bundle
+from iqpilot.selfdrive.iqmodeld.egpu_policy import MODEL_FORMAT, POLICY_FORMAT, ModelRunner, PolicyRunner, load_bundle
 from iqpilot.selfdrive.iqmodeld.model_warp import FrameWarp
 from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
 
@@ -62,6 +63,11 @@ MIN_LOAD_AVAIL_MB = 350
 MEMORY_WAIT_S = 90.0
 SETUP_RETRY_BASE_S = 3.0
 SETUP_RETRY_MAX_S = 30.0
+MAX_INVALID_STREAK = 20
+DOCK_MIN_SUPPLY_MV = 5000
+DOCK_POWER_STABLE_POLLS = 4
+DOCK_POWER_POLL_S = 0.1
+DOCK_POWER_TIMEOUT_S = 30.0
 
 
 def park(reason: str) -> None:
@@ -86,15 +92,53 @@ def _wait_for_egpu(params: Params) -> None:
   while time.monotonic() < deadline:
     try:
       if link_up():
-        return
+        break
     except Exception:
       return
     time.sleep(0.5)
+  _wait_for_dock_power(params)
 
 
-def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str) -> None:
+def dock_power_ready(reading) -> bool:
+  voltage, _current, fault = reading
+  return int(voltage) >= DOCK_MIN_SUPPLY_MV and not fault
+
+
+def _wait_for_dock_power(params: Params) -> None:
+  telemetry = EgpuDockTelemetry(None, big=False)
+  deadline = time.monotonic() + DOCK_POWER_TIMEOUT_S
+  stable = 0
+  warned = False
+  try:
+    while time.monotonic() < deadline:
+      try:
+        reading = telemetry._read_ina()
+      except Exception:
+        return
+      if reading is None:
+        return
+      stable = stable + 1 if dock_power_ready(reading) else 0
+      if stable >= DOCK_POWER_STABLE_POLLS:
+        return
+      if stable == 0 and not warned:
+        warned = True
+        cloudlog.warning(f"iqegpumodeld dock supply not ready {reading}; waiting for a stable 5V rail")
+        params.put("UsbGpuLastError", f"dock supply not ready (voltage={reading[0]}mV fault={reading[2]}); waiting")
+      time.sleep(DOCK_POWER_POLL_S)
+    cloudlog.warning("iqegpumodeld dock supply never stabilised; continuing")
+  finally:
+    handle = getattr(telemetry, "_asm_usb", None)
+    if handle is not None:
+      try:
+        handle.close()
+      except Exception:
+        pass
+
+
+def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str, cam_size: tuple[int, int]) -> None:
   cmd = [sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model",
          "--model", meta["key"], "--onnx", onnx_path, "--output", pkl_path,
+         "--format", str(MODEL_FORMAT), "--camera-resolutions", f"{cam_size[0]}x{cam_size[1]}",
          "--progress-param", "UsbGpuSetupProgress", "--progress-base", "0.5", "--progress-span", "0.48"]
   compile_env = {**os.environ, "DEV": "USB+AMD:LLVM", "FLOAT16": "1",
                  "JIT_BATCH_SIZE": "0", "GMMU": "0", "TC_OPT": "2"}
@@ -106,10 +150,36 @@ def _compile_in_subprocess(meta: dict, onnx_path: str, pkl_path: str) -> None:
 
 
 _precompiled_tried = False
+_model_precompiled_tried = False
 
 
-def _ensure_artifact(params: Params, meta: dict) -> str:
-  global _precompiled_tried
+def _ensure_artifact(params: Params, meta: dict, cam_size: tuple[int, int]) -> str:
+  global _precompiled_tried, _model_precompiled_tried
+  model_path = egpu_model_oob_pkl_path(meta)
+  if os.path.isfile(model_path):
+    return model_path
+  if meta.get("egpu_model_oob_artifact") and not _model_precompiled_tried:
+    _model_precompiled_tried = True
+    params.put_bool("UsbGpuCompiled", False)
+    params.put_bool("UsbGpuReady", False)
+    params.put("UsbGpuSetupProgress", "0.0")
+    model_last = [-1.0]
+
+    def _model_prog(p: float) -> None:
+      if p - model_last[0] >= 0.02 or p >= 1.0:
+        model_last[0] = p
+        params.put("UsbGpuSetupProgress", f"{p:.3f}")
+
+    try:
+      size_mb = int(meta["egpu_model_oob_artifact"].get("size", 0)) / 1e6
+      cloudlog.warning(f"iqegpumodeld downloading precompiled {meta['key']} (warp-on-dock, {size_mb:.0f}MB)")
+      precompiled = download_precompiled(meta, progress_cb=_model_prog, field="egpu_model_oob_artifact")
+      if precompiled is not None:
+        cloudlog.warning(f"iqegpumodeld precompiled ready -> {precompiled}")
+        return precompiled
+    except Exception as e:
+      cloudlog.warning(f"iqegpumodeld warp-on-dock artifact unavailable ({e}); falling back")
+
   oob_path = egpu_oob_pkl_path(meta)
   if os.path.isfile(oob_path):
     return oob_path
@@ -179,10 +249,10 @@ def _ensure_artifact(params: Params, meta: dict) -> str:
 
     onnx_path = download_onnx(meta, progress_cb=_prog)
 
-  cloudlog.warning(f"iqegpumodeld compiling {meta['key']} for USB-AMD (one-time, can take minutes)")
-  _compile_in_subprocess(meta, onnx_path, policy_path)
-  cloudlog.warning(f"iqegpumodeld compiled -> {policy_path}")
-  return policy_path
+  cloudlog.warning(f"iqegpumodeld compiling {meta['key']} for USB-AMD ({cam_size[0]}x{cam_size[1]}, one-time, can take minutes)")
+  _compile_in_subprocess(meta, onnx_path, model_path, cam_size)
+  cloudlog.warning(f"iqegpumodeld compiled -> {model_path}")
+  return model_path
 
 
 def _mem_available_mb() -> int:
@@ -207,7 +277,7 @@ def _wait_for_memory(need_mb: int) -> None:
     raise RuntimeError(f"insufficient memory to load the dock model: {avail}MB available, need {need_mb}MB")
 
 
-def _load_infer_fn(pkl_path: str, meta: dict):
+def _load_infer_fn(pkl_path: str, meta: dict, cam_size: tuple[int, int]):
   patch_tinygrad_fetch_fw()
   from tinygrad.tensor import Tensor
 
@@ -219,6 +289,14 @@ def _load_infer_fn(pkl_path: str, meta: dict):
   if int(bundle.get("output_len", -1)) != int(meta["output_len"]):
     quarantine_artifact(pkl_path, "pkl output_len mismatch")
     raise RuntimeError(f"artifact output_len {bundle.get('output_len')} != {meta['output_len']}")
+  if bundle.get("format") == MODEL_FORMAT:
+    jits = bundle["run_model"]
+    if cam_size not in jits:
+      have = ", ".join(f"{w}x{h}" for w, h in sorted(jits))
+      raise RuntimeError(f"artifact has no warp for the {cam_size[0]}x{cam_size[1]} camera (bundled: {have})")
+    runner = ModelRunner(jits[cam_size], bundle["input_spec"], int(bundle["frame_skip"]), meta["output_slices"]["hidden_state"],
+                         bundle.get("input_device", "AMD"), int(bundle["frame_copy_size"][cam_size]))
+    return runner, bundle["input_spec"]
   if bundle.get("format") == POLICY_FORMAT:
     runner = PolicyRunner(bundle["run_policy"], bundle["input_spec"], int(bundle["frame_skip"]),
                           meta["output_slices"]["hidden_state"], bundle.get("input_device", "AMD"))
@@ -239,7 +317,12 @@ def _load_infer_fn(pkl_path: str, meta: dict):
 def _warmup(infer_fn, input_spec: dict, output_len: int) -> float:
   zeros = {name: np.zeros(shape, dtype=dtype) for name, (shape, dtype) in input_spec.items()}
   t0 = time.perf_counter()
-  if isinstance(infer_fn, PolicyRunner):
+  if isinstance(infer_fn, ModelRunner):
+    n = infer_fn.frame_copy_size
+    eye = np.eye(3, dtype=np.float32)
+    out = infer_fn.run(np.zeros(n, dtype=np.uint8), np.zeros(n, dtype=np.uint8), eye, eye,
+                       np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32), np.zeros(2, dtype=np.float32), np.zeros(2, dtype=np.float32))
+  elif isinstance(infer_fn, PolicyRunner):
     img = input_spec["img"][0]
     out = infer_fn.run(np.zeros((2, 6, img[2], img[3]), dtype=np.uint8), np.zeros(input_spec["desire_pulse"][0][2], dtype=np.float32),
                        np.zeros(2, dtype=np.float32), np.zeros(2, dtype=np.float32))
@@ -284,9 +367,10 @@ def main(demo: bool = False) -> None:
       if meta.get("split"):
         params.put_bool("UsbGpuLoading", False)
         park(f"model {meta['key']} needs the Mac backend; the eGPU runs fused models only")
-      warp = FrameWarp(cameras._primary.width, cameras._primary.height, meta["frame_skip"])
-      pkl_path = _ensure_artifact(params, meta)
-      infer_fn, input_spec = _load_infer_fn(pkl_path, meta)
+      cam_size = (int(cameras._primary.width), int(cameras._primary.height))
+      pkl_path = _ensure_artifact(params, meta, cam_size)
+      infer_fn, input_spec = _load_infer_fn(pkl_path, meta, cam_size)
+      warp = None if isinstance(infer_fn, ModelRunner) else FrameWarp(cam_size[0], cam_size[1], meta["frame_skip"])
       warm_s = _warmup(infer_fn, input_spec, meta["output_len"])
       break
     except Exception as e:
@@ -312,7 +396,7 @@ def main(demo: bool = False) -> None:
   params.put_bool("UsbGpuReady", True)
   params.put("UsbGpuSetupProgress", "1.0")
   cloudlog.warning(f"iqegpumodeld model: {meta['key']} ({meta['model_name']})")
-  cloudlog.warning(f"iqegpumodeld model up (warmup {warm_s * 1e3:.0f}ms)")
+  cloudlog.warning(f"iqegpumodeld model up (warmup {warm_s * 1e3:.0f}ms, {'warp on dock' if warp is None else 'warp on device'})")
 
   pipeline = EgpuPipeline(meta, infer_fn)
   telemetry_pm = messaging.PubMaster(["egpuDockState"])
@@ -339,6 +423,7 @@ def main(demo: bool = False) -> None:
   stats: dict[str, list[float]] = {k: [] for k in ("pull", "warp", "infer", "publish", "loop")}
   iter_count = 0
   skip_count = 0
+  invalid_streak = 0
   last_pulled_fid = -1
   last_frame_mono = time.monotonic()
   t_loop = time.perf_counter()
@@ -394,20 +479,34 @@ def main(demo: bool = False) -> None:
     action_t = np.array([lat_action_t, long_action_t], dtype=np.float32)
 
     started_at = time.perf_counter()
+    t_warp = started_at
     try:
-      warped = warp.run(main_buf, extra_buf, main_tfm, extra_tfm)
-    except Exception as e:
-      park(f"warp run failed: {e}")
-    t_warp = time.perf_counter()
-    stats["warp"].append(t_warp - started_at)
-
-    try:
-      output = pipeline.run(warped, desire_vec, traffic, action_t)
+      if warp is None:
+        output = pipeline.run_frames(main_buf.data, extra_buf.data, main_tfm, extra_tfm, desire_vec, traffic, action_t)
+      else:
+        try:
+          warped = warp.run(main_buf, extra_buf, main_tfm, extra_tfm)
+        except Exception as e:
+          park(f"warp run failed: {e}")
+        t_warp = time.perf_counter()
+        output = pipeline.run(warped, desire_vec, traffic, action_t)
+    except EgpuOutputInvalid as e:
+      invalid_streak += 1
+      if invalid_streak == 1 or invalid_streak % MAX_INVALID_STREAK == 0:
+        cloudlog.warning(f"iqegpumodeld dropping frame {main_stamp.frame_id}: {e} (streak {invalid_streak})")
+      if invalid_streak >= MAX_INVALID_STREAK:
+        params.put("UsbGpuLastError", f"{e} for {invalid_streak} consecutive frames"[:512])
+        cloudlog.error(f"iqegpumodeld output invalid for {invalid_streak} frames; exiting for a clean restart")
+        sys.exit(1)
+      frame_meter.commit(main_stamp.frame_id)
+      continue
     except EgpuPipelineError as e:
       park(str(e))
     except Exception as e:
       park(f"eGPU inference failed: {e}")
+    invalid_streak = 0
     t_infer = time.perf_counter()
+    stats["warp"].append(t_warp - started_at)
     stats["infer"].append(t_infer - t_warp)
 
     execution_time = time.perf_counter() - started_at
@@ -431,6 +530,7 @@ def main(demo: bool = False) -> None:
     )
 
     model_msg.modelV2.big = True
+    driving_msg.drivingModelData.big = True
 
     desire_state = model_msg.modelV2.meta.desireState
     lane_change_prob = desire_state[log.Desire.laneChangeLeft] + desire_state[log.Desire.laneChangeRight]

iqpilot/selfdrive/iqmodeld/messaging.py

@@ -248,16 +248,19 @@ def populate_drive_messages(primary_msg: capnp._DynamicStructBuilder, extended_m
 def populate_odometry_message(msg: capnp._DynamicStructBuilder, outputs: dict[str, np.ndarray],
                               vipc_frame_id: int, vipc_dropped_frames: int,
                               timestamp_eof: int, live_calib_seen: bool) -> None:
-  msg.valid = live_calib_seen & (vipc_dropped_frames < 1)
+  pose = outputs["pose"][0, :6]
+  pose_stds = outputs["pose_stds"][0, :6]
+  pose_finite = bool(np.isfinite(pose).all() and np.isfinite(pose_stds).all())
+  msg.valid = live_calib_seen & (vipc_dropped_frames < 1) & pose_finite
   odo = msg.cameraOdometry
   odo.frameId = vipc_frame_id
   odo.timestampEof = timestamp_eof
-  odo.trans = outputs["pose"][0, :3].tolist()
-  odo.rot = outputs["pose"][0, 3:].tolist()
+  odo.trans = pose[:3].tolist()
+  odo.rot = pose[3:6].tolist()
   odo.wideFromDeviceEuler = outputs["wide_from_device_euler"][0, :].tolist()
   odo.roadTransformTrans = outputs["road_transform"][0, :3].tolist()
-  odo.transStd = outputs["pose_stds"][0, :3].tolist()
-  odo.rotStd = outputs["pose_stds"][0, 3:].tolist()
+  odo.transStd = pose_stds[:3].tolist()
+  odo.rotStd = pose_stds[3:6].tolist()
   odo.wideFromDeviceEulerStd = outputs["wide_from_device_euler_stds"][0, :].tolist()
   odo.roadTransformTransStd = outputs["road_transform_stds"][0, :3].tolist()
 

iqpilot/selfdrive/iqmodeld/modeld_selector.py

@@ -159,23 +159,29 @@ class BigLatch:
     return False, False
 
 
+BIG_SOURCES = frozenset({"egpu_big", "mac_big"})
+
+
 def _patch_and_send(pm: PubMaster, payload: dict, frame_drop_perc: float, selector_dropped: int,
                     target: int, source_lag: int, mismatch: bool | None = None) -> None:
   msgs = payload["msgs"]
   if mismatch is None:
     mismatch = source_lag > 0
 
+  big = payload.get("source") in BIG_SOURCES
   model_msg = log_from_bytes(msgs["modelV2"]).as_builder()
   if mismatch:
     model_msg.modelV2.frameId = target
     model_msg.modelV2.frameAge = max(model_msg.modelV2.frameAge, source_lag)
   model_msg.modelV2.frameDropPerc = frame_drop_perc
+  model_msg.modelV2.big = big
   pm.send("modelV2", model_msg)
 
   driving_msg = log_from_bytes(msgs["drivingModelData"]).as_builder()
   if mismatch:
     driving_msg.drivingModelData.frameId = target
   driving_msg.drivingModelData.frameDropPerc = frame_drop_perc
+  driving_msg.drivingModelData.big = big
   pm.send("drivingModelData", driving_msg)
 
   pose_msg = log_from_bytes(msgs["cameraOdometry"]).as_builder()

iqpilot/selfdrive/iqmodeld/tests/test_egpu_policy.py

@@ -80,3 +80,82 @@ def test_layouts():
   assert sum(sizes) == 8 + 2 + 2 + 512
   q = queue_shapes(SPEC, FS)
   assert q["img_q"][0] == (5, 6, 8, 16) and q["feat_q"][0] == (96, 1, 512) and q["desire_q"][0] == (100, 1, 8)
+
+
+CAM = (64, 48)
+
+
+def _nv12(cam_w, cam_h):
+  from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h)
+  return (cam_w, cam_h, stride, y_height, uv_height)
+
+
+def _numpy_warp_plane(src, m, w_dst, h_dst):
+  h_src, w_src = src.shape
+  x = np.tile(np.arange(w_dst, dtype=np.float32), h_dst)
+  y = np.repeat(np.arange(h_dst, dtype=np.float32), w_dst)
+  sx = (m[0, 0] * x + m[0, 1] * y + m[0, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2])
+  sy = (m[1, 0] * x + m[1, 1] * y + m[1, 2]) / (m[2, 0] * x + m[2, 1] * y + m[2, 2])
+  xi = np.clip(np.round(sx), 0, w_src - 1).astype(np.int64)
+  yi = np.clip(np.round(sy), 0, h_src - 1).astype(np.int64)
+  return src[yi, xi].reshape(h_dst, w_dst)
+
+
+def _numpy_frame_prepare(frame, m, nv12, model_w, model_h):
+  cam_w, cam_h, stride, y_height, uv_height = nv12
+  m = m.astype(np.float32)
+  y_src = frame[:cam_h * stride].reshape(cam_h, stride)
+  uv = frame[stride * y_height:stride * y_height + uv_height * stride].reshape(uv_height, stride)
+  m_uv = m * np.array([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], dtype=np.float32)
+  y = _numpy_warp_plane(y_src, m, model_w, model_h)
+  u = _numpy_warp_plane(uv[:cam_h // 2, :cam_w:2], m_uv, model_w // 2, model_h // 2)
+  v = _numpy_warp_plane(uv[:cam_h // 2, 1:cam_w:2], m_uv, model_w // 2, model_h // 2)
+  f = np.concatenate([y.ravel(), u.ravel(), v.ravel()]).reshape(model_h * 3 // 2, model_w)
+  H, W = model_h, model_w
+  return np.stack([f[0:H:2, 0::2], f[1:H:2, 0::2], f[0:H:2, 1::2], f[1:H:2, 1::2],
+                   f[H:H + H // 4].reshape(H // 2, W // 2), f[H + H // 4:H + H // 2].reshape(H // 2, W // 2)])
+
+
+def _jittered_scale(rng, cam, model_w, model_h):
+  m = np.array([[cam[0] / model_w, 0.0, 0.0], [0.0, cam[1] / model_h, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32)
+  m += (0.05 * rng.standard_normal((3, 3))).astype(np.float32) * np.array([[1, 1, 1], [1, 1, 1], [0.01, 0.01, 0.1]], dtype=np.float32)
+  return m
+
+
+def test_frame_layout():
+  from iqpilot.selfdrive.iqmodeld.egpu_policy import frame_layout, model_size, nv12_copy_size
+  shapes, sizes, npy_bytes = frame_layout(SPEC)
+  assert list(shapes) == ["tfm", "big_tfm", "desire", "traffic_convention", "action_t", "prev_feat"]
+  assert npy_bytes == (18 + 8 + 2 + 2 + 512) * 4
+  assert model_size(SPEC) == (32, 16)
+  assert nv12_copy_size(128, 64, 32) == 128 * 96
+
+
+def test_model_runner_matches_device_warp():
+  from tinygrad.engine.jit import TinyJit
+
+  from iqpilot.selfdrive.iqmodeld.egpu_policy import ModelRunner, make_run_model, make_warp, model_size, nv12_copy_size
+  nv12 = _nv12(*CAM)
+  fcs = nv12_copy_size(nv12[2], nv12[3], nv12[4])
+  model_w, model_h = model_size(SPEC)
+  run_policy = make_run_policy(_fake_model, SPEC, FS, "CPU")
+  jit = TinyJit(make_run_model(make_warp(nv12, model_w, model_h, "CPU"), run_policy, SPEC, fcs, "CPU"), prune=True)
+  runner = ModelRunner(jit, SPEC, FS, HIDDEN, "CPU", fcs)
+  ref = PolicyRunner(TinyJit(make_run_policy(_fake_model, SPEC, FS, "CPU"), prune=True), SPEC, FS, HIDDEN, "CPU")
+  rng = np.random.default_rng(7)
+  desire = np.zeros(8, dtype=np.float32)
+  for i in range(10):
+    main = rng.integers(0, 256, fcs, dtype=np.int64).astype(np.uint8)
+    extra = rng.integers(0, 256, fcs, dtype=np.int64).astype(np.uint8)
+    tfm = _jittered_scale(rng, CAM, model_w, model_h)
+    big_tfm = _jittered_scale(rng, CAM, model_w, model_h)
+    if i in (2, 6):
+      desire[:] = 0
+      desire[1 + i % 3] = 1
+    traffic = np.array([1.0, 0.0], dtype=np.float32) if i % 2 else np.array([0.0, 1.0], dtype=np.float32)
+    action_t = np.array([0.1 * i, 0.2], dtype=np.float32)
+    got = runner.run(main, extra, tfm, big_tfm, desire, traffic, action_t)
+    warped = np.stack([_numpy_frame_prepare(main, tfm, nv12, model_w, model_h), _numpy_frame_prepare(extra, big_tfm, nv12, model_w, model_h)])
+    want = ref.run(warped, desire, traffic, action_t)
+    np.testing.assert_array_equal(got, want, err_msg=f"frame {i}")

iqpilot/selfdrive/iqmodeld/tools/compile_egpu_model.py

@@ -157,25 +157,34 @@ def _policy_frame(seed: int, input_spec: dict):
   return warped
 
 
-def _tc_off_reference(onnx_path: str, meta: dict):
+def _tc_off_reference(onnx_path: str, meta: dict, fmt: int = 2, resolutions: tuple[tuple[int, int], ...] = ()):
   """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")
+    ref = os.path.join(td, "ref.npz" if fmt == 3 else "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)
+    cmd = [sys.executable, "-m", "iqpilot.selfdrive.iqmodeld.tools.compile_egpu_model",
+           "--model", meta["key"], "--onnx", onnx_path, "--tc-off", "--format", str(fmt)]
+    if resolutions:
+      cmd += ["--camera-resolutions", *(f"{w}x{h}" for w, h in resolutions)]
+    r = subprocess.run(cmd, 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 _parity_check(key: str, got: np.ndarray, ref: np.ndarray, label: str = "") -> None:
+  rel = float(np.abs(got - ref).mean() / max(1e-3, float(np.abs(ref).mean())))
+  if rel > 0.01:
+    raise _ParityFail(f"PARITY FAIL: TC kernels miscompiled {key} {label}(rel={rel:.4f} vs TC-off); recompiling with tensor cores disabled")
+  print(f"  parity vs TC-off reference {label}: rel={rel:.6f} OK")
+
+
 def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str:
   from tinygrad.device import Device
   from tinygrad.engine.jit import TinyJit
@@ -259,16 +268,138 @@ def compile_policy_model(meta: dict, onnx_path: str, out_path: str) -> str:
   _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")
+    _parity_check(meta["key"], outs[-1], _tc_off_reference(onnx_path, meta))
 
   os.replace(tmp, out_path)
   return out_path
 
 
+DEFAULT_CAMERA_RESOLUTIONS: tuple[tuple[int, int], ...] = ((1928, 1208), (1344, 760))
+
+
+def camera_nv12(cam_w: int, cam_h: int) -> tuple[int, int, int, int, int]:
+  from iqpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  stride, y_height, uv_height, _ = get_nv12_info(cam_w, cam_h)
+  return (cam_w, cam_h, stride, y_height, uv_height)
+
+
+def _fill_model_frame(packed, seed: int, res: tuple[int, int], model_w: int, model_h: int) -> None:
+  rng = np.random.default_rng(seed)
+  cam_w, cam_h = res
+  scale = np.array([[cam_w / model_w, 0.0, 0.0], [0.0, cam_h / model_h, 0.0], [0.0, 0.0, 1.0]], dtype=np.float32)
+  for name in ("tfm", "big_tfm"):
+    packed.views[name][:, :] = scale * (1.0 + 0.02 * rng.standard_normal((3, 3))).astype(np.float32)
+  for v in packed.frames.values():
+    v[:] = rng.integers(0, 256, size=v.shape, dtype=np.uint8)
+  packed.views["traffic_convention"][:] = [1, 0]
+  packed.views["action_t"][:] = [0.2, 0.3]
+
+
+def compile_model_v3(meta: dict, onnx_path: str, out_path: str,
+                     resolutions: tuple[tuple[int, int], ...] = DEFAULT_CAMERA_RESOLUTIONS) -> str:
+  from tinygrad.device import Device
+  from tinygrad.engine.jit import TinyJit
+  from tinygrad.nn.onnx import OnnxRunner
+
+  from iqpilot.selfdrive.iqmodeld.egpu_policy import (
+    MODEL_FORMAT, dump_oob, load_bundle, make_model_queues, make_run_model, make_run_policy, make_warp, model_size, nv12_copy_size,
+  )
+
+  if meta.get("split"):
+    raise RuntimeError(f"model {meta['key']} is a split model; eGPU compiles fused models only")
+  input_spec = {name: (tuple(shape), dtype) for name, (shape, dtype) in INPUT_SPEC.items()}
+  frame_skip = int(meta["frame_skip"])
+  hidden = meta["output_slices"]["hidden_state"]
+  device = Device.DEFAULT
+  model_w, model_h = model_size(input_spec)
+  runner = OnnxRunner(onnx_path)
+  run_policy = make_run_policy(runner, input_spec, frame_skip, device)
+
+  def step(jit, queues, packed, seed: int, res: tuple[int, int]) -> np.ndarray:
+    _fill_model_frame(packed, seed, res, model_w, model_h)
+    st = time.perf_counter()
+    out, = jit(**queues)
+    flat = out.numpy().reshape(-1)
+    print(f"  model step(seed={seed}, {res[0]}x{res[1]}) {(time.perf_counter() - st) * 1e3:6.1f} ms")
+    packed.views["prev_feat"][:] = flat[hidden].reshape(packed.views["prev_feat"].shape)
+    return flat
+
+  def run_three(jit, fcs: int, res: tuple[int, int]) -> list[np.ndarray]:
+    queues, packed = make_model_queues(input_spec, frame_skip, device, fcs)
+    return [step(jit, queues, packed, SEED + i, res) for i in range(3)]
+
+  jits: dict[tuple[int, int], object] = {}
+  sizes: dict[tuple[int, int], int] = {}
+  nv12s: dict[tuple[int, int], tuple[int, int, int, int, int]] = {}
+  baselines: dict[tuple[int, int], np.ndarray] = {}
+  for res in resolutions:
+    nv12 = camera_nv12(*res)
+    fcs = nv12_copy_size(nv12[2], nv12[3], nv12[4])
+    jit = TinyJit(make_run_model(make_warp(nv12, model_w, model_h, device), run_policy, input_spec, fcs, device), prune=True)
+    print(f"capture + replay {res[0]}x{res[1]} (frame copy {fcs} B)")
+    baseline = run_three(jit, fcs, res)[-1]
+    if baseline.shape[0] != meta["output_len"]:
+      raise RuntimeError(f"model output length {baseline.shape[0]} != registry {meta['output_len']}")
+    if not HOST and not np.isfinite(baseline).all():
+      raise RuntimeError("compiled model produced non-finite outputs")
+    jits[res], sizes[res], nv12s[res], baselines[res] = jit, fcs, nv12, baseline
+
+  bundle = {
+    "format": MODEL_FORMAT,
+    "run_model": jits,
+    "frame_copy_size": sizes,
+    "nv12": nv12s,
+    "model_key": meta["key"],
+    "model_sha256": meta["sha256"],
+    "output_len": int(meta["output_len"]),
+    "frame_skip": frame_skip,
+    "input_spec": input_spec,
+    "input_device": device,
+  }
+  os.makedirs(os.path.dirname(out_path), exist_ok=True)
+  tmp = out_path + ".part"
+  print("serialize (out-of-band buffers)")
+  with open(tmp, "wb") as f:
+    dump_oob(bundle, f)
+
+  del bundle, jits, run_policy, runner
+  gc.collect()
+
+  print("reload + validate")
+  loaded = load_bundle(tmp)
+  outs = {res: run_three(loaded["run_model"][res], loaded["frame_copy_size"][res], res) for res in resolutions}
+  ref_target = os.environ.get("IQ_EGPU_REFERENCE")
+  if ref_target:
+    np.savez(ref_target, **{f"{w}x{h}": outs[(w, h)][-1] for (w, h) in resolutions})
+    return out_path
+  if HOST:
+    os.replace(tmp, out_path)
+    return out_path
+  for res in resolutions:
+    if not np.array_equal(outs[res][-1], baselines[res]):
+      raise RuntimeError(f"model outputs differ from baseline after pickle round trip ({res[0]}x{res[1]})")
+    if np.array_equal(outs[res][0], outs[res][-1]):
+      raise RuntimeError(f"model outputs insensitive to inputs after pickle round trip ({res[0]}x{res[1]})")
+    if not all(np.isfinite(o).all() for o in outs[res]):
+      raise RuntimeError(f"reloaded model produced non-finite outputs ({res[0]}x{res[1]})")
+  from iqpilot.selfdrive.iqmodeld.parser import PhaseParser
+  from iqpilot.selfdrive.iqmodeld.tools.compile_supercombo import _slice_outputs, _validate_pose_outputs
+  _validate_pose_outputs(PhaseParser().parse_vision_outputs(_slice_outputs(outs[resolutions[0]][-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, fmt=3, resolutions=resolutions)
+    for (w, h) in resolutions:
+      _parity_check(meta["key"], outs[(w, h)][-1], ref[f"{w}x{h}"], label=f"{w}x{h} ")
+
+  os.replace(tmp, out_path)
+  return out_path
+
+
+def _parse_resolution(text: str) -> tuple[int, int]:
+  w, h = text.lower().split("x")
+  return int(w), int(h)
+
+
 def main() -> None:
   p = argparse.ArgumentParser()
   p.add_argument("--model", default=None, help=f"registry key, one of {sorted(EGPU_MODELS)}")
@@ -277,13 +408,16 @@ def main() -> None:
   p.add_argument("--progress-param", default=None)
   p.add_argument("--progress-base", type=float, default=None)
   p.add_argument("--progress-span", type=float, default=0.0)
-  p.add_argument("--format", type=int, default=2, choices=(1, 2))
+  p.add_argument("--format", type=int, default=3, choices=(1, 2, 3),
+                 help="3 = warp on the dock from raw NV12 (comma master); 2 = device-warped policy bundle")
+  p.add_argument("--camera-resolutions", type=_parse_resolution, nargs="+", default=list(DEFAULT_CAMERA_RESOLUTIONS),
+                 help="WxH camera sizes bundled into a format-3 artifact")
   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")
+  if args.host and args.format == 1:
+    raise SystemExit("--host supports formats 2 and 3 only")
 
   if args.model is not None:
     if args.model in EGPU_MODELS:
@@ -311,16 +445,20 @@ def main() -> None:
                                daemon=True)
     sampler.start()
   try:
-    build = compile_policy_model if args.format == 2 else compile_model
+    if args.format == 3:
+      from functools import partial
+      build = partial(compile_model_v3, resolutions=tuple(args.camera_resolutions))
+    else:
+      build = compile_policy_model if args.format == 2 else compile_model
     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:
+      if os.environ.get("TC_OPT") == "0" or args.format == 1:
         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))
+      out = build(meta, onnx_path, args.output or egpu_pkl_path(meta))
   finally:
     if stop is not None:
       stop.set()