upstream

scripts/laptop_device_build.sh

@@ -289,9 +289,7 @@ scrub_mixed_arch_artifacts() {
     "${ROOT_DIR}/common/transformations/transformations.o" \
     "${ROOT_DIR}/common/transformations/transformations.so" \
     "${ROOT_DIR}/selfdrive/modeld/models/commonmodel_pyx.o" \
-    "${ROOT_DIR}/selfdrive/modeld/models/commonmodel_pyx.so" \
-    "${ROOT_DIR}/starpilot/tinygrad_modeld/models/commonmodel_pyx.o" \
-    "${ROOT_DIR}/starpilot/tinygrad_modeld/models/commonmodel_pyx.so"
+    "${ROOT_DIR}/selfdrive/modeld/models/commonmodel_pyx.so"
 }
 
 sync_sysroot_from_device() {

selfdrive/locationd/lagd.py

@@ -29,9 +29,25 @@ MAX_LAG = 1.0
 MAX_LAG_STD = 0.1
 MAX_LAT_ACCEL = 2.0
 MAX_LAT_ACCEL_DIFF = 0.6
+MIN_LAT_ACCEL_RANGE = 0.5
 MIN_CONFIDENCE = 0.7
 CORR_BORDER_OFFSET = 5
 LAG_CANDIDATE_CORR_THRESHOLD = 0.9
+SMOOTH_K = 5
+SMOOTH_SIGMA = 1.0
+
+
+def masked_symmetric_moving_average(x: np.ndarray, mask: np.ndarray, k: int, sigma: float) -> np.ndarray:
+  assert k >= 1 and k % 2 == 1, "k must be positive and odd"
+  pad = k // 2
+  i = np.arange(k) - pad
+  w = np.exp(-0.5 * (i / sigma) ** 2)
+  w /= w.sum()
+  xp = np.pad(x * mask, pad, mode="edge")
+  mp = np.pad(mask, pad, mode="edge")
+  num = np.convolve(xp, w, mode="valid")
+  den = np.convolve(mp, w, mode="valid")
+  return np.divide(num, den, out=np.full_like(num, np.nan, dtype=np.float64), where=den != 0)
 
 
 def masked_normalized_cross_correlation(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, n: int):
@@ -297,11 +313,14 @@ class LateralLagEstimator:
 
     times, desired, actual, okay = self.points.get()
     # check if there are any new valid data points since the last update
-    is_valid = self.points_valid()
+    is_valid = self.points_valid() and (actual.max() - actual.min() >= MIN_LAT_ACCEL_RANGE)
     if self.last_estimate_t != 0 and times[0] <= self.last_estimate_t:
       new_values_start_idx = next(-i for i, t in enumerate(reversed(times)) if t <= self.last_estimate_t)
       is_valid = is_valid and not (new_values_start_idx == 0 or not np.any(okay[new_values_start_idx:]))
 
+    desired = masked_symmetric_moving_average(desired, okay, SMOOTH_K, SMOOTH_SIGMA)
+    actual = masked_symmetric_moving_average(actual, okay, SMOOTH_K, SMOOTH_SIGMA)
+
     delay, corr, confidence = self.actuator_delay(desired, actual, okay, self.dt, MAX_LAG)
     if corr < self.min_ncc or confidence < self.min_confidence or not is_valid:
       return
@@ -312,16 +331,19 @@ class LateralLagEstimator:
   @staticmethod
   def actuator_delay(expected_sig: np.ndarray, actual_sig: np.ndarray, mask: np.ndarray, dt: float, max_lag: float) -> tuple[float, float, float]:
     assert len(expected_sig) == len(actual_sig)
-    max_lag_samples = int(max_lag / dt)
-    padded_size = fft_next_good_size(len(expected_sig) + max_lag_samples)
+    max_lag_samples = int(round(max_lag / dt))
+    one_sec_samples = int(round(1.0 / dt))
+    padded_size = fft_next_good_size(len(expected_sig) + max(max_lag_samples, one_sec_samples))
 
     ncc = masked_normalized_cross_correlation(expected_sig, actual_sig, mask, padded_size)
 
     # only consider lags from 0 to max_lag
     roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + max_lag_samples]
-    extended_roi = np.s_[roi.start - CORR_BORDER_OFFSET: roi.stop + CORR_BORDER_OFFSET]
+    threshold_roi = np.s_[len(expected_sig) - 1: len(expected_sig) - 1 + one_sec_samples]
+    confidence_roi = np.s_[threshold_roi.start - CORR_BORDER_OFFSET: threshold_roi.stop + CORR_BORDER_OFFSET]
     roi_ncc = ncc[roi]
-    extended_roi_ncc = ncc[extended_roi]
+    confidence_roi_ncc = ncc[confidence_roi]
+    threshold_roi_ncc = ncc[threshold_roi]
 
     max_corr_index = np.argmax(roi_ncc)
     corr = roi_ncc[max_corr_index]
@@ -329,8 +351,8 @@ class LateralLagEstimator:
 
     # to estimate lag confidence, gather all high-correlation candidates and see how spread they are
     # if e.g. 0.8 and 0.4 are both viable, this is an ambiguous case
-    ncc_thresh = (roi_ncc.max() - roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + roi_ncc.min()
-    good_lag_candidate_mask = extended_roi_ncc >= ncc_thresh
+    ncc_thresh = (threshold_roi_ncc.max() - threshold_roi_ncc.min()) * LAG_CANDIDATE_CORR_THRESHOLD + threshold_roi_ncc.min()
+    good_lag_candidate_mask = confidence_roi_ncc >= ncc_thresh
     good_lag_candidate_edges = np.diff(good_lag_candidate_mask.astype(int), prepend=0, append=0)
     starts, ends = np.where(good_lag_candidate_edges == 1)[0], np.where(good_lag_candidate_edges == -1)[0] - 1
     run_idx = np.searchsorted(starts, max_corr_index + CORR_BORDER_OFFSET, side='right') - 1

selfdrive/locationd/locationd.py

@@ -28,6 +28,9 @@ INPUT_INVALID_LIMIT = 2.0 # 1 (camodo) / 9 (sensor) bad input[s] ignored
 INPUT_INVALID_RECOVERY = 10.0 # ~10 secs to resume after exceeding allowed bad inputs by one
 POSENET_STD_INITIAL_VALUE = 10.0
 POSENET_STD_HIST_HALF = 20
+CAM_ODO_POSE_DELAY = 0.1 # dependent on the vision model context frames and temporal frequency (current model is 5 fps with 2 context frames)
+CAM_ODO_ROT_STD_MULT = 10
+CAM_ODO_TRANS_STD_MULT = 4
 
 
 def calculate_invalid_input_decay(invalid_limit, recovery_time, frequency):
@@ -155,6 +158,8 @@ class LocationEstimator:
         self.device_from_calib = rot_from_euler(calib)
 
     elif which == "cameraOdometry":
+      # camera odometry is delayed depending on the model context frames and temporal frequency
+      t = msg.timestampEof * 1e-9 - CAM_ODO_POSE_DELAY
       if not self._validate_timestamp(t):
         return HandleLogResult.TIMING_INVALID
 
@@ -177,8 +182,8 @@ class LocationEstimator:
       self.posenet_stds[-1] = trans_calib_std[0]
 
       # Multiply by N to avoid to high certainty in kalman filter because of temporally correlated noise
-      rot_calib_std *= 10
-      trans_calib_std *= 2
+      rot_calib_std *= CAM_ODO_ROT_STD_MULT
+      trans_calib_std *= CAM_ODO_TRANS_STD_MULT
 
       rot_device_std = rotate_std(self.device_from_calib, rot_calib_std)
       trans_device_std = rotate_std(self.device_from_calib, trans_calib_std)

selfdrive/locationd/models/pose_kf.py

@@ -48,13 +48,13 @@ class PoseKalman(KalmanFilter):
   # process noise
   Q = np.diag([0.001**2, 0.001**2, 0.001**2,
                0.01**2, 0.01**2, 0.01**2,
-               0.1**2, 0.1**2, 0.1**2,
+               0.085**2, 0.085**2, 0.085**2,
                (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
                3**2, 3**2, 3**2,
                0.005**2, 0.005**2, 0.005**2])
 
   obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]),
-               ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]),
+               ObservationKind.PHONE_ACCEL: np.diag([0.75**2, 0.75**2, 0.75**2]),
                ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]),
                ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])}
 

selfdrive/locationd/test/test_lagd.py

@@ -19,8 +19,8 @@ DT = 0.05
 def process_messages(estimator, lag_frames, n_frames, vego=20.0, rejection_threshold=0.0):
   for i in range(n_frames):
     t = i * estimator.dt
-    desired_la = np.cos(10 * t) * 0.1
-    actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.1
+    desired_la = np.cos(10 * t) * 0.3
+    actual_la = np.cos(10 * (t - lag_frames * estimator.dt)) * 0.3
 
     # if sample is masked out, set it to desired value (no lag)
     rejected = random.uniform(0, 1) < rejection_threshold

selfdrive/locationd/test/test_locationd_scenarios.py

@@ -2,6 +2,7 @@ import numpy as np
 from collections import defaultdict
 from enum import Enum
 
+from openpilot.selfdrive.locationd.lagd import masked_symmetric_moving_average
 from openpilot.tools.lib.logreader import LogReader
 from openpilot.selfdrive.test.process_replay.migration import migrate_all
 from openpilot.selfdrive.test.process_replay.process_replay import replay_process_with_name
@@ -15,6 +16,7 @@ SELECT_COMPARE_FIELDS = {
   'inputs_flag': ['inputsOK'],
   'sensors_flag': ['sensorsOK'],
 }
+SMOOTH_FIELDS = ['yaw_rate', 'roll']
 JUNK_IDX = 100
 CONSISTENT_SPIKES_COUNT = 10
 
@@ -32,6 +34,8 @@ class Scenario(Enum):
 
 
 def get_select_fields_data(logs):
+  def sig_smooth(signal):
+    return masked_symmetric_moving_average(signal, np.ones_like(signal), 5, 1.0)
   def get_nested_keys(msg, keys):
     val = None
     for key in keys:
@@ -44,6 +48,8 @@ def get_select_fields_data(logs):
       data[key].append(get_nested_keys(msg, fields))
   for key in data:
     data[key] = np.array(data[key][JUNK_IDX:], dtype=float)
+    if key in SMOOTH_FIELDS:
+      data[key] = sig_smooth(data[key])
   return data
 
 
@@ -110,7 +116,7 @@ class TestLocationdScenarios:
     """
     orig_data, replayed_data = run_scenarios(Scenario.BASE, self.logs)
     assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
 
   def test_gyro_off(self):
     """
@@ -135,7 +141,7 @@ class TestLocationdScenarios:
     """
     orig_data, replayed_data = run_scenarios(Scenario.GYRO_SPIKE_MIDWAY, self.logs)
     assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
     assert np.all(replayed_data['inputs_flag'] == orig_data['inputs_flag'])
     assert np.all(replayed_data['sensors_flag'] == orig_data['sensors_flag'])
 
@@ -169,7 +175,7 @@ class TestLocationdScenarios:
     """
     orig_data, replayed_data = run_scenarios(Scenario.ACCEL_SPIKE_MIDWAY, self.logs)
     assert np.allclose(orig_data['yaw_rate'], replayed_data['yaw_rate'], atol=np.radians(0.35))
-    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.55))
+    assert np.allclose(orig_data['roll'], replayed_data['roll'], atol=np.radians(0.35))
 
   def test_single_timing_spike(self):
     """

selfdrive/modeld/constants.py

@@ -13,6 +13,12 @@ class ModelConstants:
   META_T_IDXS = [2., 4., 6., 8., 10.]
 
   # model inputs constants
+  MODEL_FREQ = 20
+  HISTORY_FREQ = 5
+  HISTORY_LEN_SECONDS = 5
+  TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
+  FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
+  INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
   N_FRAMES = 2
   MODEL_RUN_FREQ = 20
   MODEL_CONTEXT_FREQ = 5 # "model_trained_fps"

selfdrive/modeld/dmonitoringmodeld.py

@@ -3,6 +3,7 @@ import os
 from openpilot.system.hardware import TICI
 os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
 from tinygrad.tensor import Tensor
+from tinygrad.dtype import dtypes
 import time
 import pickle
 import numpy as np
@@ -15,63 +16,60 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
 from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
-from openpilot.common.file_chunker import read_file_chunked
 from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
+from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
 
 PROCESS_NAME = "selfdrive.modeld.dmonitoringmodeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
-MODEL_PKL_PATH = Path(__file__).parent / 'models/dmonitoring_model_tinygrad.pkl'
-METADATA_PATH = Path(__file__).parent / 'models/dmonitoring_model_metadata.pkl'
-MODELS_DIR = Path(__file__).parent / 'models'
+MODEL_PKL_PATH = Path(__file__).parent / "models/dmonitoring_model_tinygrad.pkl"
+METADATA_PATH = Path(__file__).parent / "models/dmonitoring_model_metadata.pkl"
+
 
 class ModelState:
   inputs: dict[str, np.ndarray]
   output: np.ndarray
 
-  def __init__(self):
+  def __init__(self, cl_ctx: CLContext):
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
       self.input_shapes = model_metadata['input_shapes']
       self.output_slices = model_metadata['output_slices']
 
+    self.frame = MonitoringModelFrame(cl_ctx)
     self.numpy_inputs = {
       'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
     }
 
-    self.warp_inputs_np = {'transform': np.zeros((3,3), dtype=np.float32)}
-    self.warp_inputs = {k: Tensor(v, device='NPY') for k,v in self.warp_inputs_np.items()}
-    self.frame_buf_params = None
-    self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
-    self._blob_cache : dict[int, Tensor] = {}
-    self.image_warp = None
-    self.model_run = pickle.loads(read_file_chunked(str(MODEL_PKL_PATH)))
+    self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.numpy_inputs.items()}
+    with open(MODEL_PKL_PATH, "rb") as f:
+      self.model_run = pickle.load(f)
 
   def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
-    self.numpy_inputs['calib'][0,:] = calib
+    self.numpy_inputs['calib'][0, :] = calib
 
     t1 = time.perf_counter()
 
-    if self.image_warp is None:
-      self.frame_buf_params = get_nv12_info(buf.width, buf.height)
-      warp_path = MODELS_DIR / f'dm_warp_{buf.width}x{buf.height}_tinygrad.pkl'
-      with open(warp_path, "rb") as f:
-        self.image_warp = pickle.load(f)
-    ptr = buf.data.ctypes.data
-    # There is a ringbuffer of imgs, just cache tensors pointing to all of them
-    if ptr not in self._blob_cache:
-      self._blob_cache[ptr] = Tensor.from_blob(ptr, (self.frame_buf_params[3],), dtype='uint8')
-
-    self.warp_inputs_np['transform'][:] = transform[:]
-    self.tensor_inputs['input_img'] = self.image_warp(self._blob_cache[ptr], self.warp_inputs['transform']).realize()
+    input_img_cl = self.frame.prepare(buf, transform.flatten())
+    if TICI:
+      if 'input_img' not in self.tensor_inputs:
+        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(
+          input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8
+        )
+    else:
+      self.tensor_inputs['input_img'] = Tensor(
+        self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8
+      ).realize()
 
     output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
 
     t2 = time.perf_counter()
     return output, t2 - t1
 
+
 def slice_outputs(model_outputs, output_slices):
-  return  {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
+  return {k: model_outputs[np.newaxis, v] for k, v in output_slices.items()}
+
 
 def parse_model_output(model_output):
   parsed = {}
@@ -80,23 +78,22 @@ def parse_model_output(model_output):
     face_descs = model_output[f'face_descs_{ds_suffix}']
     parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
     parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
-    for key in ['face_prob', 'left_eye_prob', 'right_eye_prob','left_blink_prob', 'right_blink_prob', 'sunglasses_prob', 'using_phone_prob']:
+    for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']:
       parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
   return parsed
 
+
 def fill_driver_data(msg, model_output, ds_suffix):
   msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
   msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
   msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
   msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
   msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
-  msg.leftEyeProb = model_output[f'left_eye_prob_{ds_suffix}'][0, 0].item()
-  msg.rightEyeProb = model_output[f'right_eye_prob_{ds_suffix}'][0, 0].item()
-  msg.leftBlinkProb = model_output[f'left_blink_prob_{ds_suffix}'][0, 0].item()
-  msg.rightBlinkProb = model_output[f'right_blink_prob_{ds_suffix}'][0, 0].item()
-  msg.sunglassesProb = model_output[f'sunglasses_prob_{ds_suffix}'][0, 0].item()
+  msg.eyesVisibleProb = model_output[f'eyes_visible_prob_{ds_suffix}'][0, 0].item()
+  msg.eyesClosedProb = model_output[f'eyes_closed_prob_{ds_suffix}'][0, 0].item()
   msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
 
+
 def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
   msg = messaging.new_message('driverStateV2', valid=True)
   ds = msg.driverStateV2
@@ -113,11 +110,12 @@ def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_t
 def main():
   config_realtime_process(7, 5)
 
-  model = ModelState()
+  cl_context = CLContext()
+  model = ModelState(cl_context)
   cloudlog.warning("models loaded, dmonitoringmodeld starting")
 
   cloudlog.warning("connecting to driver stream")
-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True)
+  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True, cl_context)
   while not vipc_client.connect(False):
     time.sleep(0.1)
   assert vipc_client.is_connected()

selfdrive/modeld/fill_model_msg.py

@@ -3,11 +3,25 @@ import capnp
 import numpy as np
 from cereal import log
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan, Meta
+from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 ConfidenceClass = log.ModelDataV2.ConfidenceClass
 
+# Return curvature for lateral action. If the model outputs desired_curvature and we're not in mlsim mode,
+# use it directly; otherwise derive from the plan using yaw and yaw-rate.
+def get_curvature_from_output(output: dict, plan: np.ndarray, v_ego: float, lat_action_t: float, mlsim: bool) -> float:
+  if not mlsim:
+    desired = output.get('desired_curvature')
+    if desired is not None:
+      return float(desired[0, 0])
+
+  # Use yaw (index 2) and yaw_rate (index 2)
+  theta = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
+  theta_dot = plan[:, Plan.ORIENTATION_RATE][:, 2]
+  return float(get_curvature_from_plan(theta, theta_dot, ModelConstants.T_IDXS, v_ego, lat_action_t))
+
 
 class PublishState:
   def __init__(self):
@@ -82,15 +96,35 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   modelV2.timestampEof = timestamp_eof
   modelV2.modelExecutionTime = model_execution_time
 
+  # normalize plan tensors to (IDX_N, WIDTH)
+  plan_arr = net_output_data['plan'][0]
+  plan_stds_arr = net_output_data['plan_stds'][0]
+
   # plan
-  fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.POSITION].T, *net_output_data['plan_stds'][0,:,Plan.POSITION].T)
-  fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.VELOCITY].T)
-  fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ACCELERATION].T)
-  fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.T_FROM_CURRENT_EULER].T)
-  fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
+  fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *plan_arr[:,Plan.POSITION].T, *plan_stds_arr[:,Plan.POSITION].T)
+  fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *plan_arr[:,Plan.VELOCITY].T)
+  fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *plan_arr[:,Plan.ACCELERATION].T)
+  fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *plan_arr[:,Plan.T_FROM_CURRENT_EULER].T)
+  fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *plan_arr[:,Plan.ORIENTATION_RATE].T)
+
+  # temporal pose
+  try:
+    temporal_pose = modelV2.temporalPose
+    if 'sim_pose' in net_output_data:
+      temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+      temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
+      temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+      temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
+    else:
+      temporal_pose.trans = plan_arr[0,Plan.VELOCITY].tolist()
+      temporal_pose.transStd = plan_stds_arr[0,Plan.VELOCITY].tolist()
+      temporal_pose.rot = plan_arr[0,Plan.ORIENTATION_RATE].tolist()
+      temporal_pose.rotStd = plan_stds_arr[0,Plan.ORIENTATION_RATE].tolist()
+  except AttributeError:
+    pass
 
   # poly path
-  fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
+  fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *plan_arr[:,Plan.POSITION].T)
 
   # action
   modelV2.action = action
@@ -99,12 +133,30 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   LINE_T_IDXS: list[float] = []
 
   # lane lines
-  modelV2.init('laneLines', 4)
-  for i in range(4):
-    lane_line = modelV2.laneLines[i]
-    fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
-  modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
-  modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+  has_lane_lines = all(k in net_output_data for k in ("lane_lines", "lane_lines_stds", "lane_lines_prob"))
+  modelV2.init('laneLines', 6)
+  if has_lane_lines:
+    for i in range(6):
+      lane_line = modelV2.laneLines[i]
+      if i < 4:
+        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+      elif i == 4:
+        leftLane_x = 0.5 * (net_output_data['lane_lines'][0,0,:,0] + net_output_data['lane_lines'][0,1,:,0])
+        leftLane_y = 0.5 * (net_output_data['lane_lines'][0,0,:,1] + net_output_data['lane_lines'][0,1,:,1])
+        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y)
+      elif i == 5:
+        rightLane_x = 0.5 * (net_output_data['lane_lines'][0,2,:,0] + net_output_data['lane_lines'][0,3,:,0])
+        rightLane_y = 0.5 * (net_output_data['lane_lines'][0,2,:,1] + net_output_data['lane_lines'][0,3,:,1])
+        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y)
+    modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
+    modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+  else:
+    x_vals = np.array(ModelConstants.X_IDXS)
+    zeros = np.zeros_like(x_vals, dtype=np.float32)
+    for i in range(6):
+      fill_xyzt(modelV2.laneLines[i], LINE_T_IDXS, x_vals, zeros, zeros)
+    modelV2.laneLineStds = [0.0] * 4
+    modelV2.laneLineProbs = [0.0] * 4
 
   fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
 
@@ -123,13 +175,6 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
     lead.prob = net_output_data['lead_prob'][0,i].tolist()
     lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
 
-  # temporal pose
-  temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['plan'][0,0,Plan.VELOCITY].tolist()
-  temporal_pose.transStd = net_output_data['plan_stds'][0,0,Plan.VELOCITY].tolist()
-  temporal_pose.rot = net_output_data['plan'][0,0,Plan.ORIENTATION_RATE].tolist()
-  temporal_pose.rotStd = net_output_data['plan_stds'][0,0,Plan.ORIENTATION_RATE].tolist()
-
   # meta
   meta = modelV2.meta
   meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()

selfdrive/modeld/modeld.py

@@ -1,11 +1,7 @@
 #!/usr/bin/env python3
 import os
 from openpilot.system.hardware import TICI
-os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
-USBGPU = "USBGPU" in os.environ
-if USBGPU:
-  os.environ['DEV'] = 'AMD'
-  os.environ['AMD_IFACE'] = 'USB'
+os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
 from tinygrad.tensor import Tensor
 from tinygrad.dtype import dtypes
 import time
@@ -14,84 +10,82 @@ import numpy as np
 import cereal.messaging as messaging
 from cereal import car, log
 from pathlib import Path
+from setproctitle import setproctitle
 from cereal.messaging import PubMaster, SubMaster
 from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
-from opendbc.car.car_helpers import get_demo_car_params
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
+from openpilot.system import sentry
+from opendbc.car.car_helpers import get_demo_car_params
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value, get_curvature_from_plan
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
-from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
+from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
-
-from openpilot.starpilot.common.starpilot_variables import MODELS_PATH, get_starpilot_toggles
+from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, MODELS_PATH, params_memory
 
 
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
-BUILTIN_MODEL_DIR = Path(__file__).parent / "models"
-BUILTIN_VISION_PKL_PATH = BUILTIN_MODEL_DIR / "driving_vision_tinygrad.pkl"
-BUILTIN_POLICY_PKL_PATH = BUILTIN_MODEL_DIR / "driving_policy_tinygrad.pkl"
-BUILTIN_VISION_METADATA_PATH = BUILTIN_MODEL_DIR / "driving_vision_metadata.pkl"
-BUILTIN_POLICY_METADATA_PATH = BUILTIN_MODEL_DIR / "driving_policy_metadata.pkl"
+BUILTIN_MODEL_KEY = "sc2"
+BUILTIN_MODEL_ALIASES = {BUILTIN_MODEL_KEY, "sc"}
 
 
-def _decode_param(value) -> str:
-  if isinstance(value, bytes):
-    return value.decode("utf-8")
-  return str(value or "")
-
-
-def _resolve_model_paths(params: Params) -> tuple[str, dict[str, Path]]:
-  model_key = _decode_param(params.get("DrivingModel")).strip()
-  candidates = [model_key] if model_key else []
-  if model_key.endswith("_default"):
-    candidates.append(model_key[:-8])
-
-  for candidate in candidates:
-    files = {
-      "vision_pkl": MODELS_PATH / f"{candidate}_driving_vision_tinygrad.pkl",
-      "policy_pkl": MODELS_PATH / f"{candidate}_driving_policy_tinygrad.pkl",
-      "vision_meta": MODELS_PATH / f"{candidate}_driving_vision_metadata.pkl",
-      "policy_meta": MODELS_PATH / f"{candidate}_driving_policy_metadata.pkl",
-    }
-    if all(path.is_file() for path in files.values()):
-      return candidate, files
-
-  return "builtin", {
-    "vision_pkl": BUILTIN_VISION_PKL_PATH,
-    "policy_pkl": BUILTIN_POLICY_PKL_PATH,
-    "vision_meta": BUILTIN_VISION_METADATA_PATH,
-    "policy_meta": BUILTIN_POLICY_METADATA_PATH,
-  }
-
 LAT_SMOOTH_SECONDS = 0.0
 LONG_SMOOTH_SECONDS = 0.3
 MIN_LAT_CONTROL_SPEED = 0.3
 
 
+def _get_param_str(params: Params, key: str, default: str = "") -> str:
+  try:
+    val = params.get(key)
+  except Exception:
+    return default
+  if val is None:
+    return default
+  if isinstance(val, bytes):
+    try:
+      return val.decode("utf-8")
+    except Exception:
+      return default
+  if isinstance(val, (dict, list)):
+    return default
+  return str(val)
+
+
+def _canonical_model_id(model_id: str) -> str:
+  key = (model_id or "").strip().lower()
+  return BUILTIN_MODEL_KEY if key in BUILTIN_MODEL_ALIASES else key
+
+
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                          lat_action_t: float, long_action_t: float, v_ego: float) -> log.ModelDataV2.Action:
+                          lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool, starpilot_toggles) -> log.ModelDataV2.Action:
     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=long_action_t)
+    if 'planplus' in model_output:
+      recovery_power = getattr(starpilot_toggles, "recovery_power", 1.0)
+      plan = plan + recovery_power * model_output['planplus'][0]
+      cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {recovery_power}")
+    desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
+                                                                 plan[:,Plan.ACCELERATION][:,0],
+                                                                 ModelConstants.T_IDXS,
+                                                                 action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
 
-    desired_curvature = get_curvature_from_plan(plan[:,Plan.T_FROM_CURRENT_EULER][:,2],
-                                                plan[:,Plan.ORIENTATION_RATE][:,2],
-                                                ModelConstants.T_IDXS,
-                                                v_ego,
-                                                lat_action_t)
+    if is_v9:
+      # V9: use desired_curvature if present; otherwise do NOT fall back to plan
+      if 'desired_curvature' in model_output:
+        desired_curvature = float(model_output['desired_curvature'][0, 0])
+      else:
+        desired_curvature = prev_action.desiredCurvature
+    else:
+      desired_curvature = get_curvature_from_output(model_output, plan, v_ego, lat_action_t, mlsim=mlsim)
     if v_ego > MIN_LAT_CONTROL_SPEED:
       desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
     else:
@@ -110,93 +104,202 @@ class FrameMeta:
     if vipc is not None:
       self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
 
-class InputQueues:
-  def __init__ (self, model_fps, env_fps, n_frames_input):
-    assert env_fps % model_fps == 0
-    assert env_fps >= model_fps
-    self.model_fps = model_fps
-    self.env_fps = env_fps
-    self.n_frames_input = n_frames_input
-
-    self.dtypes = {}
-    self.shapes = {}
-    self.q = {}
-
-  def update_dtypes_and_shapes(self, input_dtypes, input_shapes) -> None:
-    self.dtypes.update(input_dtypes)
-    if self.env_fps == self.model_fps:
-      self.shapes.update(input_shapes)
-    else:
-      for k in input_shapes:
-        shape = list(input_shapes[k])
-        if 'img' in k:
-          n_channels = shape[1] // self.n_frames_input
-          shape[1] = (self.env_fps // self.model_fps + (self.n_frames_input - 1)) * n_channels
-        else:
-          shape[1] = (self.env_fps // self.model_fps) * shape[1]
-        self.shapes[k] = tuple(shape)
-
-  def reset(self) -> None:
-    self.q = {k: np.zeros(self.shapes[k], dtype=self.dtypes[k]) for k in self.dtypes.keys()}
-
-  def enqueue(self, inputs:dict[str, np.ndarray]) -> None:
-    for k in inputs.keys():
-      if inputs[k].dtype != self.dtypes[k]:
-        raise ValueError(f'supplied input <{k}({inputs[k].dtype})> has wrong dtype, expected {self.dtypes[k]}')
-      input_shape = list(self.shapes[k])
-      input_shape[1] = -1
-      single_input = inputs[k].reshape(tuple(input_shape))
-      sz = single_input.shape[1]
-      self.q[k][:,:-sz] = self.q[k][:,sz:]
-      self.q[k][:,-sz:] = single_input
-
-  def get(self, *names) -> dict[str, np.ndarray]:
-    if self.env_fps == self.model_fps:
-      return {k: self.q[k] for k in names}
-    else:
-      out = {}
-      for k in names:
-        shape = self.shapes[k]
-        if 'img' in k:
-          n_channels = shape[1] // (self.env_fps // self.model_fps + (self.n_frames_input - 1))
-          out[k] = np.concatenate([self.q[k][:, s:s+n_channels] for s in np.linspace(0, shape[1] - n_channels, self.n_frames_input, dtype=int)], axis=1)
-        elif 'pulse' in k:
-          # any pulse within interval counts
-          out[k] = self.q[k].reshape((shape[0], shape[1] * self.model_fps // self.env_fps, self.env_fps // self.model_fps, -1)).max(axis=2)
-        else:
-          idxs = np.arange(-1, -shape[1], -self.env_fps // self.model_fps)[::-1]
-          out[k] = self.q[k][:, idxs]
-      return out
-
 class ModelState:
   frames: dict[str, DrivingModelFrame]
   inputs: dict[str, np.ndarray]
   output: np.ndarray
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
 
-  def __init__(self, context: CLContext, model_paths: dict[str, Path]):
-    with open(model_paths["vision_meta"], 'rb') as f:
+  def _build_policy_inputs(self, input_shapes: dict[str, tuple[int, ...]]) -> tuple[dict[str, np.ndarray], str | None]:
+    numpy_inputs: dict[str, np.ndarray] = {}
+
+    # Always-supported inputs (if model expects them)
+    desire_key_init = next((k for k in input_shapes if k.startswith('desire')), None)
+    if desire_key_init:
+      numpy_inputs[desire_key_init] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
+    if 'traffic_convention' in input_shapes:
+      numpy_inputs['traffic_convention'] = np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32)
+    if 'features_buffer' in input_shapes:
+      numpy_inputs['features_buffer'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN,  ModelConstants.FEATURE_LEN), dtype=np.float32)
+
+    # Optional inputs for non-v11 (and some v10/v9 variants)
+    # Lateral control params
+    if 'lateral_control_params' in input_shapes:
+      numpy_inputs['lateral_control_params'] = np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32)
+
+    # Previous desired curvature: handle both singular and plural key names across model versions
+    prev_desired_curv_key = None
+    if 'prev_desired_curv' in input_shapes:
+      prev_desired_curv_key = 'prev_desired_curv'
+      numpy_inputs['prev_desired_curv'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
+    elif 'prev_desired_curvs' in input_shapes:
+      prev_desired_curv_key = 'prev_desired_curvs'
+      numpy_inputs['prev_desired_curvs'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
+
+    return numpy_inputs, prev_desired_curv_key
+
+  def __init__(self, context: CLContext):
+    # Dynamically build paths based on current model ID
+    params = Params()
+    model_id_raw = _get_param_str(params, "Model")
+    if not model_id_raw:
+      model_id_raw = _get_param_str(params, "DrivingModel", BUILTIN_MODEL_KEY)
+    model_id = _canonical_model_id(model_id_raw)
+
+    model_version = _get_param_str(params, "ModelVersion")
+    if not model_version:
+      model_version = _get_param_str(params, "DrivingModelVersion")
+
+    model_dir = MODELS_PATH
+    use_builtin_model = model_id == BUILTIN_MODEL_KEY
+    model_download_id = model_id
+    if use_builtin_model and (_canonical_model_id(_get_param_str(params, "Model")) != model_id or
+                              _canonical_model_id(_get_param_str(params, "DrivingModel")) != model_id):
+      params.put("Model", model_id)
+      params.put("DrivingModel", model_id)
+
+    if use_builtin_model and not model_version:
+      model_version = "v11"
+      params.put("ModelVersion", model_version)
+      params.put("DrivingModelVersion", model_version)
+    # Use built-in files for defaults when a custom model isn't selected.
+    if use_builtin_model:
+        models_dir = Path(__file__).parent / "models"
+        VISION_PKL_PATH = models_dir / "driving_vision_tinygrad.pkl"
+        POLICY_PKL_PATH = models_dir / "driving_policy_tinygrad.pkl"
+        OFF_POLICY_PKL_PATH = models_dir / "driving_off_policy_tinygrad.pkl"
+        VISION_METADATA_PATH = models_dir / "driving_vision_metadata.pkl"
+        POLICY_METADATA_PATH = models_dir / "driving_policy_metadata.pkl"
+        OFF_POLICY_METADATA_PATH = models_dir / "driving_off_policy_metadata.pkl"
+    else:
+        VISION_PKL_PATH = model_dir / f"{model_id}_driving_vision_tinygrad.pkl"
+        POLICY_PKL_PATH = model_dir / f"{model_id}_driving_policy_tinygrad.pkl"
+        OFF_POLICY_PKL_PATH = model_dir / f"{model_id}_driving_off_policy_tinygrad.pkl"
+        VISION_METADATA_PATH = model_dir / f"{model_id}_driving_vision_metadata.pkl"
+        POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_policy_metadata.pkl"
+        OFF_POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_off_policy_metadata.pkl"
+
+    def ensure_artifact(path: Path, suffix: str | None = None, optional: bool = False) -> Path | None:
+      if path.is_file():
+        return path
+
+      if use_builtin_model:
+        if optional:
+          cloudlog.warning(f"Optional builtin model artifact missing: {path}")
+          return None
+        raise FileNotFoundError(
+          f"Missing builtin model artifact: {path}. "
+          "Rebuild model artifacts locally (./build or scons target) and deploy them."
+        )
+
+      cloudlog.error(f"Missing model artifact {path}, downloading {model_download_id}...")
+      from openpilot.starpilot.assets.model_manager import ModelManager
+      ModelManager(params, params_memory).download_model(model_download_id)
+
+      if path.is_file():
+        return path
+
+      if optional:
+        cloudlog.warning(f"Optional model artifact missing: {path}")
+        return None
+
+      raise FileNotFoundError(path)
+
+    # If ModelVersion is not set or not available, try to determine it from available model data
+    if not model_version:
+      cloudlog.warning(f"ModelVersion not available for model {model_id}, attempting to determine from model data")
+      try:
+        # Try to get version from the model versions JSON file
+        versions_file = model_dir / ".model_versions.json"
+        if versions_file.is_file():
+          import json
+          with open(versions_file, "r") as f:
+            version_map = json.load(f)
+          version_lookup_keys = [model_id]
+          if model_id and not model_id.endswith("2"):
+            version_lookup_keys.append(f"{model_id}2")
+          for key in version_lookup_keys:
+            if key in version_map:
+              model_version = version_map[key]
+              cloudlog.warning(f"Determined model version from JSON: {model_version} ({key})")
+              params.put("ModelVersion", model_version)
+              params.put("DrivingModelVersion", model_version)
+              break
+        else:
+          cloudlog.error("Model versions JSON file not found, defaulting to v8")
+          model_version = "v8"
+      except Exception as e:
+        cloudlog.error(f"Failed to determine model version: {e}, defaulting to v8")
+        model_version = "v8"
+
+    VISION_METADATA_PATH = ensure_artifact(VISION_METADATA_PATH, "driving_vision_metadata.pkl")
+    with open(VISION_METADATA_PATH, 'rb') as f:
       vision_metadata = pickle.load(f)
-      self.vision_input_shapes =  vision_metadata['input_shapes']
-      self.vision_input_names = list(self.vision_input_shapes.keys())
-      self.vision_output_slices = vision_metadata['output_slices']
-      vision_output_size = vision_metadata['output_shapes']['outputs'][1]
+    self.vision_input_shapes =  vision_metadata['input_shapes']
+    self.vision_input_names = list(self.vision_input_shapes.keys())
+    self.vision_output_slices = vision_metadata['output_slices']
+    vision_output_size = vision_metadata['output_shapes']['outputs'][1]
 
-    with open(model_paths["policy_meta"], 'rb') as f:
+    POLICY_METADATA_PATH = ensure_artifact(POLICY_METADATA_PATH, "driving_policy_metadata.pkl")
+    with open(POLICY_METADATA_PATH, 'rb') as f:
       policy_metadata = pickle.load(f)
-      self.policy_input_shapes =  policy_metadata['input_shapes']
-      self.policy_output_slices = policy_metadata['output_slices']
-      policy_output_size = policy_metadata['output_shapes']['outputs'][1]
+    self.policy_input_shapes =  policy_metadata['input_shapes']
+    self.policy_output_slices = policy_metadata['output_slices']
+    policy_output_size = policy_metadata['output_shapes']['outputs'][1]
+    # Add policy_generation attribute after loading policy_metadata
+    self.policy_generation = model_version or "v8"
+    self.is_v11 = (self.policy_generation == "v11")
+    self.is_v10 = (self.policy_generation == "v10")
+    self.is_v12 = (self.policy_generation == "v12")
+    self.is_v9 = (self.policy_generation == "v9")
+    self.mlsim = (self.policy_generation in ("v8", "v10", "v11", "v12"))
 
-    self.frames = {name: DrivingModelFrame(context, ModelConstants.MODEL_RUN_FREQ//ModelConstants.MODEL_CONTEXT_FREQ) for name in self.vision_input_names}
+    self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
 
-    # policy inputs
-    self.numpy_inputs = {k: np.zeros(self.policy_input_shapes[k], dtype=np.float32) for k in self.policy_input_shapes}
-    self.full_input_queues = InputQueues(ModelConstants.MODEL_CONTEXT_FREQ, ModelConstants.MODEL_RUN_FREQ, ModelConstants.N_FRAMES)
-    for k in ['desire_pulse', 'features_buffer']:
-      self.full_input_queues.update_dtypes_and_shapes({k: self.numpy_inputs[k].dtype}, {k: self.numpy_inputs[k].shape})
-    self.full_input_queues.reset()
+    self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN,  ModelConstants.FEATURE_LEN), dtype=np.float32)
+    self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
+    self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
+
+
+    # policy inputs (built dynamically to support all generations)
+    self.numpy_inputs, self.prev_desired_curv_key = self._build_policy_inputs(self.policy_input_shapes)
+
+    # Off-policy model (optional)
+    self.off_policy_enabled = False
+    self.off_policy_input_shapes: dict[str, tuple[int, ...]] = {}
+    self.off_policy_output_slices: dict[str, slice] = {}
+    self.off_policy_numpy_inputs: dict[str, np.ndarray] = {}
+    self.off_policy_prev_desired_curv_key: str | None = None
+    self.off_policy_desire_key: str | None = None
+    self.off_policy_inputs: dict[str, Tensor] | None = None
+    self.off_policy_output: np.ndarray | None = None
+
+    off_policy_metadata = None
+    if self.policy_generation == "v12" or OFF_POLICY_METADATA_PATH.is_file() or OFF_POLICY_PKL_PATH.is_file():
+      resolved_off_policy_meta = ensure_artifact(OFF_POLICY_METADATA_PATH, "driving_off_policy_metadata.pkl", optional=True)
+      if resolved_off_policy_meta is not None:
+        with open(resolved_off_policy_meta, 'rb') as f:
+          off_policy_metadata = pickle.load(f)
+
+    if off_policy_metadata is not None:
+      self.off_policy_input_shapes = off_policy_metadata['input_shapes']
+      self.off_policy_output_slices = off_policy_metadata['output_slices']
+      off_policy_output_size = off_policy_metadata['output_shapes']['outputs'][1]
+      self.off_policy_numpy_inputs, self.off_policy_prev_desired_curv_key = self._build_policy_inputs(self.off_policy_input_shapes)
+      self.off_policy_desire_key = next((k for k in self.off_policy_numpy_inputs if k.startswith('desire')), None)
+      self.off_policy_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.off_policy_numpy_inputs.items()}
+      self.off_policy_output = np.zeros(off_policy_output_size, dtype=np.float32)
+      resolved_off_policy_pkl = ensure_artifact(OFF_POLICY_PKL_PATH, "driving_off_policy_tinygrad.pkl", optional=True)
+      if resolved_off_policy_pkl is not None:
+        with open(resolved_off_policy_pkl, "rb") as f:
+          self.off_policy_run = pickle.load(f)
+        self.off_policy_enabled = True
+
+    # Optional temporal buffer for previous desired curvature (allocate only if any model expects it)
+    if self.prev_desired_curv_key is not None or self.off_policy_prev_desired_curv_key is not None:
+      self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
+
 
     # img buffers are managed in openCL transform code
     self.vision_inputs: dict[str, Tensor] = {}
@@ -204,13 +307,20 @@ class ModelState:
     self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
     self.policy_output = np.zeros(policy_output_size, dtype=np.float32)
     self.parser = Parser()
+    self.off_policy_parser = Parser(ignore_missing=True)
 
-    with open(model_paths["vision_pkl"], "rb") as f:
+    VISION_PKL_PATH = ensure_artifact(VISION_PKL_PATH, "driving_vision_tinygrad.pkl")
+    with open(VISION_PKL_PATH, "rb") as f:
       self.vision_run = pickle.load(f)
 
-    with open(model_paths["policy_pkl"], "rb") as f:
+    POLICY_PKL_PATH = ensure_artifact(POLICY_PKL_PATH, "driving_policy_tinygrad.pkl")
+    with open(POLICY_PKL_PATH, "rb") as f:
       self.policy_run = pickle.load(f)
 
+  @property
+  def desire_key(self) -> str:
+    return next(key for key in self.numpy_inputs if key.startswith('desire'))
+
   def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
     parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
     return parsed_model_outputs
@@ -218,13 +328,32 @@ class ModelState:
   def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
                 inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
     # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
-    inputs['desire_pulse'][0] = 0
-    new_desire = np.where(inputs['desire_pulse'] - self.prev_desire > .99, inputs['desire_pulse'], 0)
-    self.prev_desire[:] = inputs['desire_pulse']
+    inputs[self.desire_key][0] = 0
+    new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0)
+    self.prev_desire[:] = inputs[self.desire_key]
+
+    self.full_desire[0,:-1] = self.full_desire[0,1:]
+    self.full_desire[0,-1] = new_desire
+    self.numpy_inputs[self.desire_key][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
+    if self.off_policy_enabled and self.off_policy_desire_key is not None:
+      self.off_policy_numpy_inputs[self.off_policy_desire_key][:] = self.numpy_inputs[self.desire_key]
+
+    if 'traffic_convention' in self.numpy_inputs:
+      self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
+    if self.off_policy_enabled and 'traffic_convention' in self.off_policy_numpy_inputs:
+      self.off_policy_numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
+
+    if 'lateral_control_params' in self.numpy_inputs:
+      self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
+    if self.off_policy_enabled and 'lateral_control_params' in self.off_policy_numpy_inputs:
+      self.off_policy_numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
+
+    if prepare_only:
+      return None
 
     imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
 
-    if TICI and not USBGPU:
+    if TICI:
       # The imgs tensors are backed by opencl memory, only need init once
       for key in imgs_cl:
         if key not in self.vision_inputs:
@@ -234,23 +363,52 @@ class ModelState:
         frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key])
         self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize()
 
-    if prepare_only:
-      return None
-
     self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy()
     vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
 
-    self.full_input_queues.enqueue({'features_buffer': vision_outputs_dict['hidden_state'], 'desire_pulse': new_desire})
-    for k in ['desire_pulse', 'features_buffer']:
-      self.numpy_inputs[k][:] = self.full_input_queues.get(k)[k]
-    self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
+    self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
+    self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
+    if 'features_buffer' in self.numpy_inputs:
+      self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
+    if self.off_policy_enabled and 'features_buffer' in self.off_policy_numpy_inputs:
+      self.off_policy_numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
 
     self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy()
     policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
 
-    combined_outputs_dict = {**vision_outputs_dict, **policy_outputs_dict}
+    # TODO model only uses last value now
+    if hasattr(self, 'full_prev_desired_curv') and 'desired_curvature' in policy_outputs_dict:
+      self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
+      self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
+
+      if self.prev_desired_curv_key is not None:
+        # v9/v10/v11/v12 models expect zeros for prev_desired_curv(s); others use history
+        if self.is_v9 or self.is_v10 or self.is_v11 or self.is_v12:
+          self.numpy_inputs[self.prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
+        else:
+          self.numpy_inputs[self.prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
+
+      if self.off_policy_enabled and self.off_policy_prev_desired_curv_key is not None:
+        if self.is_v9 or self.is_v12:
+          self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
+        else:
+          self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
+
+    combined_outputs_dict = {**vision_outputs_dict}
+    if self.off_policy_enabled:
+      self.off_policy_output = self.off_policy_run(**self.off_policy_inputs).contiguous().realize().uop.base.buffer.numpy()
+      off_policy_outputs_dict = self.off_policy_parser.parse_policy_outputs(
+        self.slice_outputs(self.off_policy_output, self.off_policy_output_slices)
+      )
+      off_policy_outputs_dict.pop('plan', None)
+      combined_outputs_dict = {**combined_outputs_dict, **off_policy_outputs_dict, **policy_outputs_dict}
+    else:
+      combined_outputs_dict = {**combined_outputs_dict, **policy_outputs_dict}
     if SEND_RAW_PRED:
-      combined_outputs_dict['raw_pred'] = np.concatenate([self.vision_output.copy(), self.policy_output.copy()])
+      raw_pred = [self.vision_output.copy(), self.policy_output.copy()]
+      if self.off_policy_enabled and self.off_policy_output is not None:
+        raw_pred.append(self.off_policy_output.copy())
+      combined_outputs_dict['raw_pred'] = np.concatenate(raw_pred)
 
     return combined_outputs_dict
 
@@ -258,22 +416,16 @@ class ModelState:
 def main(demo=False):
   cloudlog.warning("modeld init")
 
-  starpilot_toggles = get_starpilot_toggles()
+  sentry.set_tag("daemon", PROCESS_NAME)
+  cloudlog.bind(daemon=PROCESS_NAME)
+  setproctitle(PROCESS_NAME)
+  config_realtime_process(7, 54)
 
-  if not USBGPU:
-    # USB GPU currently saturates a core so can't do this yet,
-    # also need to move the aux USB interrupts for good timings
-    config_realtime_process(7, 54)
-
-  st = time.monotonic()
   cloudlog.warning("setting up CL context")
   cl_context = CLContext()
-  params = Params()
-  selected_model_key, model_paths = _resolve_model_paths(params)
   cloudlog.warning("CL context ready; loading model")
-  model = ModelState(cl_context, model_paths)
-  cloudlog.warning(f"selected model source: {selected_model_key}")
-  cloudlog.warning(f"models loaded in {time.monotonic() - st:.1f}s, modeld starting")
+  model = ModelState(cl_context)
+  cloudlog.warning("models loaded, modeld starting")
 
   # visionipc clients
   while True:
@@ -299,13 +451,14 @@ def main(demo=False):
     cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
 
   # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "starpilotPlan"])
 
   publish_state = PublishState()
+  params = Params()
 
   # setup filter to track dropped frames
-  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_RUN_FREQ)
+  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
   frame_id = 0
   last_vipc_frame_id = 0
   run_count = 0
@@ -326,13 +479,12 @@ def main(demo=False):
 
   # TODO this needs more thought, use .2s extra for now to estimate other delays
   # TODO Move smooth seconds to action function
-  long_delay = starpilot_toggles.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
+  long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
   prev_action = log.ModelDataV2.Action()
 
   DH = DesireHelper()
 
-  sm = sm.extend(['starpilotPlan'])
-  pm = pm.extend(['starpilotModelV2'])
+  starpilot_toggles = get_starpilot_toggles(sm)
 
   while True:
     # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
@@ -373,6 +525,7 @@ def main(demo=False):
     frame_id = sm["roadCameraState"].frameId
     v_ego = max(sm["carState"].vEgo, 0.)
     lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
       dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
@@ -402,10 +555,14 @@ def main(demo=False):
 
     bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
     transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
+
     inputs:dict[str, np.ndarray] = {
-      'desire_pulse': vec_desire,
+      model.desire_key: vec_desire,
       'traffic_convention': traffic_convention,
     }
+    # Include optional inputs only if the loaded model expects them
+    if 'lateral_control_params' in model.numpy_inputs:
+      inputs['lateral_control_params'] = lateral_control_params
 
     mt1 = time.perf_counter()
     model_output = model.run(bufs, transforms, inputs, prepare_only)
@@ -414,10 +571,18 @@ def main(demo=False):
 
     if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
+      starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
       drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
 
-      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego)
+      frame_delay = DT_MDL  # Average time elapsed since the current frame finished exposing.
+      action_delay = DT_MDL / 2  # Target the midpoint between current output and the next model step.
+      action = get_action_from_model(
+        model_output, prev_action,
+        lat_delay + frame_delay + action_delay,
+        long_delay + frame_delay + action_delay,
+        v_ego, model.mlsim, model.is_v9, starpilot_toggles,
+      )
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
@@ -430,22 +595,20 @@ def main(demo=False):
       DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      starpilot_modelv2_send.starpilotModelV2.turnDirection = DH.turn_direction
       drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
       drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
 
       fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
       pm.send('modelV2', modelv2_send)
+      pm.send('starpilotModelV2', starpilot_modelv2_send)
       pm.send('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
-
-      starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
-      starpilot_modelv2_send.starpilotModelV2.turnDirection = DH.turn_direction
-
-      pm.send('starpilotModelV2', starpilot_modelv2_send)
     last_vipc_frame_id = meta_main.frame_id
 
-    starpilot_toggles = get_starpilot_toggles(sm)
-
+    # Update planner-driven parameters
+    if sm.updated['starpilotPlan']:
+      starpilot_toggles = get_starpilot_toggles(sm)
 
 if __name__ == "__main__":
   try:
@@ -455,4 +618,7 @@ if __name__ == "__main__":
     args = parser.parse_args()
     main(demo=args.demo)
   except KeyboardInterrupt:
-    cloudlog.warning("got SIGINT")
+    cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
+  except Exception:
+    sentry.capture_exception()
+    raise

selfdrive/modeld/parse_model_outputs.py

@@ -1,13 +1,16 @@
 import numpy as np
 from openpilot.selfdrive.modeld.constants import ModelConstants
 
+
 def safe_exp(x, out=None):
   # -11 is around 10**14, more causes float16 overflow
   return np.exp(np.clip(x, -np.inf, 11), out=out)
 
+
 def sigmoid(x):
   return 1. / (1. + safe_exp(-x))
 
+
 def softmax(x, axis=-1):
   x -= np.max(x, axis=axis, keepdims=True)
   if x.dtype == np.float32 or x.dtype == np.float64:
@@ -17,15 +20,15 @@ def softmax(x, axis=-1):
   x /= np.sum(x, axis=axis, keepdims=True)
   return x
 
+
 class Parser:
   def __init__(self, ignore_missing=False):
     self.ignore_missing = ignore_missing
 
   def check_missing(self, outs, name):
-    missing = name not in outs
-    if missing and not self.ignore_missing:
+    if name not in outs and not self.ignore_missing:
       raise ValueError(f"Missing output {name}")
-    return missing
+    return name not in outs
 
   def parse_categorical_crossentropy(self, name, outs, out_shape=None):
     if self.check_missing(outs, name):
@@ -85,34 +88,53 @@ class Parser:
     outs[name] = pred_mu_final.reshape(final_shape)
     outs[name + '_stds'] = pred_std_final.reshape(final_shape)
 
-  def is_mhp(self, outs, name, shape):
-    if self.check_missing(outs, name):
-      return False
-    if outs[name].shape[1] == 2 * shape:
-      return False
-    return True
+  def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'lead' in outs:
+      if outs['lead'].shape[1] == 2 * ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH:
+        self.parse_mdn('lead', outs, in_N=0, out_N=0,
+                       out_shape=(ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
+      else:
+        self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
+                       out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
+    if 'plan' in outs:
+      plan_mhp = outs['plan'].shape[1] != 2 * ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH
+      plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
+      self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N,
+                     out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+      if 'planplus' in outs:
+        self.parse_mdn('planplus', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    elif 'planplus' in outs:
+      planplus_mhp = outs['planplus'].shape[1] != 2 * ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH
+      planplus_in_N, planplus_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if planplus_mhp else (0, 0)
+      self.parse_mdn('planplus', outs, in_N=planplus_in_N, out_N=planplus_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    if 'lane_lines' in outs:
+      self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
+                     out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    if 'road_edges' in outs:
+      self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
+                     out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    if 'sim_pose' in outs:
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    if 'lane_lines_prob' in outs:
+      self.parse_binary_crossentropy('lane_lines_prob', outs)
+    if 'lead_prob' in outs:
+      self.parse_binary_crossentropy('lead_prob', outs)
 
   def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
     self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('lane_lines', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_LANE_LINES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
-    self.parse_mdn('road_edges', outs, in_N=0, out_N=0, out_shape=(ModelConstants.NUM_ROAD_EDGES,ModelConstants.IDX_N,ModelConstants.LANE_LINES_WIDTH))
-    self.parse_binary_crossentropy('lane_lines_prob', outs)
-    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
+    self.split_outputs(outs)
+    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
     self.parse_binary_crossentropy('meta', outs)
-    self.parse_binary_crossentropy('lead_prob', outs)
-    lead_mhp = self.is_mhp(outs, 'lead', ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH)
-    lead_in_N, lead_out_N = (ModelConstants.LEAD_MHP_N, ModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0)
-    lead_out_shape = (ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH) if lead_mhp else \
-        (ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)
-    self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape)
     return outs
 
   def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    plan_mhp = self.is_mhp(outs, 'plan',  ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH)
-    plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
-    self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.split_outputs(outs)
+    if 'lat_planner_solution' in outs:
+      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
     return outs
 

selfdrive/test/process_replay/process_replay.py

@@ -496,6 +496,7 @@ CONFIGS = [
     ignore=["logMonoTime"],
     should_recv_callback=MessageBasedRcvCallback("cameraOdometry"),
     tolerance=NUMPY_TOLERANCE,
+    processing_time=0.01,
   ),
   ProcessConfig(
     proc_name="paramsd",

starpilot/tinygrad_modeld/constants.py

@@ -1,93 +0,0 @@
-import numpy as np
-
-def index_function(idx, max_val=192, max_idx=32):
-  return (max_val) * ((idx/max_idx)**2)
-
-class ModelConstants:
-  # time and distance indices
-  IDX_N = 33
-  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
-  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
-  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
-  LEAD_T_OFFSETS = [0., 2., 4.]
-  META_T_IDXS = [2., 4., 6., 8., 10.]
-
-  # model inputs constants
-  MODEL_FREQ = 20
-  HISTORY_FREQ = 5
-  HISTORY_LEN_SECONDS = 5
-  TEMPORAL_SKIP = MODEL_FREQ // HISTORY_FREQ
-  FULL_HISTORY_BUFFER_LEN = MODEL_FREQ * HISTORY_LEN_SECONDS
-  INPUT_HISTORY_BUFFER_LEN = HISTORY_FREQ * HISTORY_LEN_SECONDS
-  N_FRAMES = 2
-  MODEL_RUN_FREQ = 20
-  MODEL_CONTEXT_FREQ = 5 # "model_trained_fps"
-
-  FEATURE_LEN = 512
-
-  DESIRE_LEN = 8
-  TRAFFIC_CONVENTION_LEN = 2
-  LAT_PLANNER_STATE_LEN = 4
-  LATERAL_CONTROL_PARAMS_LEN = 2
-  PREV_DESIRED_CURV_LEN = 1
-
-  # model outputs constants
-  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
-  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
-  FCW_5MS2_PROBS_WIDTH = 5
-  FCW_3MS2_PROBS_WIDTH = 2
-
-  DISENGAGE_WIDTH = 5
-  POSE_WIDTH = 6
-  WIDE_FROM_DEVICE_WIDTH = 3
-  LEAD_WIDTH = 4
-  LANE_LINES_WIDTH = 2
-  ROAD_EDGES_WIDTH = 2
-  PLAN_WIDTH = 15
-  DESIRE_PRED_WIDTH = 8
-  LAT_PLANNER_SOLUTION_WIDTH = 4
-  DESIRED_CURV_WIDTH = 1
-
-  NUM_LANE_LINES = 4
-  NUM_ROAD_EDGES = 2
-
-  LEAD_TRAJ_LEN = 6
-  DESIRE_PRED_LEN = 4
-
-  PLAN_MHP_N = 5
-  LEAD_MHP_N = 2
-  PLAN_MHP_SELECTION = 1
-  LEAD_MHP_SELECTION = 3
-
-  FCW_THRESHOLD_5MS2_HIGH = 0.15
-  FCW_THRESHOLD_5MS2_LOW = 0.05
-  FCW_THRESHOLD_3MS2 = 0.7
-
-  CONFIDENCE_BUFFER_LEN = 5
-  RYG_GREEN = 0.01165
-  RYG_YELLOW = 0.06157
-
-  POLY_PATH_DEGREE = 4
-
-# model outputs slices
-class Plan:
-  POSITION = slice(0, 3)
-  VELOCITY = slice(3, 6)
-  ACCELERATION = slice(6, 9)
-  T_FROM_CURRENT_EULER = slice(9, 12)
-  ORIENTATION_RATE = slice(12, 15)
-
-class Meta:
-  ENGAGED = slice(0, 1)
-  # next 2, 4, 6, 8, 10 seconds
-  GAS_DISENGAGE = slice(1, 31, 6)
-  BRAKE_DISENGAGE = slice(2, 31, 6)
-  STEER_OVERRIDE = slice(3, 31, 6)
-  HARD_BRAKE_3 = slice(4, 31, 6)
-  HARD_BRAKE_4 = slice(5, 31, 6)
-  HARD_BRAKE_5 = slice(6, 31, 6)
-  # next 0, 2, 4, 6, 8, 10 seconds
-  GAS_PRESS = slice(31, 55, 4)
-  BRAKE_PRESS = slice(32, 55, 4)
-  LEFT_BLINKER = slice(33, 55, 4)
-  RIGHT_BLINKER = slice(34, 55, 4)

starpilot/tinygrad_modeld/dmonitoringmodeld.py

@@ -1,158 +0,0 @@
-#!/usr/bin/env python3
-import os
-from openpilot.system.hardware import TICI
-os.environ['DEV'] = 'QCOM' if TICI else 'CPU'
-from tinygrad.tensor import Tensor
-from tinygrad.dtype import dtypes
-import time
-import pickle
-import numpy as np
-from pathlib import Path
-
-from cereal import messaging
-from cereal.messaging import PubMaster, SubMaster
-from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
-from openpilot.common.swaglog import cloudlog
-from openpilot.common.realtime import config_realtime_process
-from openpilot.common.transformations.model import dmonitoringmodel_intrinsics
-from openpilot.common.transformations.camera import _ar_ox_fisheye, _os_fisheye
-from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid, safe_exp
-from openpilot.selfdrive.modeld.models.commonmodel_pyx import CLContext, MonitoringModelFrame
-from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
-
-PROCESS_NAME = "starpilot.tinygrad_modeld.dmonitoringmodeld"
-SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
-MODEL_PKL_PATH = Path(__file__).resolve().parents[2] / "selfdrive/modeld/models/dmonitoring_model_tinygrad.pkl"
-METADATA_PATH = Path(__file__).parent / "models/dmonitoring_model_metadata.pkl"
-
-
-class ModelState:
-  inputs: dict[str, np.ndarray]
-  output: np.ndarray
-
-  def __init__(self, cl_ctx: CLContext):
-    with open(METADATA_PATH, 'rb') as f:
-      model_metadata = pickle.load(f)
-      self.input_shapes = model_metadata['input_shapes']
-      self.output_slices = model_metadata['output_slices']
-
-    self.frame = MonitoringModelFrame(cl_ctx)
-    self.numpy_inputs = {
-      'calib': np.zeros(self.input_shapes['calib'], dtype=np.float32),
-    }
-
-    self.tensor_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.numpy_inputs.items()}
-    with open(MODEL_PKL_PATH, "rb") as f:
-      self.model_run = pickle.load(f)
-
-  def run(self, buf: VisionBuf, calib: np.ndarray, transform: np.ndarray) -> tuple[np.ndarray, float]:
-    self.numpy_inputs['calib'][0, :] = calib
-
-    t1 = time.perf_counter()
-
-    input_img_cl = self.frame.prepare(buf, transform.flatten())
-    if TICI:
-      if 'input_img' not in self.tensor_inputs:
-        self.tensor_inputs['input_img'] = qcom_tensor_from_opencl_address(
-          input_img_cl.mem_address, self.input_shapes['input_img'], dtype=dtypes.uint8
-        )
-    else:
-      self.tensor_inputs['input_img'] = Tensor(
-        self.frame.buffer_from_cl(input_img_cl).reshape(self.input_shapes['input_img']), dtype=dtypes.uint8
-      ).realize()
-
-    output = self.model_run(**self.tensor_inputs).contiguous().realize().uop.base.buffer.numpy().flatten()
-
-    t2 = time.perf_counter()
-    return output, t2 - t1
-
-
-def slice_outputs(model_outputs, output_slices):
-  return {k: model_outputs[np.newaxis, v] for k, v in output_slices.items()}
-
-
-def parse_model_output(model_output):
-  parsed = {}
-  parsed['wheel_on_right'] = sigmoid(model_output['wheel_on_right'])
-  for ds_suffix in ['lhd', 'rhd']:
-    face_descs = model_output[f'face_descs_{ds_suffix}']
-    parsed[f'face_descs_{ds_suffix}'] = face_descs[:, :-6]
-    parsed[f'face_descs_{ds_suffix}_std'] = safe_exp(face_descs[:, -6:])
-    for key in ['face_prob', 'eyes_visible_prob', 'eyes_closed_prob', 'using_phone_prob']:
-      parsed[f'{key}_{ds_suffix}'] = sigmoid(model_output[f'{key}_{ds_suffix}'])
-  return parsed
-
-
-def fill_driver_data(msg, model_output, ds_suffix):
-  msg.faceOrientation = model_output[f'face_descs_{ds_suffix}'][0, :3].tolist()
-  msg.faceOrientationStd = model_output[f'face_descs_{ds_suffix}_std'][0, :3].tolist()
-  msg.facePosition = model_output[f'face_descs_{ds_suffix}'][0, 3:5].tolist()
-  msg.facePositionStd = model_output[f'face_descs_{ds_suffix}_std'][0, 3:5].tolist()
-  msg.faceProb = model_output[f'face_prob_{ds_suffix}'][0, 0].item()
-  msg.eyesVisibleProb = model_output[f'eyes_visible_prob_{ds_suffix}'][0, 0].item()
-  msg.eyesClosedProb = model_output[f'eyes_closed_prob_{ds_suffix}'][0, 0].item()
-  msg.phoneProb = model_output[f'using_phone_prob_{ds_suffix}'][0, 0].item()
-
-
-def get_driverstate_packet(model_output, frame_id: int, location_ts: int, exec_time: float, gpu_exec_time: float):
-  msg = messaging.new_message('driverStateV2', valid=True)
-  ds = msg.driverStateV2
-  ds.frameId = frame_id
-  ds.modelExecutionTime = exec_time
-  ds.gpuExecutionTime = gpu_exec_time
-  ds.rawPredictions = model_output['raw_pred']
-  ds.wheelOnRightProb = model_output['wheel_on_right'][0, 0].item()
-  fill_driver_data(ds.leftDriverData, model_output, 'lhd')
-  fill_driver_data(ds.rightDriverData, model_output, 'rhd')
-  return msg
-
-
-def main():
-  config_realtime_process(7, 5)
-
-  cl_context = CLContext()
-  model = ModelState(cl_context)
-  cloudlog.warning("models loaded, dmonitoringmodeld starting")
-
-  cloudlog.warning("connecting to driver stream")
-  vipc_client = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_DRIVER, True, cl_context)
-  while not vipc_client.connect(False):
-    time.sleep(0.1)
-  assert vipc_client.is_connected()
-  cloudlog.warning(f"connected with buffer size: {vipc_client.buffer_len}")
-
-  sm = SubMaster(["liveCalibration"])
-  pm = PubMaster(["driverStateV2"])
-
-  calib = np.zeros(model.numpy_inputs['calib'].size, dtype=np.float32)
-  model_transform = None
-
-  while True:
-    buf = vipc_client.recv()
-    if buf is None:
-      continue
-
-    if model_transform is None:
-      cam = _os_fisheye if buf.width == _os_fisheye.width else _ar_ox_fisheye
-      model_transform = np.linalg.inv(np.dot(dmonitoringmodel_intrinsics, np.linalg.inv(cam.intrinsics))).astype(np.float32)
-
-    sm.update(0)
-    if sm.updated["liveCalibration"]:
-      calib[:] = np.array(sm["liveCalibration"].rpyCalib)
-
-    t1 = time.perf_counter()
-    model_output, gpu_execution_time = model.run(buf, calib, model_transform)
-    t2 = time.perf_counter()
-    raw_pred = model_output.tobytes() if SEND_RAW_PRED else b''
-    model_output = slice_outputs(model_output, model.output_slices)
-    model_output = parse_model_output(model_output)
-    model_output['raw_pred'] = raw_pred
-    msg = get_driverstate_packet(model_output, vipc_client.frame_id, vipc_client.timestamp_sof, t2 - t1, gpu_execution_time)
-    pm.send("driverStateV2", msg)
-
-
-if __name__ == "__main__":
-  try:
-    main()
-  except KeyboardInterrupt:
-    cloudlog.warning("got SIGINT")

starpilot/tinygrad_modeld/fill_model_msg.py

@@ -1,245 +0,0 @@
-import os
-import capnp
-import numpy as np
-from cereal import log
-from openpilot.starpilot.tinygrad_modeld.constants import ModelConstants, Plan, Meta
-from openpilot.selfdrive.controls.lib.drive_helpers import get_curvature_from_plan
-
-SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
-
-ConfidenceClass = log.ModelDataV2.ConfidenceClass
-
-# Return curvature for lateral action. If the model outputs desired_curvature and we're not in mlsim mode,
-# use it directly; otherwise derive from the plan using yaw and yaw-rate.
-def get_curvature_from_output(output: dict, plan: np.ndarray, v_ego: float, lat_action_t: float, mlsim: bool) -> float:
-  if not mlsim:
-    desired = output.get('desired_curvature')
-    if desired is not None:
-      return float(desired[0, 0])
-
-  # Use yaw (index 2) and yaw_rate (index 2)
-  theta = plan[:, Plan.T_FROM_CURRENT_EULER][:, 2]
-  theta_dot = plan[:, Plan.ORIENTATION_RATE][:, 2]
-  return float(get_curvature_from_plan(theta, theta_dot, ModelConstants.T_IDXS, v_ego, lat_action_t))
-
-
-class PublishState:
-  def __init__(self):
-    self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
-    self.prev_brake_5ms2_probs = np.zeros(ModelConstants.FCW_5MS2_PROBS_WIDTH, dtype=np.float32)
-    self.prev_brake_3ms2_probs = np.zeros(ModelConstants.FCW_3MS2_PROBS_WIDTH, dtype=np.float32)
-
-def fill_xyzt(builder, t, x, y, z, x_std=None, y_std=None, z_std=None):
-  builder.t = t
-  builder.x = x.tolist()
-  builder.y = y.tolist()
-  builder.z = z.tolist()
-  if x_std is not None:
-    builder.xStd = x_std.tolist()
-  if y_std is not None:
-    builder.yStd = y_std.tolist()
-  if z_std is not None:
-    builder.zStd = z_std.tolist()
-
-def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std=None):
-  builder.t = t
-  builder.x = x.tolist()
-  builder.y = y.tolist()
-  builder.v = v.tolist()
-  builder.a = a.tolist()
-  if x_std is not None:
-    builder.xStd = x_std.tolist()
-  if y_std is not None:
-    builder.yStd = y_std.tolist()
-  if v_std is not None:
-    builder.vStd = v_std.tolist()
-  if a_std is not None:
-    builder.aStd = a_std.tolist()
-
-def fill_xyz_poly(builder, degree, x, y, z):
-  xyz = np.stack([x, y, z], axis=1)
-  coeffs = np.polynomial.polynomial.polyfit(ModelConstants.T_IDXS, xyz, deg=degree)
-  builder.xCoefficients = coeffs[:, 0].tolist()
-  builder.yCoefficients = coeffs[:, 1].tolist()
-  builder.zCoefficients = coeffs[:, 2].tolist()
-
-def fill_lane_line_meta(builder, lane_lines, lane_line_probs):
-  builder.leftY = lane_lines[1].y[0]
-  builder.leftProb = lane_line_probs[1]
-  builder.rightY = lane_lines[2].y[0]
-  builder.rightProb = lane_line_probs[2]
-
-def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._DynamicStructBuilder,
-                   net_output_data: dict[str, np.ndarray], action: log.ModelDataV2.Action,
-                   publish_state: PublishState, vipc_frame_id: int, vipc_frame_id_extra: int,
-                   frame_id: int, frame_drop: float, timestamp_eof: int, model_execution_time: float,
-                   valid: bool) -> None:
-  frame_age = frame_id - vipc_frame_id if frame_id > vipc_frame_id else 0
-  frame_drop_perc = frame_drop * 100
-  extended_msg.valid = valid
-  base_msg.valid = valid
-
-  driving_model_data = base_msg.drivingModelData
-
-  driving_model_data.frameId = vipc_frame_id
-  driving_model_data.frameIdExtra = vipc_frame_id_extra
-  driving_model_data.frameDropPerc = frame_drop_perc
-  driving_model_data.modelExecutionTime = model_execution_time
-
-  driving_model_data.action = action
-
-  modelV2 = extended_msg.modelV2
-  modelV2.frameId = vipc_frame_id
-  modelV2.frameIdExtra = vipc_frame_id_extra
-  modelV2.frameAge = frame_age
-  modelV2.frameDropPerc = frame_drop_perc
-  modelV2.timestampEof = timestamp_eof
-  modelV2.modelExecutionTime = model_execution_time
-
-  # normalize plan tensors to (IDX_N, WIDTH)
-  plan_arr = net_output_data['plan'][0]
-  plan_stds_arr = net_output_data['plan_stds'][0]
-
-  # plan
-  fill_xyzt(modelV2.position, ModelConstants.T_IDXS, *plan_arr[:,Plan.POSITION].T, *plan_stds_arr[:,Plan.POSITION].T)
-  fill_xyzt(modelV2.velocity, ModelConstants.T_IDXS, *plan_arr[:,Plan.VELOCITY].T)
-  fill_xyzt(modelV2.acceleration, ModelConstants.T_IDXS, *plan_arr[:,Plan.ACCELERATION].T)
-  fill_xyzt(modelV2.orientation, ModelConstants.T_IDXS, *plan_arr[:,Plan.T_FROM_CURRENT_EULER].T)
-  fill_xyzt(modelV2.orientationRate, ModelConstants.T_IDXS, *plan_arr[:,Plan.ORIENTATION_RATE].T)
-
-  # temporal pose
-  try:
-    temporal_pose = modelV2.temporalPose
-    if 'sim_pose' in net_output_data:
-      temporal_pose.trans = net_output_data['sim_pose'][0,:ModelConstants.POSE_WIDTH//2].tolist()
-      temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:ModelConstants.POSE_WIDTH//2].tolist()
-      temporal_pose.rot = net_output_data['sim_pose'][0,ModelConstants.POSE_WIDTH//2:].tolist()
-      temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,ModelConstants.POSE_WIDTH//2:].tolist()
-    else:
-      temporal_pose.trans = plan_arr[0,Plan.VELOCITY].tolist()
-      temporal_pose.transStd = plan_stds_arr[0,Plan.VELOCITY].tolist()
-      temporal_pose.rot = plan_arr[0,Plan.ORIENTATION_RATE].tolist()
-      temporal_pose.rotStd = plan_stds_arr[0,Plan.ORIENTATION_RATE].tolist()
-  except AttributeError:
-    pass
-
-  # poly path
-  fill_xyz_poly(driving_model_data.path, ModelConstants.POLY_PATH_DEGREE, *plan_arr[:,Plan.POSITION].T)
-
-  # action
-  modelV2.action = action
-
-  # times at X_IDXS of edges and lines aren't used
-  LINE_T_IDXS: list[float] = []
-
-  # lane lines
-  has_lane_lines = all(k in net_output_data for k in ("lane_lines", "lane_lines_stds", "lane_lines_prob"))
-  modelV2.init('laneLines', 6)
-  if has_lane_lines:
-    for i in range(6):
-      lane_line = modelV2.laneLines[i]
-      if i < 4:
-        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
-      elif i == 4:
-        leftLane_x = 0.5 * (net_output_data['lane_lines'][0,0,:,0] + net_output_data['lane_lines'][0,1,:,0])
-        leftLane_y = 0.5 * (net_output_data['lane_lines'][0,0,:,1] + net_output_data['lane_lines'][0,1,:,1])
-        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y)
-      elif i == 5:
-        rightLane_x = 0.5 * (net_output_data['lane_lines'][0,2,:,0] + net_output_data['lane_lines'][0,3,:,0])
-        rightLane_y = 0.5 * (net_output_data['lane_lines'][0,2,:,1] + net_output_data['lane_lines'][0,3,:,1])
-        fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y)
-    modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
-    modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
-  else:
-    x_vals = np.array(ModelConstants.X_IDXS)
-    zeros = np.zeros_like(x_vals, dtype=np.float32)
-    for i in range(6):
-      fill_xyzt(modelV2.laneLines[i], LINE_T_IDXS, x_vals, zeros, zeros)
-    modelV2.laneLineStds = [0.0] * 4
-    modelV2.laneLineProbs = [0.0] * 4
-
-  fill_lane_line_meta(driving_model_data.laneLineMeta, modelV2.laneLines, modelV2.laneLineProbs)
-
-  # road edges
-  modelV2.init('roadEdges', 2)
-  for i in range(2):
-    road_edge = modelV2.roadEdges[i]
-    fill_xyzt(road_edge, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1])
-  modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist()
-
-  # leads
-  modelV2.init('leadsV3', 3)
-  for i in range(3):
-    lead = modelV2.leadsV3[i]
-    fill_xyvat(lead, ModelConstants.LEAD_T_IDXS, *net_output_data['lead'][0,i].T, *net_output_data['lead_stds'][0,i].T)
-    lead.prob = net_output_data['lead_prob'][0,i].tolist()
-    lead.probTime = ModelConstants.LEAD_T_OFFSETS[i]
-
-  # meta
-  meta = modelV2.meta
-  meta.desireState = net_output_data['desire_state'][0].reshape(-1).tolist()
-  meta.desirePrediction = net_output_data['desire_pred'][0].reshape(-1).tolist()
-  meta.engagedProb = net_output_data['meta'][0,Meta.ENGAGED].item()
-  meta.init('disengagePredictions')
-  disengage_predictions = meta.disengagePredictions
-  disengage_predictions.t = ModelConstants.META_T_IDXS
-  disengage_predictions.brakeDisengageProbs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE].tolist()
-  disengage_predictions.gasDisengageProbs = net_output_data['meta'][0,Meta.GAS_DISENGAGE].tolist()
-  disengage_predictions.steerOverrideProbs = net_output_data['meta'][0,Meta.STEER_OVERRIDE].tolist()
-  disengage_predictions.brake3MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_3].tolist()
-  disengage_predictions.brake4MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_4].tolist()
-  disengage_predictions.brake5MetersPerSecondSquaredProbs = net_output_data['meta'][0,Meta.HARD_BRAKE_5].tolist()
-  disengage_predictions.gasPressProbs = net_output_data['meta'][0,Meta.GAS_PRESS].tolist()
-  disengage_predictions.brakePressProbs = net_output_data['meta'][0,Meta.BRAKE_PRESS].tolist()
-
-  publish_state.prev_brake_5ms2_probs[:-1] = publish_state.prev_brake_5ms2_probs[1:]
-  publish_state.prev_brake_5ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_5][0]
-  publish_state.prev_brake_3ms2_probs[:-1] = publish_state.prev_brake_3ms2_probs[1:]
-  publish_state.prev_brake_3ms2_probs[-1] = net_output_data['meta'][0,Meta.HARD_BRAKE_3][0]
-  hard_brake_predicted = (publish_state.prev_brake_5ms2_probs > ModelConstants.FCW_THRESHOLDS_5MS2).all() and \
-    (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
-  meta.hardBrakePredicted = hard_brake_predicted.item()
-
-  # confidence
-  if vipc_frame_id % (2*ModelConstants.MODEL_RUN_FREQ) == 0:
-    # any disengage prob
-    brake_disengage_probs = net_output_data['meta'][0,Meta.BRAKE_DISENGAGE]
-    gas_disengage_probs = net_output_data['meta'][0,Meta.GAS_DISENGAGE]
-    steer_override_probs = net_output_data['meta'][0,Meta.STEER_OVERRIDE]
-    any_disengage_probs = 1-((1-brake_disengage_probs)*(1-gas_disengage_probs)*(1-steer_override_probs))
-    # independent disengage prob for each 2s slice
-    ind_disengage_probs = np.r_[any_disengage_probs[0], np.diff(any_disengage_probs) / (1 - any_disengage_probs[:-1])]
-    # rolling buf for 2, 4, 6, 8, 10s
-    publish_state.disengage_buffer[:-ModelConstants.DISENGAGE_WIDTH] = publish_state.disengage_buffer[ModelConstants.DISENGAGE_WIDTH:]
-    publish_state.disengage_buffer[-ModelConstants.DISENGAGE_WIDTH:] = ind_disengage_probs
-
-  score = 0.
-  for i in range(ModelConstants.DISENGAGE_WIDTH):
-    score += publish_state.disengage_buffer[i*ModelConstants.DISENGAGE_WIDTH+ModelConstants.DISENGAGE_WIDTH-1-i].item() / ModelConstants.DISENGAGE_WIDTH
-  if score < ModelConstants.RYG_GREEN:
-    modelV2.confidence = ConfidenceClass.green
-  elif score < ModelConstants.RYG_YELLOW:
-    modelV2.confidence = ConfidenceClass.yellow
-  else:
-    modelV2.confidence = ConfidenceClass.red
-
-  # raw prediction if enabled
-  if SEND_RAW_PRED:
-    modelV2.rawPredictions = net_output_data['raw_pred'].tobytes()
-
-def fill_pose_msg(msg: capnp._DynamicStructBuilder, net_output_data: 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)
-  cameraOdometry = msg.cameraOdometry
-
-  cameraOdometry.frameId = vipc_frame_id
-  cameraOdometry.timestampEof = timestamp_eof
-
-  cameraOdometry.trans = net_output_data['pose'][0,:3].tolist()
-  cameraOdometry.rot = net_output_data['pose'][0,3:].tolist()
-  cameraOdometry.wideFromDeviceEuler = net_output_data['wide_from_device_euler'][0,:].tolist()
-  cameraOdometry.roadTransformTrans = net_output_data['road_transform'][0,:3].tolist()
-  cameraOdometry.transStd = net_output_data['pose_stds'][0,:3].tolist()
-  cameraOdometry.rotStd = net_output_data['pose_stds'][0,3:].tolist()
-  cameraOdometry.wideFromDeviceEulerStd = net_output_data['wide_from_device_euler_stds'][0,:].tolist()
-  cameraOdometry.roadTransformTransStd = net_output_data['road_transform_stds'][0,:3].tolist()

starpilot/tinygrad_modeld/get_model_metadata.py

@@ -1,37 +0,0 @@
-#!/usr/bin/env python3
-import sys
-import pathlib
-import onnx
-import codecs
-import pickle
-from typing import Any
-
-def get_name_and_shape(value_info:onnx.ValueInfoProto) -> tuple[str, tuple[int,...]]:
-  shape = tuple([int(dim.dim_value) for dim in value_info.type.tensor_type.shape.dim])
-  name = value_info.name
-  return name, shape
-
-def get_metadata_value_by_name(model:onnx.ModelProto, name:str) -> str | Any:
-  for prop in model.metadata_props:
-    if prop.key == name:
-      return prop.value
-  return None
-
-if __name__ == "__main__":
-  model_path = pathlib.Path(sys.argv[1])
-  model = onnx.load(str(model_path))
-  output_slices = get_metadata_value_by_name(model, 'output_slices')
-  assert output_slices is not None, 'output_slices not found in metadata'
-
-  metadata = {
-    'model_checkpoint': get_metadata_value_by_name(model, 'model_checkpoint'),
-    'output_slices': pickle.loads(codecs.decode(output_slices.encode(), "base64")),
-    'input_shapes': dict([get_name_and_shape(x) for x in model.graph.input]),
-    'output_shapes': dict([get_name_and_shape(x) for x in model.graph.output])
-  }
-
-  metadata_path = model_path.parent / (model_path.stem + '_metadata.pkl')
-  with open(metadata_path, 'wb') as f:
-    pickle.dump(metadata, f)
-
-  print(f'saved metadata to {metadata_path}')

starpilot/tinygrad_modeld/models/README.md

@@ -1,66 +0,0 @@
-## Neural networks in openpilot
-To view the architecture of the ONNX networks, you can use [netron](https://netron.app/)
-
-## Driving Model (vision model + temporal policy model)
-### Vision inputs (Full size: 799906 x float32)
-* **image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-* **wide image stream**
-  * Two consecutive images (256 * 512 * 3 in RGB) recorded at 20 Hz : 393216 = 2 * 6 * 128 * 256
-    * Each 256 * 512 image is represented in YUV420 with 6 channels : 6 * 128 * 256
-      * Channels 0,1,2,3 represent the full-res Y channel and are represented in numpy as Y[::2, ::2], Y[::2, 1::2], Y[1::2, ::2], and Y[1::2, 1::2]
-      * Channel 4 represents the half-res U channel
-      * Channel 5 represents the half-res V channel
-### Policy inputs
-* **desire**
-  * one-hot encoded buffer to command model to execute certain actions, bit needs to be sent for the past 5 seconds (at 20FPS) : 100 * 8
-* **traffic convention**
-  * one-hot encoded vector to tell model whether traffic is right-hand or left-hand traffic : 2
-* **lateral control params**
-  * speed and steering delay for predicting the desired curvature: 2
-* **previous desired curvatures**
-  * vector of previously predicted desired curvatures: 100 * 1
-* **feature buffer**
-  * a buffer of intermediate features including the current feature to form a 5 seconds temporal context (at 20FPS) : 100 * 512
-
-
-### Driving Model output format (Full size: XXX x float32)
-Refer to **slice_outputs** and **parse_vision_outputs/parse_policy_outputs** in modeld.
-
-
-## Driver Monitoring Model
-* .onnx model can be run with onnx runtimes
-* .dlc file is a pre-quantized model and only runs on qualcomm DSPs
-
-### input format
-* single image W = 1440 H = 960 luminance channel (Y) from the planar YUV420 format:
-  * full input size is 1440 * 960 = 1382400
-  * normalized ranging from 0.0 to 1.0 in float32 (onnx runner) or ranging from 0 to 255 in uint8 (snpe runner)
-* camera calibration angles (roll, pitch, yaw) from liveCalibration: 3 x float32 inputs
-
-### output format
-* 84 x float32 outputs = 2 + 41 * 2 ([parsing example](https://github.com/commaai/openpilot/blob/22ce4e17ba0d3bfcf37f8255a4dd1dc683fe0c38/selfdrive/modeld/models/dmonitoring.cc#L33))
-  * for each person in the front seats (2 * 41)
-    * face pose: 12 = 6 + 6
-      * face orientation [pitch, yaw, roll] in camera frame: 3
-      * face position [dx, dy] relative to image center: 2
-      * normalized face size: 1
-      * standard deviations for above outputs: 6
-    * face visible probability: 1
-    * eyes: 20 = (8 + 1) + (8 + 1) + 1 + 1
-      * eye position and size, and their standard deviations: 8
-      * eye visible probability: 1
-      * eye closed probability: 1
-    * wearing sunglasses probability: 1
-    * face occluded probability: 1
-    * touching wheel probability: 1
-    * paying attention probability: 1
-    * (deprecated) distracted probabilities: 2
-    * using phone probability: 1
-    * distracted probability: 1
-  * common outputs 1
-    * left hand drive probability: 1

starpilot/tinygrad_modeld/models/big_driving_policy.onnx

@@ -1 +0,0 @@
-driving_policy.onnx

starpilot/tinygrad_modeld/models/big_driving_vision.onnx

@@ -1 +0,0 @@
-driving_vision.onnx

starpilot/tinygrad_modeld/models/commonmodel.cc

@@ -1,64 +0,0 @@
-#include "selfdrive/modeld/models/commonmodel.h"
-
-#include <cmath>
-#include <cstring>
-
-#include "common/clutil.h"
-
-DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip) : ModelFrame(device_id, context) {
-  input_frames = std::make_unique<uint8_t[]>(buf_size);
-  temporal_skip = _temporal_skip;
-  input_frames_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
-  img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (temporal_skip+1)*frame_size_bytes, NULL, &err));
-  region.origin = temporal_skip * frame_size_bytes;
-  region.size = frame_size_bytes;
-  last_img_cl = CL_CHECK_ERR(clCreateSubBuffer(img_buffer_20hz_cl, CL_MEM_READ_WRITE, CL_BUFFER_CREATE_TYPE_REGION, &region, &err));
-
-  loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
-  init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
-}
-
-cl_mem* DrivingModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
-  run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
-
-  for (int i = 0; i < temporal_skip; i++) {
-    CL_CHECK(clEnqueueCopyBuffer(q, img_buffer_20hz_cl, img_buffer_20hz_cl, (i+1)*frame_size_bytes, i*frame_size_bytes, frame_size_bytes, 0, nullptr, nullptr));
-  }
-  loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, last_img_cl);
-
-  copy_queue(&loadyuv, q, img_buffer_20hz_cl, input_frames_cl, 0, 0, frame_size_bytes);
-  copy_queue(&loadyuv, q, last_img_cl, input_frames_cl, 0, frame_size_bytes, frame_size_bytes);
-
-  // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
-  clFinish(q);
-  return &input_frames_cl;
-}
-
-DrivingModelFrame::~DrivingModelFrame() {
-  deinit_transform();
-  loadyuv_destroy(&loadyuv);
-  CL_CHECK(clReleaseMemObject(input_frames_cl));
-  CL_CHECK(clReleaseMemObject(img_buffer_20hz_cl));
-  CL_CHECK(clReleaseMemObject(last_img_cl));
-  CL_CHECK(clReleaseCommandQueue(q));
-}
-
-
-MonitoringModelFrame::MonitoringModelFrame(cl_device_id device_id, cl_context context) : ModelFrame(device_id, context) {
-  input_frames = std::make_unique<uint8_t[]>(buf_size);
-  input_frame_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, buf_size, NULL, &err));
-
-  init_transform(device_id, context, MODEL_WIDTH, MODEL_HEIGHT);
-}
-
-cl_mem* MonitoringModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
-  run_transform(yuv_cl, MODEL_WIDTH, MODEL_HEIGHT, frame_width, frame_height, frame_stride, frame_uv_offset, projection);
-  clFinish(q);
-  return &y_cl;
-}
-
-MonitoringModelFrame::~MonitoringModelFrame() {
-  deinit_transform();
-  CL_CHECK(clReleaseMemObject(input_frame_cl));
-  CL_CHECK(clReleaseCommandQueue(q));
-}

starpilot/tinygrad_modeld/models/commonmodel.h

@@ -1,97 +0,0 @@
-#pragma once
-
-#include <cfloat>
-#include <cstdlib>
-#include <cassert>
-
-#include <memory>
-
-#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
-#ifdef __APPLE__
-#include <OpenCL/cl.h>
-#else
-#include <CL/cl.h>
-#endif
-
-#include "common/mat.h"
-#include "selfdrive/modeld/transforms/loadyuv.h"
-#include "selfdrive/modeld/transforms/transform.h"
-
-class ModelFrame {
-public:
-  ModelFrame(cl_device_id device_id, cl_context context) {
-    q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
-  }
-  virtual ~ModelFrame() {}
-  virtual cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) { return NULL; }
-  uint8_t* buffer_from_cl(cl_mem *in_frames, int buffer_size) {
-    CL_CHECK(clEnqueueReadBuffer(q, *in_frames, CL_TRUE, 0, buffer_size, input_frames.get(), 0, nullptr, nullptr));
-    clFinish(q);
-    return &input_frames[0];
-  }
-
-  int MODEL_WIDTH;
-  int MODEL_HEIGHT;
-  int MODEL_FRAME_SIZE;
-  int buf_size;
-
-protected:
-  cl_mem y_cl, u_cl, v_cl;
-  Transform transform;
-  cl_command_queue q;
-  std::unique_ptr<uint8_t[]> input_frames;
-
-  void init_transform(cl_device_id device_id, cl_context context, int model_width, int model_height) {
-    y_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, model_width * model_height, NULL, &err));
-    u_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
-    v_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, (model_width / 2) * (model_height / 2), NULL, &err));
-    transform_init(&transform, context, device_id);
-  }
-
-  void deinit_transform() {
-    transform_destroy(&transform);
-    CL_CHECK(clReleaseMemObject(v_cl));
-    CL_CHECK(clReleaseMemObject(u_cl));
-    CL_CHECK(clReleaseMemObject(y_cl));
-  }
-
-  void run_transform(cl_mem yuv_cl, int model_width, int model_height, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection) {
-    transform_queue(&transform, q,
-        yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
-        y_cl, u_cl, v_cl, model_width, model_height, projection);
-  }
-};
-
-class DrivingModelFrame : public ModelFrame {
-public:
-  DrivingModelFrame(cl_device_id device_id, cl_context context, int _temporal_skip);
-  ~DrivingModelFrame();
-  cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
-
-  const int MODEL_WIDTH = 512;
-  const int MODEL_HEIGHT = 256;
-  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
-  const int buf_size = MODEL_FRAME_SIZE * 2; // 2 frames are temporal_skip frames apart
-  const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
-
-private:
-  LoadYUVState loadyuv;
-  cl_mem img_buffer_20hz_cl, last_img_cl, input_frames_cl;
-  cl_buffer_region region;
-  int temporal_skip;
-};
-
-class MonitoringModelFrame : public ModelFrame {
-public:
-  MonitoringModelFrame(cl_device_id device_id, cl_context context);
-  ~MonitoringModelFrame();
-  cl_mem* prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3& projection);
-
-  const int MODEL_WIDTH = 1440;
-  const int MODEL_HEIGHT = 960;
-  const int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT;
-  const int buf_size = MODEL_FRAME_SIZE;
-
-private:
-  cl_mem input_frame_cl;
-};

starpilot/tinygrad_modeld/models/commonmodel.pxd

@@ -1,27 +0,0 @@
-# distutils: language = c++
-
-from msgq.visionipc.visionipc cimport cl_device_id, cl_context, cl_mem
-
-cdef extern from "common/mat.h":
-  cdef struct mat3:
-    float v[9]
-
-cdef extern from "common/clutil.h":
-  cdef unsigned long CL_DEVICE_TYPE_DEFAULT
-  cl_device_id cl_get_device_id(unsigned long)
-  cl_context cl_create_context(cl_device_id)
-  void cl_release_context(cl_context)
-
-cdef extern from "selfdrive/modeld/models/commonmodel.h":
-  cppclass ModelFrame:
-    int buf_size
-    unsigned char * buffer_from_cl(cl_mem*, int);
-    cl_mem * prepare(cl_mem, int, int, int, int, mat3)
-
-  cppclass DrivingModelFrame:
-    int buf_size
-    DrivingModelFrame(cl_device_id, cl_context, int)
-
-  cppclass MonitoringModelFrame:
-    int buf_size
-    MonitoringModelFrame(cl_device_id, cl_context)

starpilot/tinygrad_modeld/models/commonmodel_pyx.pxd

@@ -1,13 +0,0 @@
-# distutils: language = c++
-
-from msgq.visionipc.visionipc cimport cl_mem
-from msgq.visionipc.visionipc_pyx cimport CLContext as BaseCLContext
-
-cdef class CLContext(BaseCLContext):
-  pass
-
-cdef class CLMem:
-  cdef cl_mem * mem
-
-  @staticmethod
-  cdef create(void*)

starpilot/tinygrad_modeld/models/commonmodel_pyx.pyx

@@ -1,74 +0,0 @@
-# distutils: language = c++
-# cython: c_string_encoding=ascii, language_level=3
-
-import numpy as np
-cimport numpy as cnp
-from libc.string cimport memcpy
-from libc.stdint cimport uintptr_t
-
-from msgq.visionipc.visionipc cimport cl_mem
-from msgq.visionipc.visionipc_pyx cimport VisionBuf, CLContext as BaseCLContext
-from .commonmodel cimport CL_DEVICE_TYPE_DEFAULT, cl_get_device_id, cl_create_context, cl_release_context
-from .commonmodel cimport mat3, ModelFrame as cppModelFrame, DrivingModelFrame as cppDrivingModelFrame, MonitoringModelFrame as cppMonitoringModelFrame
-
-
-cdef class CLContext(BaseCLContext):
-  def __cinit__(self):
-    self.device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT)
-    self.context = cl_create_context(self.device_id)
-
-  def __dealloc__(self):
-    if self.context:
-      cl_release_context(self.context)
-
-cdef class CLMem:
-  @staticmethod
-  cdef create(void * cmem):
-    mem = CLMem()
-    mem.mem = <cl_mem*> cmem
-    return mem
-
-  @property
-  def mem_address(self):
-    return <uintptr_t>(self.mem)
-
-def cl_from_visionbuf(VisionBuf buf):
-  return CLMem.create(<void*>&buf.buf.buf_cl)
-
-
-cdef class ModelFrame:
-  cdef cppModelFrame * frame
-  cdef int buf_size
-
-  def __dealloc__(self):
-    del self.frame
-
-  def prepare(self, VisionBuf buf, float[:] projection):
-    cdef mat3 cprojection
-    memcpy(cprojection.v, &projection[0], 9*sizeof(float))
-    cdef cl_mem * data
-    data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection)
-    return CLMem.create(data)
-
-  def buffer_from_cl(self, CLMem in_frames):
-    cdef unsigned char * data2
-    data2 = self.frame.buffer_from_cl(in_frames.mem, self.buf_size)
-    return np.asarray(<cnp.uint8_t[:self.buf_size]> data2)
-
-
-cdef class DrivingModelFrame(ModelFrame):
-  cdef cppDrivingModelFrame * _frame
-
-  def __cinit__(self, CLContext context, int temporal_skip):
-    self._frame = new cppDrivingModelFrame(context.device_id, context.context, temporal_skip)
-    self.frame = <cppModelFrame*>(self._frame)
-    self.buf_size = self._frame.buf_size
-
-cdef class MonitoringModelFrame(ModelFrame):
-  cdef cppMonitoringModelFrame * _frame
-
-  def __cinit__(self, CLContext context):
-    self._frame = new cppMonitoringModelFrame(context.device_id, context.context)
-    self.frame = <cppModelFrame*>(self._frame)
-    self.buf_size = self._frame.buf_size
-

starpilot/tinygrad_modeld/parse_model_outputs.py

@@ -1,144 +0,0 @@
-import numpy as np
-from openpilot.starpilot.tinygrad_modeld.constants import ModelConstants
-
-
-def safe_exp(x, out=None):
-  # -11 is around 10**14, more causes float16 overflow
-  return np.exp(np.clip(x, -np.inf, 11), out=out)
-
-
-def sigmoid(x):
-  return 1. / (1. + safe_exp(-x))
-
-
-def softmax(x, axis=-1):
-  x -= np.max(x, axis=axis, keepdims=True)
-  if x.dtype == np.float32 or x.dtype == np.float64:
-    safe_exp(x, out=x)
-  else:
-    x = safe_exp(x)
-  x /= np.sum(x, axis=axis, keepdims=True)
-  return x
-
-
-class Parser:
-  def __init__(self, ignore_missing=False):
-    self.ignore_missing = ignore_missing
-
-  def check_missing(self, outs, name):
-    if name not in outs and not self.ignore_missing:
-      raise ValueError(f"Missing output {name}")
-    return name not in outs
-
-  def parse_categorical_crossentropy(self, name, outs, out_shape=None):
-    if self.check_missing(outs, name):
-      return
-    raw = outs[name]
-    if out_shape is not None:
-      raw = raw.reshape((raw.shape[0],) + out_shape)
-    outs[name] = softmax(raw, axis=-1)
-
-  def parse_binary_crossentropy(self, name, outs):
-    if self.check_missing(outs, name):
-      return
-    raw = outs[name]
-    outs[name] = sigmoid(raw)
-
-  def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
-    if self.check_missing(outs, name):
-      return
-    raw = outs[name]
-    raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
-
-    n_values = (raw.shape[2] - out_N)//2
-    pred_mu = raw[:,:,:n_values]
-    pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
-
-    if in_N > 1:
-      weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
-      for i in range(out_N):
-        weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
-
-      if out_N == 1:
-        for fidx in range(weights.shape[0]):
-          idxs = np.argsort(weights[fidx][:,0])[::-1]
-          weights[fidx] = weights[fidx][idxs]
-          pred_mu[fidx] = pred_mu[fidx][idxs]
-          pred_std[fidx] = pred_std[fidx][idxs]
-      full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
-      outs[name + '_weights'] = weights
-      outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
-      outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
-
-      pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
-      pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
-      for fidx in range(weights.shape[0]):
-        for hidx in range(out_N):
-          idxs = np.argsort(weights[fidx,:,hidx])[::-1]
-          pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
-          pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
-    else:
-      pred_mu_final = pred_mu
-      pred_std_final = pred_std
-
-    if out_N > 1:
-      final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
-    else:
-      final_shape = tuple([raw.shape[0],] + list(out_shape))
-    outs[name] = pred_mu_final.reshape(final_shape)
-    outs[name + '_stds'] = pred_std_final.reshape(final_shape)
-
-  def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
-    if 'lead' in outs:
-      if outs['lead'].shape[1] == 2 * ModelConstants.LEAD_MHP_SELECTION * ModelConstants.LEAD_TRAJ_LEN * ModelConstants.LEAD_WIDTH:
-        self.parse_mdn('lead', outs, in_N=0, out_N=0,
-                       out_shape=(ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
-      else:
-        self.parse_mdn('lead', outs, in_N=ModelConstants.LEAD_MHP_N, out_N=ModelConstants.LEAD_MHP_SELECTION,
-                       out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
-    if 'plan' in outs:
-      plan_mhp = outs['plan'].shape[1] != 2 * ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH
-      plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
-      self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N,
-                     out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-      if 'planplus' in outs:
-        self.parse_mdn('planplus', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    elif 'planplus' in outs:
-      planplus_mhp = outs['planplus'].shape[1] != 2 * ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH
-      planplus_in_N, planplus_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if planplus_mhp else (0, 0)
-      self.parse_mdn('planplus', outs, in_N=planplus_in_N, out_N=planplus_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'lane_lines' in outs:
-      self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
-                     out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'road_edges' in outs:
-      self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
-                     out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'sim_pose' in outs:
-      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'lane_lines_prob' in outs:
-      self.parse_binary_crossentropy('lane_lines_prob', outs)
-    if 'lead_prob' in outs:
-      self.parse_binary_crossentropy('lead_prob', outs)
-
-  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
-    self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.split_outputs(outs)
-    self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
-    self.parse_binary_crossentropy('meta', outs)
-    return outs
-
-  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    self.split_outputs(outs)
-    if 'lat_planner_solution' in outs:
-      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
-    if 'desired_curvature' in outs:
-      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
-    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
-    return outs
-
-  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    outs = self.parse_vision_outputs(outs)
-    outs = self.parse_policy_outputs(outs)
-    return outs

starpilot/tinygrad_modeld/runners/tinygrad_helpers.py

@@ -1,8 +0,0 @@
-
-from tinygrad.tensor import Tensor
-from tinygrad.helpers import to_mv
-
-def qcom_tensor_from_opencl_address(opencl_address, shape, dtype):
-  cl_buf_desc_ptr = to_mv(opencl_address, 8).cast('Q')[0]
-  rawbuf_ptr = to_mv(cl_buf_desc_ptr, 0x100).cast('Q')[20] # offset 0xA0 is a raw gpu pointer.
-  return Tensor.from_blob(rawbuf_ptr, shape, dtype=dtype, device='QCOM')

starpilot/tinygrad_modeld/tinygrad_modeld.py

@@ -1,620 +0,0 @@
-#!/usr/bin/env python3
-import os
-from openpilot.system.hardware import TICI
-os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
-from tinygrad.tensor import Tensor
-from tinygrad.dtype import dtypes
-import time
-import pickle
-import numpy as np
-import cereal.messaging as messaging
-from cereal import car, log
-from pathlib import Path
-from setproctitle import setproctitle
-from cereal.messaging import PubMaster, SubMaster
-from msgq.visionipc import VisionIpcClient, VisionStreamType, VisionBuf
-from openpilot.common.swaglog import cloudlog
-from openpilot.common.params import Params
-from openpilot.common.filter_simple import FirstOrderFilter
-from openpilot.common.realtime import config_realtime_process, DT_MDL
-from openpilot.common.transformations.camera import DEVICE_CAMERAS
-from openpilot.common.transformations.model import get_warp_matrix
-from openpilot.system import sentry
-from opendbc.car.car_helpers import get_demo_car_params
-from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan_tomb_raider, smooth_value
-from openpilot.starpilot.tinygrad_modeld.parse_model_outputs import Parser
-from openpilot.starpilot.tinygrad_modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
-from openpilot.starpilot.tinygrad_modeld.constants import ModelConstants, Plan
-try:
-  from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
-except ImportError:
-  from openpilot.starpilot.tinygrad_modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
-from openpilot.starpilot.tinygrad_modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
-from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, MODELS_PATH, params_memory
-
-
-PROCESS_NAME = "starpilot.tinygrad_modeld.tinygrad_modeld"
-SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
-
-BUILTIN_MODEL_KEY = "sc2"
-BUILTIN_MODEL_ALIASES = {BUILTIN_MODEL_KEY, "sc"}
-
-
-LAT_SMOOTH_SECONDS = 0.0
-LONG_SMOOTH_SECONDS = 0.3
-MIN_LAT_CONTROL_SPEED = 0.3
-
-
-def _get_param_str(params: Params, key: str, default: str = "") -> str:
-  try:
-    val = params.get(key)
-  except Exception:
-    return default
-  if val is None:
-    return default
-  if isinstance(val, bytes):
-    try:
-      return val.decode("utf-8")
-    except Exception:
-      return default
-  if isinstance(val, (dict, list)):
-    return default
-  return str(val)
-
-
-def _canonical_model_id(model_id: str) -> str:
-  key = (model_id or "").strip().lower()
-  return BUILTIN_MODEL_KEY if key in BUILTIN_MODEL_ALIASES else key
-
-
-def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
-                          lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool, starpilot_toggles) -> log.ModelDataV2.Action:
-    plan = model_output['plan'][0]
-    if 'planplus' in model_output:
-      recovery_power = getattr(starpilot_toggles, "recovery_power", 1.0)
-      plan = plan + recovery_power * model_output['planplus'][0]
-      cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {recovery_power}")
-    desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0],
-                                                                 plan[:,Plan.ACCELERATION][:,0],
-                                                                 ModelConstants.T_IDXS,
-                                                                 action_t=long_action_t)
-    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
-
-    if is_v9:
-      # V9: use desired_curvature if present; otherwise do NOT fall back to plan
-      if 'desired_curvature' in model_output:
-        desired_curvature = float(model_output['desired_curvature'][0, 0])
-      else:
-        desired_curvature = prev_action.desiredCurvature
-    else:
-      desired_curvature = get_curvature_from_output(model_output, plan, v_ego, lat_action_t, mlsim=mlsim)
-    if v_ego > MIN_LAT_CONTROL_SPEED:
-      desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
-    else:
-      desired_curvature = prev_action.desiredCurvature
-
-    return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
-                                  desiredAcceleration=float(desired_accel),
-                                  shouldStop=bool(should_stop))
-
-class FrameMeta:
-  frame_id: int = 0
-  timestamp_sof: int = 0
-  timestamp_eof: int = 0
-
-  def __init__(self, vipc=None):
-    if vipc is not None:
-      self.frame_id, self.timestamp_sof, self.timestamp_eof = vipc.frame_id, vipc.timestamp_sof, vipc.timestamp_eof
-
-class ModelState:
-  frames: dict[str, DrivingModelFrame]
-  inputs: dict[str, np.ndarray]
-  output: np.ndarray
-  prev_desire: np.ndarray  # for tracking the rising edge of the pulse
-
-  def _build_policy_inputs(self, input_shapes: dict[str, tuple[int, ...]]) -> tuple[dict[str, np.ndarray], str | None]:
-    numpy_inputs: dict[str, np.ndarray] = {}
-
-    # Always-supported inputs (if model expects them)
-    desire_key_init = next((k for k in input_shapes if k.startswith('desire')), None)
-    if desire_key_init:
-      numpy_inputs[desire_key_init] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
-    if 'traffic_convention' in input_shapes:
-      numpy_inputs['traffic_convention'] = np.zeros((1, ModelConstants.TRAFFIC_CONVENTION_LEN), dtype=np.float32)
-    if 'features_buffer' in input_shapes:
-      numpy_inputs['features_buffer'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN,  ModelConstants.FEATURE_LEN), dtype=np.float32)
-
-    # Optional inputs for non-v11 (and some v10/v9 variants)
-    # Lateral control params
-    if 'lateral_control_params' in input_shapes:
-      numpy_inputs['lateral_control_params'] = np.zeros((1, ModelConstants.LATERAL_CONTROL_PARAMS_LEN), dtype=np.float32)
-
-    # Previous desired curvature: handle both singular and plural key names across model versions
-    prev_desired_curv_key = None
-    if 'prev_desired_curv' in input_shapes:
-      prev_desired_curv_key = 'prev_desired_curv'
-      numpy_inputs['prev_desired_curv'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
-    elif 'prev_desired_curvs' in input_shapes:
-      prev_desired_curv_key = 'prev_desired_curvs'
-      numpy_inputs['prev_desired_curvs'] = np.zeros((1, ModelConstants.INPUT_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
-
-    return numpy_inputs, prev_desired_curv_key
-
-  def __init__(self, context: CLContext):
-    # Dynamically build paths based on current model ID
-    params = Params()
-    model_id_raw = _get_param_str(params, "Model")
-    if not model_id_raw:
-      model_id_raw = _get_param_str(params, "DrivingModel", BUILTIN_MODEL_KEY)
-    model_id = _canonical_model_id(model_id_raw)
-
-    model_version = _get_param_str(params, "ModelVersion")
-    if not model_version:
-      model_version = _get_param_str(params, "DrivingModelVersion")
-
-    model_dir = MODELS_PATH
-    use_builtin_model = model_id == BUILTIN_MODEL_KEY
-    model_download_id = model_id
-    if use_builtin_model and (_canonical_model_id(_get_param_str(params, "Model")) != model_id or
-                              _canonical_model_id(_get_param_str(params, "DrivingModel")) != model_id):
-      params.put("Model", model_id)
-      params.put("DrivingModel", model_id)
-
-    if use_builtin_model and not model_version:
-      model_version = "v11"
-      params.put("ModelVersion", model_version)
-      params.put("DrivingModelVersion", model_version)
-    # Use built-in files for defaults when a custom model isn't selected.
-    if use_builtin_model:
-        models_dir = Path(__file__).resolve().parents[2] / "selfdrive/modeld/models"
-        VISION_PKL_PATH = models_dir / "driving_vision_tinygrad.pkl"
-        POLICY_PKL_PATH = models_dir / "driving_policy_tinygrad.pkl"
-        OFF_POLICY_PKL_PATH = models_dir / "driving_off_policy_tinygrad.pkl"
-        VISION_METADATA_PATH = models_dir / "driving_vision_metadata.pkl"
-        POLICY_METADATA_PATH = models_dir / "driving_policy_metadata.pkl"
-        OFF_POLICY_METADATA_PATH = models_dir / "driving_off_policy_metadata.pkl"
-    else:
-        VISION_PKL_PATH = model_dir / f"{model_id}_driving_vision_tinygrad.pkl"
-        POLICY_PKL_PATH = model_dir / f"{model_id}_driving_policy_tinygrad.pkl"
-        OFF_POLICY_PKL_PATH = model_dir / f"{model_id}_driving_off_policy_tinygrad.pkl"
-        VISION_METADATA_PATH = model_dir / f"{model_id}_driving_vision_metadata.pkl"
-        POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_policy_metadata.pkl"
-        OFF_POLICY_METADATA_PATH = model_dir / f"{model_id}_driving_off_policy_metadata.pkl"
-
-    def ensure_artifact(path: Path, suffix: str | None = None, optional: bool = False) -> Path | None:
-      if path.is_file():
-        return path
-
-      if use_builtin_model:
-        if optional:
-          cloudlog.warning(f"Optional builtin model artifact missing: {path}")
-          return None
-        raise FileNotFoundError(
-          f"Missing builtin model artifact: {path}. "
-          "Rebuild model artifacts locally (./build or scons target) and deploy them."
-        )
-
-      cloudlog.error(f"Missing model artifact {path}, downloading {model_download_id}...")
-      from openpilot.starpilot.assets.model_manager import ModelManager
-      ModelManager(params, params_memory).download_model(model_download_id)
-
-      if path.is_file():
-        return path
-
-      if optional:
-        cloudlog.warning(f"Optional model artifact missing: {path}")
-        return None
-
-      raise FileNotFoundError(path)
-
-    # If ModelVersion is not set or not available, try to determine it from available model data
-    if not model_version:
-      cloudlog.warning(f"ModelVersion not available for model {model_id}, attempting to determine from model data")
-      try:
-        # Try to get version from the model versions JSON file
-        versions_file = model_dir / ".model_versions.json"
-        if versions_file.is_file():
-          import json
-          with open(versions_file, "r") as f:
-            version_map = json.load(f)
-          version_lookup_keys = [model_id]
-          if model_id and not model_id.endswith("2"):
-            version_lookup_keys.append(f"{model_id}2")
-          for key in version_lookup_keys:
-            if key in version_map:
-              model_version = version_map[key]
-              cloudlog.warning(f"Determined model version from JSON: {model_version} ({key})")
-              params.put("ModelVersion", model_version)
-              params.put("DrivingModelVersion", model_version)
-              break
-        else:
-          cloudlog.error("Model versions JSON file not found, defaulting to v8")
-          model_version = "v8"
-      except Exception as e:
-        cloudlog.error(f"Failed to determine model version: {e}, defaulting to v8")
-        model_version = "v8"
-
-    VISION_METADATA_PATH = ensure_artifact(VISION_METADATA_PATH, "driving_vision_metadata.pkl")
-    with open(VISION_METADATA_PATH, 'rb') as f:
-      vision_metadata = pickle.load(f)
-    self.vision_input_shapes =  vision_metadata['input_shapes']
-    self.vision_input_names = list(self.vision_input_shapes.keys())
-    self.vision_output_slices = vision_metadata['output_slices']
-    vision_output_size = vision_metadata['output_shapes']['outputs'][1]
-
-    POLICY_METADATA_PATH = ensure_artifact(POLICY_METADATA_PATH, "driving_policy_metadata.pkl")
-    with open(POLICY_METADATA_PATH, 'rb') as f:
-      policy_metadata = pickle.load(f)
-    self.policy_input_shapes =  policy_metadata['input_shapes']
-    self.policy_output_slices = policy_metadata['output_slices']
-    policy_output_size = policy_metadata['output_shapes']['outputs'][1]
-    # Add policy_generation attribute after loading policy_metadata
-    self.policy_generation = model_version or "v8"
-    self.is_v11 = (self.policy_generation == "v11")
-    self.is_v10 = (self.policy_generation == "v10")
-    self.is_v12 = (self.policy_generation == "v12")
-    self.is_v9 = (self.policy_generation == "v9")
-    self.mlsim = (self.policy_generation in ("v8", "v10", "v11", "v12"))
-
-    self.frames = {name: DrivingModelFrame(context, ModelConstants.TEMPORAL_SKIP) for name in self.vision_input_names}
-    self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
-
-    self.full_features_buffer = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN,  ModelConstants.FEATURE_LEN), dtype=np.float32)
-    self.full_desire = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.DESIRE_LEN), dtype=np.float32)
-    self.temporal_idxs = slice(-1-(ModelConstants.TEMPORAL_SKIP*(ModelConstants.INPUT_HISTORY_BUFFER_LEN-1)), None, ModelConstants.TEMPORAL_SKIP)
-
-
-    # policy inputs (built dynamically to support all generations)
-    self.numpy_inputs, self.prev_desired_curv_key = self._build_policy_inputs(self.policy_input_shapes)
-
-    # Off-policy model (optional)
-    self.off_policy_enabled = False
-    self.off_policy_input_shapes: dict[str, tuple[int, ...]] = {}
-    self.off_policy_output_slices: dict[str, slice] = {}
-    self.off_policy_numpy_inputs: dict[str, np.ndarray] = {}
-    self.off_policy_prev_desired_curv_key: str | None = None
-    self.off_policy_desire_key: str | None = None
-    self.off_policy_inputs: dict[str, Tensor] | None = None
-    self.off_policy_output: np.ndarray | None = None
-
-    off_policy_metadata = None
-    if self.policy_generation == "v12" or OFF_POLICY_METADATA_PATH.is_file() or OFF_POLICY_PKL_PATH.is_file():
-      resolved_off_policy_meta = ensure_artifact(OFF_POLICY_METADATA_PATH, "driving_off_policy_metadata.pkl", optional=True)
-      if resolved_off_policy_meta is not None:
-        with open(resolved_off_policy_meta, 'rb') as f:
-          off_policy_metadata = pickle.load(f)
-
-    if off_policy_metadata is not None:
-      self.off_policy_input_shapes = off_policy_metadata['input_shapes']
-      self.off_policy_output_slices = off_policy_metadata['output_slices']
-      off_policy_output_size = off_policy_metadata['output_shapes']['outputs'][1]
-      self.off_policy_numpy_inputs, self.off_policy_prev_desired_curv_key = self._build_policy_inputs(self.off_policy_input_shapes)
-      self.off_policy_desire_key = next((k for k in self.off_policy_numpy_inputs if k.startswith('desire')), None)
-      self.off_policy_inputs = {k: Tensor(v, device='NPY').realize() for k, v in self.off_policy_numpy_inputs.items()}
-      self.off_policy_output = np.zeros(off_policy_output_size, dtype=np.float32)
-      resolved_off_policy_pkl = ensure_artifact(OFF_POLICY_PKL_PATH, "driving_off_policy_tinygrad.pkl", optional=True)
-      if resolved_off_policy_pkl is not None:
-        with open(resolved_off_policy_pkl, "rb") as f:
-          self.off_policy_run = pickle.load(f)
-        self.off_policy_enabled = True
-
-    # Optional temporal buffer for previous desired curvature (allocate only if any model expects it)
-    if self.prev_desired_curv_key is not None or self.off_policy_prev_desired_curv_key is not None:
-      self.full_prev_desired_curv = np.zeros((1, ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
-
-
-    # img buffers are managed in openCL transform code
-    self.vision_inputs: dict[str, Tensor] = {}
-    self.vision_output = np.zeros(vision_output_size, dtype=np.float32)
-    self.policy_inputs = {k: Tensor(v, device='NPY').realize() for k,v in self.numpy_inputs.items()}
-    self.policy_output = np.zeros(policy_output_size, dtype=np.float32)
-    self.parser = Parser()
-    self.off_policy_parser = Parser(ignore_missing=True)
-
-    VISION_PKL_PATH = ensure_artifact(VISION_PKL_PATH, "driving_vision_tinygrad.pkl")
-    with open(VISION_PKL_PATH, "rb") as f:
-      self.vision_run = pickle.load(f)
-
-    POLICY_PKL_PATH = ensure_artifact(POLICY_PKL_PATH, "driving_policy_tinygrad.pkl")
-    with open(POLICY_PKL_PATH, "rb") as f:
-      self.policy_run = pickle.load(f)
-
-  @property
-  def desire_key(self) -> str:
-    return next(key for key in self.numpy_inputs if key.startswith('desire'))
-
-  def slice_outputs(self, model_outputs: np.ndarray, output_slices: dict[str, slice]) -> dict[str, np.ndarray]:
-    parsed_model_outputs = {k: model_outputs[np.newaxis, v] for k,v in output_slices.items()}
-    return parsed_model_outputs
-
-  def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
-                inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
-    # Model decides when action is completed, so desire input is just a pulse triggered on rising edge
-    inputs[self.desire_key][0] = 0
-    new_desire = np.where(inputs[self.desire_key] - self.prev_desire > .99, inputs[self.desire_key], 0)
-    self.prev_desire[:] = inputs[self.desire_key]
-
-    self.full_desire[0,:-1] = self.full_desire[0,1:]
-    self.full_desire[0,-1] = new_desire
-    self.numpy_inputs[self.desire_key][:] = self.full_desire.reshape((1,ModelConstants.INPUT_HISTORY_BUFFER_LEN,ModelConstants.TEMPORAL_SKIP,-1)).max(axis=2)
-    if self.off_policy_enabled and self.off_policy_desire_key is not None:
-      self.off_policy_numpy_inputs[self.off_policy_desire_key][:] = self.numpy_inputs[self.desire_key]
-
-    if 'traffic_convention' in self.numpy_inputs:
-      self.numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
-    if self.off_policy_enabled and 'traffic_convention' in self.off_policy_numpy_inputs:
-      self.off_policy_numpy_inputs['traffic_convention'][:] = inputs['traffic_convention']
-
-    if 'lateral_control_params' in self.numpy_inputs:
-      self.numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
-    if self.off_policy_enabled and 'lateral_control_params' in self.off_policy_numpy_inputs:
-      self.off_policy_numpy_inputs['lateral_control_params'][:] = inputs['lateral_control_params']
-
-    if prepare_only:
-      return None
-
-    imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.vision_input_names}
-
-    if TICI:
-      # The imgs tensors are backed by opencl memory, only need init once
-      for key in imgs_cl:
-        if key not in self.vision_inputs:
-          self.vision_inputs[key] = qcom_tensor_from_opencl_address(imgs_cl[key].mem_address, self.vision_input_shapes[key], dtype=dtypes.uint8)
-    else:
-      for key in imgs_cl:
-        frame_input = self.frames[key].buffer_from_cl(imgs_cl[key]).reshape(self.vision_input_shapes[key])
-        self.vision_inputs[key] = Tensor(frame_input, dtype=dtypes.uint8).realize()
-
-    self.vision_output = self.vision_run(**self.vision_inputs).contiguous().realize().uop.base.buffer.numpy()
-    vision_outputs_dict = self.parser.parse_vision_outputs(self.slice_outputs(self.vision_output, self.vision_output_slices))
-
-    self.full_features_buffer[0,:-1] = self.full_features_buffer[0,1:]
-    self.full_features_buffer[0,-1] = vision_outputs_dict['hidden_state'][0, :]
-    if 'features_buffer' in self.numpy_inputs:
-      self.numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
-    if self.off_policy_enabled and 'features_buffer' in self.off_policy_numpy_inputs:
-      self.off_policy_numpy_inputs['features_buffer'][:] = self.full_features_buffer[0, self.temporal_idxs]
-
-    self.policy_output = self.policy_run(**self.policy_inputs).contiguous().realize().uop.base.buffer.numpy()
-    policy_outputs_dict = self.parser.parse_policy_outputs(self.slice_outputs(self.policy_output, self.policy_output_slices))
-
-    # TODO model only uses last value now
-    if hasattr(self, 'full_prev_desired_curv') and 'desired_curvature' in policy_outputs_dict:
-      self.full_prev_desired_curv[0,:-1] = self.full_prev_desired_curv[0,1:]
-      self.full_prev_desired_curv[0,-1,:] = policy_outputs_dict['desired_curvature'][0, :]
-
-      if self.prev_desired_curv_key is not None:
-        # v9/v10/v11/v12 models expect zeros for prev_desired_curv(s); others use history
-        if self.is_v9 or self.is_v10 or self.is_v11 or self.is_v12:
-          self.numpy_inputs[self.prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
-        else:
-          self.numpy_inputs[self.prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
-
-      if self.off_policy_enabled and self.off_policy_prev_desired_curv_key is not None:
-        if self.is_v9 or self.is_v12:
-          self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = 0 * self.full_prev_desired_curv[0, self.temporal_idxs]
-        else:
-          self.off_policy_numpy_inputs[self.off_policy_prev_desired_curv_key][:] = self.full_prev_desired_curv[0, self.temporal_idxs]
-
-    combined_outputs_dict = {**vision_outputs_dict}
-    if self.off_policy_enabled:
-      self.off_policy_output = self.off_policy_run(**self.off_policy_inputs).contiguous().realize().uop.base.buffer.numpy()
-      off_policy_outputs_dict = self.off_policy_parser.parse_policy_outputs(
-        self.slice_outputs(self.off_policy_output, self.off_policy_output_slices)
-      )
-      off_policy_outputs_dict.pop('plan', None)
-      combined_outputs_dict = {**combined_outputs_dict, **off_policy_outputs_dict, **policy_outputs_dict}
-    else:
-      combined_outputs_dict = {**combined_outputs_dict, **policy_outputs_dict}
-    if SEND_RAW_PRED:
-      raw_pred = [self.vision_output.copy(), self.policy_output.copy()]
-      if self.off_policy_enabled and self.off_policy_output is not None:
-        raw_pred.append(self.off_policy_output.copy())
-      combined_outputs_dict['raw_pred'] = np.concatenate(raw_pred)
-
-    return combined_outputs_dict
-
-
-def main(demo=False):
-  cloudlog.warning("modeld init")
-
-  sentry.set_tag("daemon", PROCESS_NAME)
-  cloudlog.bind(daemon=PROCESS_NAME)
-  setproctitle(PROCESS_NAME)
-  config_realtime_process(7, 54)
-
-  cloudlog.warning("setting up CL context")
-  cl_context = CLContext()
-  cloudlog.warning("CL context ready; loading model")
-  model = ModelState(cl_context)
-  cloudlog.warning("models loaded, modeld starting")
-
-  # visionipc clients
-  while True:
-    available_streams = VisionIpcClient.available_streams("camerad", block=False)
-    if available_streams:
-      use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_ROAD in available_streams
-      main_wide_camera = VisionStreamType.VISION_STREAM_ROAD not in available_streams
-      break
-    time.sleep(.1)
-
-  vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_ROAD
-  vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True, cl_context)
-  vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
-  cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
-
-  while not vipc_client_main.connect(False):
-    time.sleep(0.1)
-  while use_extra_client and not vipc_client_extra.connect(False):
-    time.sleep(0.1)
-
-  cloudlog.warning(f"connected main cam with buffer size: {vipc_client_main.buffer_len} ({vipc_client_main.width} x {vipc_client_main.height})")
-  if use_extra_client:
-    cloudlog.warning(f"connected extra cam with buffer size: {vipc_client_extra.buffer_len} ({vipc_client_extra.width} x {vipc_client_extra.height})")
-
-  # messaging
-  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "liveDelay", "starpilotPlan"])
-
-  publish_state = PublishState()
-  params = Params()
-
-  # setup filter to track dropped frames
-  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / ModelConstants.MODEL_FREQ)
-  frame_id = 0
-  last_vipc_frame_id = 0
-  run_count = 0
-
-  model_transform_main = np.zeros((3, 3), dtype=np.float32)
-  model_transform_extra = np.zeros((3, 3), dtype=np.float32)
-  live_calib_seen = False
-  buf_main, buf_extra = None, None
-  meta_main = FrameMeta()
-  meta_extra = FrameMeta()
-
-
-  if demo:
-    CP = get_demo_car_params()
-  else:
-    CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
-  cloudlog.info("tinygrad_modeld got CarParams: %s", CP.brand)
-
-  # TODO this needs more thought, use .2s extra for now to estimate other delays
-  # TODO Move smooth seconds to action function
-  long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
-  prev_action = log.ModelDataV2.Action()
-
-  DH = DesireHelper()
-
-  starpilot_toggles = get_starpilot_toggles(sm)
-
-  while True:
-    # Keep receiving frames until we are at least 1 frame ahead of previous extra frame
-    while meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
-      buf_main = vipc_client_main.recv()
-      meta_main = FrameMeta(vipc_client_main)
-      if buf_main is None:
-        break
-
-    if buf_main is None:
-      cloudlog.debug("vipc_client_main no frame")
-      continue
-
-    if use_extra_client:
-      # Keep receiving extra frames until frame id matches main camera
-      while True:
-        buf_extra = vipc_client_extra.recv()
-        meta_extra = FrameMeta(vipc_client_extra)
-        if buf_extra is None or meta_main.timestamp_sof < meta_extra.timestamp_sof + 25000000:
-          break
-
-      if buf_extra is None:
-        cloudlog.debug("vipc_client_extra no frame")
-        continue
-
-      if abs(meta_main.timestamp_sof - meta_extra.timestamp_sof) > 10000000:
-        cloudlog.error(f"frames out of sync! main: {meta_main.frame_id} ({meta_main.timestamp_sof / 1e9:.5f}),\
-                         extra: {meta_extra.frame_id} ({meta_extra.timestamp_sof / 1e9:.5f})")
-
-    else:
-      # Use single camera
-      buf_extra = buf_main
-      meta_extra = meta_main
-
-    sm.update(0)
-    desire = DH.desire
-    is_rhd = sm["driverMonitoringState"].isRHD
-    frame_id = sm["roadCameraState"].frameId
-    v_ego = max(sm["carState"].vEgo, 0.)
-    lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
-    lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
-    if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
-      device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
-      dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
-      model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
-      model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
-      live_calib_seen = True
-
-    traffic_convention = np.zeros(2)
-    traffic_convention[int(is_rhd)] = 1
-
-    vec_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
-    if desire >= 0 and desire < ModelConstants.DESIRE_LEN:
-      vec_desire[desire] = 1
-
-    # tracked dropped frames
-    vipc_dropped_frames = max(0, meta_main.frame_id - last_vipc_frame_id - 1)
-    frames_dropped = frame_dropped_filter.update(min(vipc_dropped_frames, 10))
-    if run_count < 10: # let frame drops warm up
-      frame_dropped_filter.x = 0.
-      frames_dropped = 0.
-    run_count = run_count + 1
-
-    frame_drop_ratio = frames_dropped / (1 + frames_dropped)
-    prepare_only = vipc_dropped_frames > 0
-    if prepare_only:
-      cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")
-
-    bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
-    transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
-
-    inputs:dict[str, np.ndarray] = {
-      model.desire_key: vec_desire,
-      'traffic_convention': traffic_convention,
-    }
-    # Include optional inputs only if the loaded model expects them
-    if 'lateral_control_params' in model.numpy_inputs:
-      inputs['lateral_control_params'] = lateral_control_params
-
-    mt1 = time.perf_counter()
-    model_output = model.run(bufs, transforms, inputs, prepare_only)
-    mt2 = time.perf_counter()
-    model_execution_time = mt2 - mt1
-
-    if model_output is not None:
-      modelv2_send = messaging.new_message('modelV2')
-      starpilot_modelv2_send = messaging.new_message('starpilotModelV2')
-      drivingdata_send = messaging.new_message('drivingModelData')
-      posenet_send = messaging.new_message('cameraOdometry')
-
-      action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, model.mlsim, model.is_v9, starpilot_toggles)
-      prev_action = action
-      fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
-                     publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,
-                     frame_drop_ratio, meta_main.timestamp_eof, model_execution_time, live_calib_seen)
-
-      desire_state = modelv2_send.modelV2.meta.desireState
-      l_lane_change_prob = desire_state[log.Desire.laneChangeLeft]
-      r_lane_change_prob = desire_state[log.Desire.laneChangeRight]
-      lane_change_prob = l_lane_change_prob + r_lane_change_prob
-      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['starpilotPlan'], starpilot_toggles)
-      modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
-      modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
-      starpilot_modelv2_send.starpilotModelV2.turnDirection = DH.turn_direction
-      drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
-      drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
-
-      fill_pose_msg(posenet_send, model_output, meta_main.frame_id, vipc_dropped_frames, meta_main.timestamp_eof, live_calib_seen)
-      pm.send('modelV2', modelv2_send)
-      pm.send('starpilotModelV2', starpilot_modelv2_send)
-      pm.send('drivingModelData', drivingdata_send)
-      pm.send('cameraOdometry', posenet_send)
-    last_vipc_frame_id = meta_main.frame_id
-
-    # Update planner-driven parameters
-    if sm.updated['starpilotPlan']:
-      starpilot_toggles = get_starpilot_toggles(sm)
-
-if __name__ == "__main__":
-  try:
-    import argparse
-    parser = argparse.ArgumentParser()
-    parser.add_argument('--demo', action='store_true', help='A boolean for demo mode.')
-    args = parser.parse_args()
-    main(demo=args.demo)
-  except KeyboardInterrupt:
-    cloudlog.warning(f"child {PROCESS_NAME} got SIGINT")
-  except Exception:
-    sentry.capture_exception()
-    raise

starpilot/tinygrad_modeld/transforms/loadyuv.cc

@@ -1,76 +0,0 @@
-#include "starpilot/tinygrad_modeld/transforms/loadyuv.h"
-
-#include <cassert>
-#include <cstdio>
-#include <cstring>
-
-void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) {
-  memset(s, 0, sizeof(*s));
-
-  s->width = width;
-  s->height = height;
-
-  char args[1024];
-  snprintf(args, sizeof(args),
-           "-cl-fast-relaxed-math -cl-denorms-are-zero "
-           "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d",
-           width, height);
-  cl_program prg = cl_program_from_file(ctx, device_id, LOADYUV_PATH, args);
-
-  s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
-  s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
-  s->copy_krnl = CL_CHECK_ERR(clCreateKernel(prg, "copy", &err));
-
-  // done with this
-  CL_CHECK(clReleaseProgram(prg));
-}
-
-void loadyuv_destroy(LoadYUVState* s) {
-  CL_CHECK(clReleaseKernel(s->loadys_krnl));
-  CL_CHECK(clReleaseKernel(s->loaduv_krnl));
-  CL_CHECK(clReleaseKernel(s->copy_krnl));
-}
-
-void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
-                   cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
-                   cl_mem out_cl) {
-  cl_int global_out_off = 0;
-
-  CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
-  CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
-  CL_CHECK(clSetKernelArg(s->loadys_krnl, 2, sizeof(cl_int), &global_out_off));
-
-  const size_t loadys_work_size = (s->width*s->height)/8;
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
-                               &loadys_work_size, NULL, 0, 0, NULL));
-
-  const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
-  global_out_off += (s->width*s->height);
-
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
-
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
-                               &loaduv_work_size, NULL, 0, 0, NULL));
-
-  global_out_off += (s->width/2)*(s->height/2);
-
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
-
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
-                               &loaduv_work_size, NULL, 0, 0, NULL));
-}
-
-void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst,
-                 size_t src_offset, size_t dst_offset, size_t size) {
-  CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &src));
-  CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_mem), &dst));
-  CL_CHECK(clSetKernelArg(s->copy_krnl, 2, sizeof(cl_int), &src_offset));
-  CL_CHECK(clSetKernelArg(s->copy_krnl, 3, sizeof(cl_int), &dst_offset));
-  const size_t copy_work_size = size/8;
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
-                              &copy_work_size, NULL, 0, 0, NULL));
-}

starpilot/tinygrad_modeld/transforms/loadyuv.cl

@@ -1,47 +0,0 @@
-#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
-
-__kernel void loadys(__global uchar8 const * const Y,
-                     __global uchar * out,
-                     int out_offset)
-{
-    const int gid = get_global_id(0);
-    const int ois = gid * 8;
-    const int oy = ois / TRANSFORMED_WIDTH;
-    const int ox = ois % TRANSFORMED_WIDTH;
-
-    const uchar8 ys = Y[gid];
-
-    // 02
-    // 13
-
-    __global uchar* outy0;
-    __global uchar* outy1;
-    if ((oy & 1) == 0) {
-      outy0 = out + out_offset; //y0
-      outy1 = out + out_offset + UV_SIZE*2; //y2
-    } else {
-      outy0 = out + out_offset + UV_SIZE; //y1
-      outy1 = out + out_offset + UV_SIZE*3; //y3
-    }
-
-    vstore4(ys.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
-    vstore4(ys.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
-}
-
-__kernel void loaduv(__global uchar8 const * const in,
-                     __global uchar8 * out,
-                     int out_offset)
-{
-  const int gid = get_global_id(0);
-  const uchar8 inv = in[gid];
-  out[gid + out_offset / 8] = inv;
-}
-
-__kernel void copy(__global uchar8 * in,
-                   __global uchar8 * out,
-                   int in_offset,
-                   int out_offset)
-{
-  const int gid = get_global_id(0);
-  out[gid + out_offset / 8] = in[gid + in_offset / 8];
-}

starpilot/tinygrad_modeld/transforms/loadyuv.h

@@ -1,20 +0,0 @@
-#pragma once
-
-#include "common/clutil.h"
-
-typedef struct {
-  int width, height;
-  cl_kernel loadys_krnl, loaduv_krnl, copy_krnl;
-} LoadYUVState;
-
-void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height);
-
-void loadyuv_destroy(LoadYUVState* s);
-
-void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
-                   cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
-                   cl_mem out_cl);
-
-
-void copy_queue(LoadYUVState* s, cl_command_queue q, cl_mem src, cl_mem dst,
-                 size_t src_offset, size_t dst_offset, size_t size);

starpilot/tinygrad_modeld/transforms/transform.cc

@@ -1,97 +0,0 @@
-#include "starpilot/tinygrad_modeld/transforms/transform.h"
-
-#include <cassert>
-#include <cstring>
-
-#include "common/clutil.h"
-
-void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
-  memset(s, 0, sizeof(*s));
-
-  cl_program prg = cl_program_from_file(ctx, device_id, TRANSFORM_PATH, "");
-  s->krnl = CL_CHECK_ERR(clCreateKernel(prg, "warpPerspective", &err));
-  // done with this
-  CL_CHECK(clReleaseProgram(prg));
-
-  s->m_y_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
-  s->m_uv_cl = CL_CHECK_ERR(clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err));
-}
-
-void transform_destroy(Transform* s) {
-  CL_CHECK(clReleaseMemObject(s->m_y_cl));
-  CL_CHECK(clReleaseMemObject(s->m_uv_cl));
-  CL_CHECK(clReleaseKernel(s->krnl));
-}
-
-void transform_queue(Transform* s,
-                     cl_command_queue q,
-                     cl_mem in_yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
-                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
-                     int out_width, int out_height,
-                     const mat3& projection) {
-  const int zero = 0;
-
-  // sampled using pixel center origin
-  // (because that's how fastcv and opencv does it)
-
-  mat3 projection_y = projection;
-
-  // in and out uv is half the size of y.
-  mat3 projection_uv = transform_scale_buffer(projection, 0.5);
-
-  CL_CHECK(clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL));
-  CL_CHECK(clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL));
-
-  const int in_y_width = in_width;
-  const int in_y_height = in_height;
-  const int in_y_px_stride = 1;
-  const int in_uv_width = in_width/2;
-  const int in_uv_height = in_height/2;
-  const int in_uv_px_stride = 2;
-  const int in_u_offset = in_uv_offset;
-  const int in_v_offset = in_uv_offset + 1;
-
-  const int out_y_width = out_width;
-  const int out_y_height = out_height;
-  const int out_uv_width = out_width/2;
-  const int out_uv_height = out_height/2;
-
-  CL_CHECK(clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv));  // src
-  CL_CHECK(clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_stride));  // src_row_stride
-  CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_px_stride));  // src_px_stride
-  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &zero));  // src_offset
-  CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_height));  // src_rows
-  CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_y_width));  // src_cols
-  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_y));  // dst
-  CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_y_width));  // dst_row_stride
-  CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero));  // dst_offset
-  CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_height));  // dst_rows
-  CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_y_width));  // dst_cols
-  CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_y_cl));  // M
-
-  const size_t work_size_y[2] = {(size_t)out_y_width, (size_t)out_y_height};
-
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
-                              (const size_t*)&work_size_y, NULL, 0, 0, NULL));
-
-  const size_t work_size_uv[2] = {(size_t)out_uv_width, (size_t)out_uv_height};
-
-  CL_CHECK(clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_uv_px_stride));  // src_px_stride
-  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_u_offset));  // src_offset
-  CL_CHECK(clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_height));  // src_rows
-  CL_CHECK(clSetKernelArg(s->krnl, 5, sizeof(cl_int), &in_uv_width));  // src_cols
-  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_u));  // dst
-  CL_CHECK(clSetKernelArg(s->krnl, 7, sizeof(cl_int), &out_uv_width));  // dst_row_stride
-  CL_CHECK(clSetKernelArg(s->krnl, 8, sizeof(cl_int), &zero));  // dst_offset
-  CL_CHECK(clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_height));  // dst_rows
-  CL_CHECK(clSetKernelArg(s->krnl, 10, sizeof(cl_int), &out_uv_width));  // dst_cols
-  CL_CHECK(clSetKernelArg(s->krnl, 11, sizeof(cl_mem), &s->m_uv_cl));  // M
-
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
-                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL));
-  CL_CHECK(clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_v_offset));  // src_ofset
-  CL_CHECK(clSetKernelArg(s->krnl, 6, sizeof(cl_mem), &out_v));  // dst
-
-  CL_CHECK(clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
-                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL));
-}

starpilot/tinygrad_modeld/transforms/transform.cl

@@ -1,54 +0,0 @@
-#define INTER_BITS 5
-#define INTER_TAB_SIZE (1 << INTER_BITS)
-#define INTER_SCALE 1.f / INTER_TAB_SIZE
-
-#define INTER_REMAP_COEF_BITS 15
-#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
-
-__kernel void warpPerspective(__global const uchar * src,
-                              int src_row_stride, int src_px_stride, int src_offset, int src_rows, int src_cols,
-                              __global uchar * dst,
-                              int dst_row_stride, int dst_offset, int dst_rows, int dst_cols,
-                              __constant float * M)
-{
-    int dx = get_global_id(0);
-    int dy = get_global_id(1);
-
-    if (dx < dst_cols && dy < dst_rows)
-    {
-        float X0 = M[0] * dx + M[1] * dy + M[2];
-        float Y0 = M[3] * dx + M[4] * dy + M[5];
-        float W = M[6] * dx + M[7] * dy + M[8];
-        W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
-        int X = rint(X0 * W), Y = rint(Y0 * W);
-
-        int sx = convert_short_sat(X >> INTER_BITS);
-        int sy = convert_short_sat(Y >> INTER_BITS);
-
-        short sx_clamp = clamp(sx, 0, src_cols - 1);
-        short sx_p1_clamp = clamp(sx + 1, 0, src_cols - 1);
-        short sy_clamp = clamp(sy, 0, src_rows - 1);
-        short sy_p1_clamp = clamp(sy + 1, 0, src_rows - 1);
-        int v0 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
-        int v1 = convert_int(src[mad24(sy_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
-        int v2 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_clamp*src_px_stride)]);
-        int v3 = convert_int(src[mad24(sy_p1_clamp, src_row_stride, src_offset + sx_p1_clamp*src_px_stride)]);
-
-        short ay = (short)(Y & (INTER_TAB_SIZE - 1));
-        short ax = (short)(X & (INTER_TAB_SIZE - 1));
-        float taby = 1.f/INTER_TAB_SIZE*ay;
-        float tabx = 1.f/INTER_TAB_SIZE*ax;
-
-        int dst_index = mad24(dy, dst_row_stride, dst_offset + dx);
-
-        int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
-        int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
-        int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
-        int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
-
-        int val = v0 * itab0 +  v1 * itab1 + v2 * itab2 + v3 * itab3;
-
-        uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
-        dst[dst_index] = pix;
-    }
-}

starpilot/tinygrad_modeld/transforms/transform.h

@@ -1,25 +0,0 @@
-#pragma once
-
-#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
-#ifdef __APPLE__
-#include <OpenCL/cl.h>
-#else
-#include <CL/cl.h>
-#endif
-
-#include "common/mat.h"
-
-typedef struct {
-  cl_kernel krnl;
-  cl_mem m_y_cl, m_uv_cl;
-} Transform;
-
-void transform_init(Transform* s, cl_context ctx, cl_device_id device_id);
-
-void transform_destroy(Transform* transform);
-
-void transform_queue(Transform* s, cl_command_queue q,
-                     cl_mem yuv, int in_width, int in_height, int in_stride, int in_uv_offset,
-                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
-                     int out_width, int out_height,
-                     const mat3& projection);

starpilot/tools/compile_models.py

@@ -12,7 +12,7 @@ from openpilot.starpilot.assets.model_manager import CANCEL_DOWNLOAD_PARAM, DOWN
 from openpilot.starpilot.common.starpilot_utilities import delete_file, run_cmd
 from openpilot.starpilot.common.starpilot_variables import MODELS_PATH, RESOURCES_REPO
 
-METADATA_SCRIPT = Path(BASEDIR) / "starpilot/tinygrad_modeld/get_model_metadata.py"
+METADATA_SCRIPT = Path(BASEDIR) / "selfdrive/modeld/get_model_metadata.py"
 TINYGRAD_REPO_PATH = Path(BASEDIR) / "tinygrad_repo"
 
 MODELS_SOURCE = f"https://api.github.com/repos/{RESOURCES_REPO}/contents/uncompiled?ref=Models"

system/manager/process_config.py

@@ -92,8 +92,8 @@ procs = [
   PythonProcess("micd", "system.micd", iscar),
   PythonProcess("timed", "system.timed", always_run, enabled=not PC),
 
-  PythonProcess("modeld", "starpilot.tinygrad_modeld.tinygrad_modeld", only_onroad),
-  PythonProcess("dmonitoringmodeld", "starpilot.tinygrad_modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)),
+  PythonProcess("modeld", "selfdrive.modeld.modeld", only_onroad),
+  PythonProcess("dmonitoringmodeld", "selfdrive.modeld.dmonitoringmodeld", driverview, enabled=(WEBCAM or not PC)),
 
   PythonProcess("sensord", "system.sensord.sensord", only_onroad, enabled=not PC),
   PythonProcess("soundd", "selfdrive.ui.soundd", driverview),