v16

selfdrive/car/cruise_state.py

@@ -0,0 +1,22 @@
+from opendbc.car import structs
+
+
+def hyundai_openpilot_longitudinal_acc_req_feedback(CP: structs.CarParams) -> bool:
+  return CP.brand == 'hyundai' and CP.openpilotLongitudinalControl and not CP.pcmCruise
+
+
+def should_cancel_stock_cruise(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool) -> bool:
+  if not cruise_enabled:
+    return False
+  if not controls_enabled:
+    return True
+  return not CP.pcmCruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP)
+
+
+def should_flag_cruise_mismatch(CP: structs.CarParams, cruise_enabled: bool, controls_enabled: bool,
+                                effective_pcm_cruise: bool) -> bool:
+  if not cruise_enabled:
+    return False
+  if not controls_enabled:
+    return True
+  return not effective_pcm_cruise and not hyundai_openpilot_longitudinal_acc_req_feedback(CP)

selfdrive/car/tests/test_car_specific.py

@@ -0,0 +1,41 @@
+from cereal import car
+
+from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise, should_flag_cruise_mismatch
+
+
+def make_cp(brand="hyundai", op_long=True, pcm_cruise=False):
+  cp = car.CarParams.new_message()
+  cp.brand = brand
+  cp.openpilotLongitudinalControl = op_long
+  cp.pcmCruise = pcm_cruise
+  return cp
+
+
+def test_hyundai_openpilot_long_does_not_cancel_active_acc_req_feedback():
+  cp = make_cp()
+
+  assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False)
+
+
+def test_hyundai_openpilot_long_still_flags_cruise_when_controls_disabled():
+  cp = make_cp()
+
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=False)
+
+
+def test_non_hyundai_openpilot_long_behavior_is_unchanged():
+  cp = make_cp(brand="toyota")
+
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=False)
+
+
+def test_pcm_cruise_behavior_is_unchanged():
+  cp = make_cp(op_long=False, pcm_cruise=True)
+
+  assert not should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=True)
+  assert should_cancel_stock_cruise(cp, cruise_enabled=True, controls_enabled=False)
+  assert not should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=True, effective_pcm_cruise=True)
+  assert should_flag_cruise_mismatch(cp, cruise_enabled=True, controls_enabled=False, effective_pcm_cruise=True)

selfdrive/controls/controlsd.py

@@ -23,6 +23,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import (
   get_bolt_2017_steer_ratio_scale,
 )
 from openpilot.selfdrive.controls.lib.longcontrol import LongControl
+from openpilot.selfdrive.car.cruise_state import should_cancel_stock_cruise
 from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 
@@ -275,7 +276,7 @@ class Controls:
       CC.enabled,
       self.sm['starpilotCarState'].alwaysOnLateralEnabled,
     )
-    cancel_requested = CS.cruiseState.enabled and (not CC.enabled or not self.CP.pcmCruise)
+    cancel_requested = should_cancel_stock_cruise(self.CP, CS.cruiseState.enabled, CC.enabled)
     CC.cruiseControl.cancel = cancel_requested and not pacifica_hybrid_aol
 
     legacy_resume_hack = False

selfdrive/controls/lib/latcontrol_torque.py

@@ -484,9 +484,9 @@ IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
 IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60
 IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.88
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 9.3
-IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 0.9
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.96
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.0
+IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.4
 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT = 0.10
 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_LAT_WIDTH = 0.06
 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.82

selfdrive/controls/tests/test_latcontrol.py

@@ -476,6 +476,8 @@ class TestLatControl:
     assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0)
     assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0)
     assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 20.0)
+    assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 6.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0)
+    assert get_ioniq_6_directional_taper_scale(-0.70, -0.70, 12.0) > get_ioniq_6_directional_taper_scale(-0.70, -0.70, 20.0)
 
   def test_ioniq_6_output_taper_curve(self):
     assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0

selfdrive/modeld/compile_modeld.py

@@ -4,114 +4,89 @@ import atexit
 import os
 import pickle
 import time
-from collections import defaultdict, namedtuple
 from functools import partial
+from collections import namedtuple
 
 import numpy as np
 
-
 def _patch_tinygrad_fetch_fw():
   import hashlib
   import pathlib
-
   import zstandard
   from tinygrad import helpers
-
-  original_fetch_fw = getattr(helpers, "fetch_fw", None)
-  if original_fetch_fw is None:
+  _orig = getattr(helpers, "fetch_fw", None)
+  if _orig is None:
     return
-
   def fetch_fw(path, name, sha256):
-    firmware_path = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
-    if firmware_path.is_file():
-      blob = zstandard.ZstdDecompressor().stream_reader(firmware_path.read_bytes()).read()
+    p = pathlib.Path(f"/lib/firmware/{path}/{name}.zst")
+    if p.is_file():
+      blob = zstandard.ZstdDecompressor().stream_reader(p.read_bytes()).read()
       if hashlib.sha256(blob).hexdigest() == sha256:
         return blob
-    return original_fetch_fw(path, name, sha256)
-
+    return _orig(path, name, sha256)
   helpers.fetch_fw = fetch_fw
-
-
 _patch_tinygrad_fetch_fw()
 
+from tinygrad.tensor import Tensor
+from tinygrad.helpers import Context
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
-from tinygrad.helpers import Context
-from tinygrad.tensor import Tensor
 
 
-NV12Frame = namedtuple("NV12Frame", ["width", "height", "stride", "y_height", "uv_height", "size"])
-WARP_INPUTS = ["img_q", "big_img_q", "tfm", "big_tfm"]
-POLICY_INPUTS = ["feat_q", "desire_q", "desire", "traffic_convention", "action_t"]
+NV12Frame = namedtuple("NV12Frame", ['width', 'height', 'stride', 'y_height', 'uv_height', 'size'])
+WARP_INPUTS = ['img_q', 'big_img_q', 'tfm', 'big_tfm']
+POLICY_INPUTS = ['feat_q', 'desire_q', 'desire', 'traffic_convention', 'action_t']
 
-WARP_DEV = os.getenv("WARP_DEV")
+UV_SCALE_MATRIX = np.array([[0.5, 0, 0], [0, 0.5, 0], [0, 0, 1]], dtype=np.float32)
+UV_SCALE_MATRIX_INV = np.linalg.inv(UV_SCALE_MATRIX)
+
+WARP_DEV = os.getenv('WARP_DEV')
 
 
 def make_random_images(keys, shape, device=None):
-  return {key: Tensor.randint(shape, low=0, high=256, dtype="uint8", device=device).realize() for key in keys}
+  return {k: Tensor.randint(shape, low=0, high=256, dtype='uint8', device=device).realize() for k in keys}
 
 
-class _BlobTensorInputs(dict):
-  _backing_arrays: dict[str, np.ndarray]
+def warp_perspective_tinygrad(src_flat, M_inv, dst_shape, src_shape, stride_pad, border_fill_val=None):
+  w_dst, h_dst = dst_shape
+  h_src, w_src = src_shape
 
+  x = Tensor.arange(w_dst, device=WARP_DEV).reshape(1, w_dst).expand(h_dst, w_dst).reshape(-1)
+  y = Tensor.arange(h_dst, device=WARP_DEV).reshape(h_dst, 1).expand(h_dst, w_dst).reshape(-1)
 
-def make_random_blob_images(keys, shape, device=None):
-  blob_shape = shape if isinstance(shape, tuple) else (shape,)
-  backing_arrays = {
-    key: np.random.randint(0, 256, size=blob_shape, dtype=np.uint8)
-    for key in keys
-  }
-  inputs = _BlobTensorInputs({
-    key: Tensor.from_blob(array.ctypes.data, array.shape, dtype="uint8", device=device).realize()
-    for key, array in backing_arrays.items()
-  })
-  # Keep the numpy storage alive for the duration of the JIT capture/replay call.
-  inputs._backing_arrays = backing_arrays
-  return inputs
-
-
-def warp_perspective_tinygrad(src_flat, matrix_inverse, dst_shape, src_shape, stride_pad, border_fill_val=None):
-  width_dst, height_dst = dst_shape
-  height_src, width_src = src_shape
-
-  x = Tensor.arange(width_dst, device=WARP_DEV).reshape(1, width_dst).expand(height_dst, width_dst).reshape(-1)
-  y = Tensor.arange(height_dst, device=WARP_DEV).reshape(height_dst, 1).expand(height_dst, width_dst).reshape(-1)
-
-  # Inline 3x3 matmul as elementwise to avoid reduce ops and enable fusion with gather.
-  src_x = matrix_inverse[0, 0] * x + matrix_inverse[0, 1] * y + matrix_inverse[0, 2]
-  src_y = matrix_inverse[1, 0] * x + matrix_inverse[1, 1] * y + matrix_inverse[1, 2]
-  src_w = matrix_inverse[2, 0] * x + matrix_inverse[2, 1] * y + matrix_inverse[2, 2]
+  # inline 3x3 matmul as elementwise to avoid reduce op (enables fusion with gather)
+  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, width_src - 1).cast("int")
-  y_nn_clipped = y_round.clip(0, height_src - 1).cast("int")
-  idx = y_nn_clipped * (width_src + stride_pad) + x_nn_clipped
+  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 <= width_src - 1) &
-               (y_round >= 0) & (y_round <= height_src - 1)).cast(sampled.dtype)
+  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):
-  height = (frames.shape[0] * 2) // 3
-  width = frames.shape[1]
-  return Tensor.cat(
-    frames[0:height:2, 0::2],
-    frames[1:height:2, 0::2],
-    frames[0:height:2, 1::2],
-    frames[1:height:2, 1::2],
-    frames[height:height + height // 4].reshape((height // 2, width // 2)),
-    frames[height + height // 4:height + height // 2].reshape((height // 2, width // 2)),
-    dim=0,
-  ).reshape((6, height // 2, width // 2))
+  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: NV12Frame, model_w, model_h):
@@ -119,81 +94,65 @@ def make_frame_prepare(nv12: NV12Frame, model_w, model_h):
   uv_offset = stride * y_height
   stride_pad = stride - cam_w
 
-  def frame_prepare_tinygrad(input_frame, matrix_inverse):
-    # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling.
-    matrix_inverse_uv = matrix_inverse * Tensor([[1.0, 1.0, 0.5], [1.0, 1.0, 0.5], [2.0, 2.0, 1.0]], device=WARP_DEV)
-    # Deinterleave NV12 UV plane (UVUV... -> separate U, V).
+  def frame_prepare_tinygrad(input_frame, M_inv):
+    # UV_SCALE @ M_inv @ UV_SCALE_INV simplifies to elementwise scaling
+    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=WARP_DEV)
+    # deinterleave NV12 UV plane (UVUV... -> separate U, V)
     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],
-        matrix_inverse,
-        (model_w, model_h),
-        (cam_h, cam_w),
-        stride_pad,
-      ).realize()
-      u = warp_perspective_tinygrad(
-        uv[:cam_h // 2, :cam_w:2].flatten(),
-        matrix_inverse_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(),
-        matrix_inverse_uv,
-        (model_w // 2, model_h // 2),
-        (cam_h // 2, cam_w // 2),
-        0,
-      ).realize()
+      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)
-
+    tensor = frames_to_tensor(yuv)
+    return tensor
   return frame_prepare_tinygrad
 
 
-def make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device):
-  img = vision_input_shapes["img"]
+def make_warp_input_queues(vision_input_shapes, frame_skip, device):
+  img = vision_input_shapes['img']  # (1, 12, 128, 256)
   n_frames = img[1] // 6
   img_buf_shape = (frame_skip * (n_frames - 1) + 1, 6, img[2], img[3])
 
-  features_buffer = policy_input_shapes["features_buffer"]
-  desire_pulse = policy_input_shapes["desire_pulse"]
-
-  return {
-    "img_q": Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    "big_img_q": Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
-    "feat_q": Tensor(
-      np.zeros((frame_skip * (features_buffer[1] - 1) + 1, features_buffer[0], features_buffer[2]), dtype=np.float32),
-      device=device,
-    ).contiguous().realize(),
-    "desire_q": Tensor(
-      np.zeros((frame_skip * desire_pulse[1], desire_pulse[0], desire_pulse[2]), dtype=np.float32),
-      device=device,
-    ).contiguous().realize(),
-  }
-
-
-def make_npy_inputs(policy_input_shapes):
-  desire_pulse = policy_input_shapes["desire_pulse"]
-  traffic_convention = policy_input_shapes["traffic_convention"]
-
   npy = {
-    "desire": np.zeros(desire_pulse[2], dtype=np.float32),
-    "traffic_convention": np.zeros(traffic_convention, dtype=np.float32),
-    "tfm": np.zeros((3, 3), dtype=np.float32),
-    "big_tfm": np.zeros((3, 3), dtype=np.float32),
+    'tfm': np.zeros((3, 3), dtype=np.float32),
+    'big_tfm': np.zeros((3, 3), dtype=np.float32),
   }
-  if "action_t" in policy_input_shapes:
-    npy["action_t"] = np.zeros(policy_input_shapes["action_t"], dtype=np.float32)
-  npy_tensors = {key: Tensor(value, device="NPY").realize() for key, value in npy.items()}
-  return npy, npy_tensors
+  input_queues = {
+    'img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    'big_img_q': Tensor(np.zeros(img_buf_shape, dtype=np.uint8), device=device).contiguous().realize(),
+    **{k: Tensor(v, device='NPY').realize() for k, v in npy.items()},
+  }
+  return input_queues, npy
 
 
 def make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, device):
-  tensor_inputs = make_tensor_inputs(vision_input_shapes, policy_input_shapes, frame_skip, device)
-  npy, npy_tensors = make_npy_inputs(policy_input_shapes)
-  return {**tensor_inputs, **npy_tensors}, npy
+  input_queues, npy = make_warp_input_queues(vision_input_shapes, frame_skip, device)
+
+  fb = policy_input_shapes['features_buffer']  # (1, 25, 512)
+  dp = policy_input_shapes['desire_pulse']  # (1, 25, 8)
+  tc = policy_input_shapes['traffic_convention']  # (1, 2)
+  #TODO action_t is hardcoded to match tc for future compatibility
+  at = tc
+
+  policy_npy = {
+    'desire': np.zeros(dp[2], dtype=np.float32),
+    'traffic_convention': np.zeros(tc, dtype=np.float32),
+    'action_t': np.zeros(at, dtype=np.float32),
+  }
+  npy.update(policy_npy)
+  input_queues.update({
+    'feat_q': Tensor(np.zeros((frame_skip * (fb[1] - 1) + 1, fb[0], fb[2]), dtype=np.float32), device=device).contiguous().realize(),
+    'desire_q': Tensor(np.zeros((frame_skip * dp[1], dp[0], dp[2]), dtype=np.float32), device=device).contiguous().realize(),
+    **{k: Tensor(v, device='NPY').realize() for k, v in policy_npy.items()},
+  })
+  return input_queues, npy
 
 
 def shift_and_sample(buf, new_val, sample_fn):
@@ -223,44 +182,39 @@ def make_warp(nv12, model_w, model_h, frame_skip):
     img = shift_and_sample(img_q, warped_frame, sample_skip_fn)
     big_img = shift_and_sample(big_img_q, warped_big_frame, sample_skip_fn)
     return img, big_img
-
   return warp_enqueue
 
 
-def make_run_policy(vision_runner, off_policy_runner, on_policy_runner, vision_features_slice, frame_skip):
+def make_run_policy(model_runners, model_metadata, frame_skip):
   sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)
   sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  vision_features_slice = model_metadata['vision']['output_slices']['hidden_state']
 
   def run_policy(img, big_img, feat_q, desire_q, desire, traffic_convention, action_t):
     desire = desire.to(Device.DEFAULT)
     traffic_convention = traffic_convention.to(Device.DEFAULT)
     action_t = action_t.to(Device.DEFAULT)
     Tensor.realize(desire, traffic_convention, action_t)
-
     desire_buf = shift_and_sample(desire_q, desire.reshape(1, 1, -1), sample_desire_fn)
-    vision_out = next(iter(vision_runner({"img": img, "big_img": big_img}).values())).cast("float32")
+    vision_out = next(iter(model_runners['vision']({'img': img, 'big_img': big_img}).values())).cast('float32')
 
     new_feat = vision_out[:, vision_features_slice].reshape(1, -1).unsqueeze(0)
     feat_buf = shift_and_sample(feat_q, new_feat, sample_skip_fn)
 
     inputs = {
-      "features_buffer": feat_buf,
-      "desire_pulse": desire_buf,
-      "traffic_convention": traffic_convention,
-      "action_t": action_t,
+      'features_buffer': feat_buf,
+      'desire_pulse': desire_buf,
+      'traffic_convention': traffic_convention,
+      'action_t': action_t,
     }
-    on_policy_out = next(iter(on_policy_runner(inputs).values())).cast("float32")
-    off_policy_out = next(iter(off_policy_runner(inputs).values())).cast("float32")
+    on_policy_out = next(iter(model_runners['on_policy'](inputs).values())).cast('float32')
+    off_policy_out = next(iter(model_runners['off_policy'](inputs).values())).cast('float32')
     return vision_out, on_policy_out, off_policy_out
-
   return run_policy
 
 
-def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata, policy_metadata):
-  vision_input_shapes = vision_metadata["input_shapes"]
-  policy_input_shapes = policy_metadata["input_shapes"]
-
-  seed = 42
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
+  SEED = 42
   validation_rtol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0
   validation_atol = 5e-3 if Device.DEFAULT == "QCOM" else 0.0
 
@@ -271,118 +225,104 @@ def compile_jit(jit, make_random_inputs, input_keys, frame_skip, vision_metadata
       return np.allclose(lhs, rhs, rtol=validation_rtol, atol=validation_atol, equal_nan=True)
     return np.array_equal(lhs, rhs)
 
-  def random_inputs_run(fn, current_seed, test_val=None, test_buffers=None, expect_match=True):
-    input_queues, npy = make_input_queues(vision_input_shapes, policy_input_shapes, frame_skip, Device.DEFAULT)
-    np.random.seed(current_seed)
-    Tensor.manual_seed(current_seed)
+  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
+    input_queues, npy = make_queues(Device.DEFAULT)
+    np.random.seed(seed)
+    Tensor.manual_seed(seed)
 
     testing = test_val is not None or test_buffers is not None
     n_runs = 1 if testing else 3
 
-    for idx in range(n_runs):
-      for value in npy.values():
-        value[:] = np.random.randn(*value.shape).astype(value.dtype)
+    for i in range(n_runs):
+      for v in npy.values():
+        v[:] = np.random.randn(*v.shape).astype(v.dtype)
       Device.default.synchronize()
       random_inputs = make_random_inputs()
-      start = time.perf_counter()
-      outs = fn(**{key: input_queues[key] for key in input_keys}, **random_inputs)
-      mid = time.perf_counter()
+      st = time.perf_counter()
+      outs = fn(**{k: input_queues[k] for k in input_keys}, **random_inputs)
+      mt = time.perf_counter()
       Device.default.synchronize()
-      end = time.perf_counter()
-      print(f"  [{idx + 1}/{n_runs}] enqueue {(mid - start) * 1e3:6.2f} ms -- total {(end - start) * 1e3:6.2f} ms")
+      et = time.perf_counter()
+      print(f"  [{i+1}/{n_runs}] enqueue {(mt-st)*1e3:6.2f} ms -- total {(et-st)*1e3:6.2f} ms")
 
-      if idx == 0:
-        val = [np.copy(value.numpy()) for value in outs]
-        buffers = [np.copy(value.numpy().copy()) for value in input_queues.values()]
+      if i == 0:
+        val = [np.copy(v.numpy()) for v in outs]
+        buffers = [np.copy(v.numpy().copy()) for v in input_queues.values()]
 
     if Device.DEFAULT != "QCOM":
       if test_val is not None:
-        match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(val, test_val, strict=True))
-        assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={current_seed})"
+        match = all(arrays_match(a, b) for a, b in zip(val, test_val, strict=True))
+        assert match == expect_match, f"outputs {'differ from' if expect_match else 'match'} baseline (seed={seed})"
       if test_buffers is not None:
-        match = all(arrays_match(lhs, rhs) for lhs, rhs in zip(buffers, test_buffers, strict=True))
-        assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={current_seed})"
+        match = all(arrays_match(a, b) for a, b in zip(buffers, test_buffers, strict=True))
+        assert match == expect_match, f"buffers {'differ from' if expect_match else 'match'} baseline (seed={seed})"
     return val, buffers
 
-  print("capture + replay")
-  test_val, test_buffers = random_inputs_run(jit, seed)
-  print("pickle round trip")
+  print('capture + replay')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
   jit = pickle.loads(pickle.dumps(jit))
-  random_inputs_run(jit, seed, test_val, test_buffers, expect_match=True)
-  random_inputs_run(jit, seed + 1, test_val, test_buffers, expect_match=False)
+  random_inputs_run(jit, SEED, test_val, test_buffers, expect_match=True)
+  random_inputs_run(jit, SEED+1, test_val, test_buffers, expect_match=False)
   return jit
 
 
-def _parse_size(size):
-  width, height = size.lower().split("x")
-  return int(width), int(height)
+def _parse_size(s):
+  w, h = s.lower().split('x')
+  return int(w), int(h)
 
 
 def read_file_chunked_to_shm(path):
   from openpilot.common.file_chunker import read_file_chunked
   from openpilot.system.hardware.hw import Paths
-
   shm_path = os.path.join(Paths.shm_path(), os.path.basename(path))
   atexit.register(lambda: os.path.exists(shm_path) and os.remove(shm_path))
-  with open(shm_path, "wb") as f:
+  with open(shm_path, 'wb') as f:
     f.write(read_file_chunked(path))
   return shm_path
 
 
 if __name__ == "__main__":
   from tinygrad.nn.onnx import OnnxRunner
-  from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
   from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+  from openpilot.selfdrive.modeld.get_model_metadata import make_metadata_dict
+  p = argparse.ArgumentParser()
+  p.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
+  p.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True,
+                 help='camera resolutions WxH (one or more)')
+  p.add_argument('--vision-onnx', required=True)
+  p.add_argument('--off-policy-onnx', required=True)
+  p.add_argument('--on-policy-onnx', required=True)
+  p.add_argument('--output', required=True)
+  p.add_argument('--frame-skip', type=int, required=True)
+  args = p.parse_args()
 
-  parser = argparse.ArgumentParser()
-  parser.add_argument("--model-size", type=_parse_size, required=True, help="model input WxH")
-  parser.add_argument("--camera-resolutions", type=_parse_size, nargs="+", required=True, help="camera resolutions WxH (one or more)")
-  parser.add_argument("--vision-onnx", required=True)
-  parser.add_argument("--off-policy-onnx", required=True)
-  parser.add_argument("--on-policy-onnx", required=True)
-  parser.add_argument("--output", required=True)
-  parser.add_argument("--frame-skip", type=int, required=True)
-  args = parser.parse_args()
-
-  out = defaultdict(dict)
-  vision_path = read_file_chunked_to_shm(args.vision_onnx)
-  off_policy_path = read_file_chunked_to_shm(args.off_policy_onnx)
-  on_policy_path = read_file_chunked_to_shm(args.on_policy_onnx)
+  model_paths = {
+    'vision': read_file_chunked_to_shm(args.vision_onnx),
+    'off_policy': read_file_chunked_to_shm(args.off_policy_onnx),
+    'on_policy': read_file_chunked_to_shm(args.on_policy_onnx),
+  }
   model_w, model_h = args.model_size
 
-  vision_runner = OnnxRunner(vision_path)
-  off_policy_runner = OnnxRunner(off_policy_path)
-  on_policy_runner = OnnxRunner(on_policy_path)
-  vision_metadata = make_metadata_dict(vision_path)
-  off_policy_metadata = make_metadata_dict(off_policy_path)
-  on_policy_metadata = make_metadata_dict(on_policy_path)
-  assert off_policy_metadata["input_shapes"] == on_policy_metadata["input_shapes"]
+  model_runners = {name: OnnxRunner(path) for name, path in model_paths.items()}
+  out = {'metadata': {name: make_metadata_dict(path) for name, path in model_paths.items()}}
 
-  run_policy_jit = TinyJit(
-    make_run_policy(
-      vision_runner,
-      off_policy_runner,
-      on_policy_runner,
-      vision_metadata["output_slices"]["hidden_state"],
-      args.frame_skip,
-    ),
-    prune=True,
-  )
+  assert out['metadata']['off_policy']['input_shapes'] == out['metadata']['on_policy']['input_shapes']
 
-  out["metadata"]["vision"] = vision_metadata
-  out["metadata"]["off_policy"] = off_policy_metadata
-  out["metadata"]["on_policy"] = on_policy_metadata
-  out["tensor_inputs"] = make_tensor_inputs(vision_metadata["input_shapes"], on_policy_metadata["input_shapes"], args.frame_skip, Device.DEFAULT)
+  run_policy_jit = TinyJit(make_run_policy(model_runners, out['metadata'], args.frame_skip), prune=True)
 
-  make_random_model_inputs = partial(make_random_images, keys=["img", "big_img"], shape=vision_metadata["input_shapes"]["img"])
-  out["run_policy"] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+  make_policy_queues = partial(make_input_queues, out['metadata']['vision']['input_shapes'],
+                               out['metadata']['on_policy']['input_shapes'], args.frame_skip)
+  make_random_model_inputs = partial(make_random_images, keys=['img', 'big_img'], shape=out['metadata']['vision']['input_shapes']['img'])
+  out['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS,
+                                  make_policy_queues)
 
   for cam_w, cam_h in args.camera_resolutions:
     nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-    # Capture warp against blob-backed frames so the JIT ABI matches runtime VisionBuf inputs.
-    make_random_warp_inputs = partial(make_random_blob_images, keys=["frame", "big_frame"], shape=nv12.size, device=WARP_DEV)
+    make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
     warp_enqueue = TinyJit(make_warp(nv12, model_w, model_h, args.frame_skip), prune=True)
-    out[(cam_w, cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, args.frame_skip, vision_metadata, on_policy_metadata)
+    make_warp_queues = partial(make_warp_input_queues, out['metadata']['vision']['input_shapes'], args.frame_skip)
+    out[(cam_w,cam_h)] = compile_jit(warp_enqueue, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)
 
   with open(args.output, "wb") as f:
     pickle.dump(out, f)

selfdrive/modeld/modeld_v16.py

@@ -27,9 +27,9 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import smooth_value
-from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, WARP_INPUTS, make_npy_inputs, make_tensor_inputs
-from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value
+from openpilot.selfdrive.modeld.compile_modeld import POLICY_INPUTS, WARP_INPUTS, make_input_queues
+from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.fill_model_msg import PublishState, fill_model_msg, fill_pose_msg
 from openpilot.selfdrive.modeld.helpers import get_tg_input_devices
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
@@ -113,9 +113,25 @@ def _combined_model_path(model_id: str, use_builtin_model: bool) -> Path:
 
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, v_ego: float) -> log.ModelDataV2.Action:
-  desired_curv_unscaled, desired_accel = model_output["action"][0]
-  desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
-  should_stop = (v_ego < 0.3 and desired_accel < 0.1)
+  if "action" in model_output:
+    desired_curv_unscaled, desired_accel = model_output["action"][0]
+    desired_curvature = float(desired_curv_unscaled) / max(1.0, v_ego) ** 2
+    should_stop = (v_ego < 0.3 and desired_accel < 0.1)
+  else:
+    plan = model_output["plan"][0]
+    desired_accel, should_stop = get_accel_from_plan(
+      plan[:, Plan.VELOCITY][:, 0],
+      plan[:, Plan.ACCELERATION][:, 0],
+      ModelConstants.T_IDXS,
+      action_t=DT_MDL,
+    )
+    desired_curvature = get_curvature_from_plan(
+      plan[:, Plan.T_FROM_CURRENT_EULER][:, 2],
+      plan[:, Plan.ORIENTATION_RATE][:, 2],
+      ModelConstants.T_IDXS,
+      v_ego,
+      DT_MDL,
+    )
 
   desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
   if v_ego > MIN_LAT_CONTROL_SPEED:
@@ -184,25 +200,16 @@ class ModelState:
     self.frame_skip = ModelConstants.MODEL_RUN_FREQ // ModelConstants.MODEL_CONTEXT_FREQ
     input_devices = get_tg_input_devices(PROCESS_NAME, usbgpu)
     self.WARP_DEV, self.QUEUE_DEV = input_devices["WARP_DEV"], input_devices["QUEUE_DEV"]
-    tensor_inputs = jits.get("tensor_inputs")
-    if tensor_inputs is None:
-      tensor_inputs = make_tensor_inputs(self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV)
-    self.npy, npy_tensors = make_npy_inputs(self.policy_input_shapes)
-    self.input_queues = {**tensor_inputs, **npy_tensors}
+    self.input_queues, self.npy = make_input_queues(
+      self.vision_input_shapes, self.policy_input_shapes, self.frame_skip, device=self.QUEUE_DEV
+    )
     self.full_frames: dict[str, Tensor] = {}
     self._blob_cache: dict[tuple[str, int], Tensor] = {}
     self.parser = Parser()
     self.frame_buf_params = {key: get_nv12_info(cam_w, cam_h) for key in ("img", "big_img")}
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
-
-    camera_jit = jits[(cam_w, cam_h)]
-    self.split_warp_layout = "run_policy" in jits and not isinstance(camera_jit, dict)
-    if self.split_warp_layout:
-      self.run_policy = jits["run_policy"]
-      self.warp_enqueue = camera_jit
-    else:
-      self.run_policy = camera_jit["run_policy"]
-      self.warp_enqueue = camera_jit["warp_enqueue"]
+    self.run_policy = jits["run_policy"]
+    self.warp_enqueue = jits[(cam_w, cam_h)]
 
   def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
     return {key: model_outputs[np.newaxis, value] for key, value in output_slices.items()}
@@ -225,25 +232,20 @@ class ModelState:
     self.npy["tfm"][:, :] = transforms["img"][:, :]
     self.npy["big_tfm"][:, :] = transforms["big_img"][:, :]
 
-    if self.split_warp_layout:
-      img, big_img = self.warp_enqueue(
-        **{key: self.input_queues[key] for key in WARP_INPUTS},
-        frame=self.full_frames["img"],
-        big_frame=self.full_frames["big_img"],
-      )
-      if prepare_only:
-        return None
-      policy_inputs = {key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues}
-      vision_output, policy_output, off_policy_output = self.run_policy(**policy_inputs, img=img, big_img=big_img)
-    else:
-      if prepare_only:
-        self.warp_enqueue(**self.input_queues, frame=self.full_frames["img"], big_frame=self.full_frames["big_img"])
-        return None
-      vision_output, policy_output, off_policy_output = self.run_policy(
-        **self.input_queues,
-        frame=self.full_frames["img"],
-        big_frame=self.full_frames["big_img"],
-      )
+    img, big_img = self.warp_enqueue(
+      **{key: self.input_queues[key] for key in WARP_INPUTS},
+      frame=self.full_frames["img"],
+      big_frame=self.full_frames["big_img"],
+    )
+
+    if prepare_only:
+      return None
+
+    vision_output, policy_output, off_policy_output = self.run_policy(
+      **{key: self.input_queues[key] for key in POLICY_INPUTS if key in self.input_queues},
+      img=img,
+      big_img=big_img,
+    )
 
     vision_output = vision_output.numpy().flatten()
     policy_output = policy_output.numpy().flatten()

selfdrive/selfdrived/selfdrived.py

@@ -18,6 +18,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.gps import get_gps_location_service
 
 from openpilot.selfdrive.car.car_specific import CarSpecificEvents
+from openpilot.selfdrive.car.cruise_state import should_flag_cruise_mismatch
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from openpilot.selfdrive.selfdrived.events import Events, ET
 from openpilot.selfdrive.selfdrived.helpers import ExcessiveActuationCheck
@@ -463,7 +464,8 @@ class SelfdriveD:
         self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise
       )
       effective_pcm_cruise = self.CP.pcmCruise or preap_software_cruise
-      cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not effective_pcm_cruise) and not pacifica_hybrid_aol
+      cruise_mismatch = should_flag_cruise_mismatch(self.CP, CS.cruiseState.enabled, self.enabled,
+                                                    effective_pcm_cruise) and not pacifica_hybrid_aol
       self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
       if self.cruise_mismatch_counter > int(6. / DT_CTRL):
         self.events.add(EventName.cruiseMismatch)