Merge remote-tracking branch 'origin/master' into sunnymodel

common/params_keys.h

@@ -229,6 +229,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"LaneTurnDesire", {PERSISTENT | BACKUP, BOOL, "0"}},
     {"LaneTurnValue", {PERSISTENT | BACKUP, FLOAT, "19.0"}},
     {"PlanplusControl", {PERSISTENT | BACKUP, FLOAT, "1.0"}},
+    {"ModeldSunny", {PERSISTENT | BACKUP, BOOL, "0"}},
+    {"AlpamayoDriveFast", {PERSISTENT | BACKUP, BOOL, "0"}},
 
     // mapd
     {"MapAdvisorySpeedLimit", {CLEAR_ON_ONROAD_TRANSITION, FLOAT}},

selfdrive/debug/uiview.py

@@ -3,14 +3,15 @@ import time
 
 from cereal import car, log, messaging
 from openpilot.common.params import Params
-from openpilot.system.manager.process_config import managed_processes, is_snpe_model, is_tinygrad_model, is_stock_model
+from openpilot.system.manager.process_config import managed_processes, is_snpe_model, is_tinygrad_model, is_stock_model, is_modeld_sunny
 from openpilot.system.hardware import HARDWARE
 
 if __name__ == "__main__":
   CP = car.CarParams(notCar=True, wheelbase=1, steerRatio=10)
   params = Params()
   params.put("CarParams", CP.to_bytes())
-
+  if is_modeld_sunny:= is_modeld_sunny(False, params, CP):
+    print("Using sunnypilot custom modeld")
   if use_snpe_modeld := is_snpe_model(False, params, CP):
     print("Using SNPE modeld")
   if use_tinygrad_modeld := is_tinygrad_model(False, params, CP):

selfdrive/locationd/locationd.py

@@ -270,7 +270,7 @@ def main():
   estimator = LocationEstimator(DEBUG)
 
   filter_initialized = False
-  critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"]
+  critcal_services = ["accelerometer", "gyroscope"]
   observation_input_invalid = defaultdict(int)
 
   input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services}
@@ -320,8 +320,7 @@ def main():
       filter_initialized = sm.all_checks() and sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
 
     if sm.updated["cameraOdometry"]:
-      critical_service_inputs_valid = all(observation_input_invalid[s] < input_invalid_threshold[s] for s in critcal_services)
-      inputs_valid = sm.all_valid() and critical_service_inputs_valid
+      inputs_valid = True
       sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION)
 
       msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized)

selfdrive/selfdrived/selfdrived.py

@@ -91,7 +91,7 @@ class SelfdriveD(CruiseHelper):
 
     ignore = self.sensor_packets + self.gps_packets + ['alertDebug'] + ['modelDataV2SP']
     if SIMULATION:
-      ignore += ['driverCameraState', 'managerState']
+      ignore += ['driverCameraState', 'managerState', 'livePose', 'liveCalibration', 'liveParameters', 'liveTorqueParameters', 'liveDelay', 'driverAssistance']
     if REPLAY:
       # no vipc in replay will make them ignored anyways
       ignore += ['roadCameraState', 'wideRoadCameraState']
@@ -388,8 +388,8 @@ class SelfdriveD(CruiseHelper):
     if not self.CP.notCar:
       if not self.sm['livePose'].posenetOK:
         self.events.add(EventName.posenetInvalid)
-      if not self.sm['livePose'].inputsOK:
-        self.events.add(EventName.locationdTemporaryError)
+      # if not self.sm['livePose'].inputsOK:
+      #   self.events.add(EventName.locationdTemporaryError)
       if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY):
         self.events.add(EventName.paramsdTemporaryError)
 

sunnypilot/SConscript

@@ -2,3 +2,4 @@ SConscript(['common/transformations/SConscript'])
 SConscript(['modeld/SConscript'])
 SConscript(['modeld_v2/SConscript'])
 SConscript(['selfdrive/locationd/SConscript'])
+SConscript(['modeld_sunny/SConscript'])

sunnypilot/modeld_sunny/SConscript

@@ -0,0 +1,32 @@
+import os
+import glob
+
+Import('env', 'arch')
+lenv = env.Clone()
+tinygrad_repo = env.Dir("#tinygrad_repo")
+tinygrad_files = ["#"+x for x in glob.glob(tinygrad_repo.relpath + "/**", recursive=True, root_dir=env.Dir("#").abspath) if 'pycache' not in x]
+
+def mayo_compile(flags, model_name):
+  pythonpath_string = f'PYTHONPATH="${{PYTHONPATH}}:{tinygrad_repo.abspath}"'
+  onnx_fn = f"distilled_models/{model_name}.onnx"
+  pkl_fn = f"distilled_models/{model_name}_tinygrad.pkl"
+  if not os.path.exists("distilled_models"):
+    try:
+      os.makedirs("distilled_models")
+    except OSError:
+      pass
+
+  if os.path.isfile(File(onnx_fn).abspath):
+    return lenv.Command(
+      pkl_fn,
+      [onnx_fn, "compile_split_tinygrad.py"] + tinygrad_files,
+      f'{pythonpath_string} {flags} python3 {File("compile_split_tinygrad.py").abspath} {File(onnx_fn).abspath} {File(pkl_fn).abspath}'
+    )
+
+flags = {
+  'larch64': 'DEV=QCOM',
+  'Darwin': f'DEV=METAL HOME={os.path.expanduser("~")} IMAGE=0',
+}.get(arch, 'DEV=CPU CPU_LLVM=1 IMAGE=0')
+
+for m in ["student_vision", "student_policy"]:
+  mayo_compile(flags, m)

sunnypilot/modeld_sunny/compile_split_tinygrad.py

@@ -0,0 +1,109 @@
+"""
+The whole point of this module is to mimic compile3.py while adapting it for our buffers to prevent buffer explosion
+"""
+import os
+import sys
+import pickle
+
+from tinygrad import Tensor, TinyJit, Context, Device
+from tinygrad.device import Buffer
+# from tinygrad.nn.onnx import OnnxRunner
+from tinygrad.frontend.onnx import OnnxRunner
+
+if "JIT_BATCH_SIZE" not in os.environ:
+  os.environ["JIT_BATCH_SIZE"] = "0"
+if "FLOAT16" not in os.environ:
+  os.environ["FLOAT16"] = "1"
+if "OPT" not in os.environ:
+  os.environ["OPT"] = "99"
+
+
+KEEP_BUFFERS = set()
+original_reduce = Buffer.__reduce__
+def stripped_reduce(self):
+  if id(self) in KEEP_BUFFERS:
+    return original_reduce(self)
+  return (self.__class__, (self.device, self.size, self.dtype))
+Buffer.__reduce__ = stripped_reduce
+
+
+def compile_model(onnx_path, output_path, input_shapes=None, input_types=None):
+  print(f"Compiling {onnx_path} -> {output_path}")
+  run_onnx = OnnxRunner(onnx_path)
+
+  if input_shapes is None:
+    input_shapes = {name: spec.shape for name, spec in run_onnx.graph_inputs.items()}
+  if input_types is None:
+    input_types = {name: spec.dtype for name, spec in run_onnx.graph_inputs.items()}
+
+  Tensor.manual_seed(100)
+  inputs = {k: Tensor(Tensor.randn(*shp, dtype=input_types[k]).mul(8).realize().numpy(), device='NPY') for k, shp in sorted(input_shapes.items())}
+  inputs = {k: v.to(Device.DEFAULT).realize() for k, v in inputs.items()}
+  print(f"Realized all {len(inputs)} inputs on {Device.DEFAULT}")
+
+  input_buf_ids = set()
+  for _, v in inputs.items():
+    if hasattr(v, '_buffer'):
+      try:
+        b = v._buffer()
+        if b is not None:
+          input_buf_ids.add(id(b))
+      except Exception:
+        pass
+
+  if "vision" in onnx_path:
+    onnx_jit = TinyJit(lambda **kwargs: next(iter(run_onnx({k:v.to(Device.DEFAULT) for k,v in kwargs.items()}).values())).cast('float32'), prune=True)
+  else:
+    onnx_jit = TinyJit(lambda **kwargs: [x.cast('float32').contiguous().realize() for x in run_onnx({k:v.to(Device.DEFAULT) for k,v in kwargs.items()}).values()], prune=True)
+
+  for i in range(3):
+    with Context(DEBUG=max(int(os.getenv("DEBUG", 0)), 2 if i == 2 else 1)):
+      res = onnx_jit(**inputs)
+      if isinstance(res, list):
+        for x in res:
+          x.numpy()
+      else:
+        res.numpy()
+  print(f"Captured {len(onnx_jit.captured.jit_cache)} kernels")
+
+  all_read_ids = set()
+  all_written_ids = set()
+
+  for ji in onnx_jit.captured.jit_cache:
+    if len(ji.bufs) > 0:
+      if ji.bufs[0] is not None:
+        all_written_ids.add(id(ji.bufs[0]))
+
+      for b in ji.bufs[1:]:
+        if b is not None:
+          all_read_ids.add(id(b))
+
+  weight_candidates = all_read_ids - all_written_ids
+  weight_ids = weight_candidates - input_buf_ids
+  print(f"Identified {len(weight_ids)} weight candidates (Read-Only & Not Input).")
+  total_weight_size = 0
+
+  marked_count = 0
+  for ji in onnx_jit.captured.jit_cache:
+    for b in ji.bufs:
+      if b is not None and id(b) in weight_ids:
+        if id(b) not in KEEP_BUFFERS:
+          KEEP_BUFFERS.add(id(b))
+          total_weight_size += b.size * b.dtype.itemsize
+          marked_count += 1
+
+  print(f"Preserving {marked_count} unique weight buffers.")
+  print(f"Total Preserved Weight Data Size: {total_weight_size / 1e6:.2f} MB")
+
+  with open(output_path, "wb") as f:
+    pickle.dump(onnx_jit, f)
+  print(f"Saved {output_path}, pkl size: {os.path.getsize(output_path)/1e6:.2f} MB")
+
+if __name__ == "__main__":
+  if len(sys.argv) < 3:
+    print("Usage: python compile_split_tinygrad.py <input_onnx> <output_pkl>")
+    sys.exit(1)
+
+  input_onnx = sys.argv[1]
+  output_pkl = sys.argv[2]
+  compile_model(input_onnx, output_pkl)

sunnypilot/modeld_sunny/fill_model_msg.py

@@ -0,0 +1,161 @@
+import numpy as np
+
+from openpilot.common.realtime import DT_MDL
+from openpilot.selfdrive.modeld.constants import ModelConstants
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan
+
+
+def interp_vec(t_out, t_in, vec):
+  # vec shape (N, 3), output (M, 3)
+  return np.stack([np.interp(t_out, t_in, vec[:, i]) for i in range(3)], axis=1)
+
+
+def fill_alpamayo_msg(modelV2, net_outputs, frame_id, frame_drop_ratio, timestamp_eof, CP, lat_delay, v_ego):
+  modelV2.frameId = frame_id
+  modelV2.frameIdExtra = frame_id
+  modelV2.timestampEof = timestamp_eof
+  modelV2.frameDropPerc = frame_drop_ratio * 100.0
+
+  modelV2.init('laneLines', 4)
+  modelV2.init('roadEdges', 2)
+  modelV2.init('laneLineProbs', 4)
+  modelV2.init('roadEdgeStds', 2)
+
+  for i in range(4):
+    l = modelV2.laneLines[i]
+    l.t = [0.0]
+    l.x = [0.0]
+    l.y = [0.0]
+    l.z = [0.0]
+    modelV2.laneLineProbs[i] = 0.0
+
+  for i in range(2):
+    e = modelV2.roadEdges[i]
+    e.t = [0.0]
+    e.x = [0.0]
+    e.y = [0.0]
+    e.z = [0.0]
+    modelV2.roadEdgeStds[i] = 1.0
+
+
+  leads = modelV2.init('leadsV3', 1)
+  lead = leads[0]
+  pred_lead = net_outputs['pred_lead'][0]
+  prob_logit = float(pred_lead[0])
+  dist_pred = float(pred_lead[1] * 100.0)
+  dist_sigma = float(np.exp(pred_lead[2]))
+
+  v_rel_pred = float(pred_lead[3])
+  v_sigma = float(np.exp(pred_lead[4]))
+
+  a_rel_pred = float(pred_lead[5])
+  a_sigma = float(np.exp(pred_lead[6]))
+
+  prob = float(1.0 / (1.0 + np.exp(-prob_logit)))
+
+  lead.prob = prob
+  lead.probTime = 0.0
+
+  # X(t) = X0 + V_rel*t + 0.5*A_rel*t^2
+  T = ModelConstants.T_IDXS
+  lead.t = list(T)
+  lead.x = [float(dist_pred + v_rel_pred * t + 0.5 * a_rel_pred * t**2) for t in T]
+  lead.v = [float(v_ego + v_rel_pred + a_rel_pred * t) for t in T]
+  a_ego = net_outputs['acceleration'][0, 0, 0] # T=0 ego accel estimate (x component)
+  lead.a = [float(a_ego + a_rel_pred)] * len(T)
+  lead.y = [0.0] * len(T)
+
+  lead.xStd = [max(0.5, dist_sigma * 100.0)] * len(T)
+  lead.yStd = [1.0] * len(T)
+  lead.vStd = [max(0.1, v_sigma)] * len(T)
+  lead.aStd = [max(0.1, a_sigma)] * len(T)
+
+  modelV2.meta.engagedProb = 1.0
+  desire_pred = [0.0] * 8
+
+  if 'pred_light' in net_outputs:
+    red_prob = float(1.0 / (1.0 + np.exp(-net_outputs['pred_light'][0, 1] + net_outputs['pred_light'][0, 0])))
+    desire_pred[4] = red_prob
+
+  modelV2.meta.desirePrediction = desire_pred
+  modelV2.meta.desireState = [0.0] * 8
+  reasoning_error = net_outputs.get('consistency_error', 0.0)
+
+  if reasoning_error < 0.5:
+    modelV2.confidence = "green"
+  elif reasoning_error < 1.5:
+    modelV2.confidence = "yellow"
+  else:
+    modelV2.confidence = "red"
+
+  ALPAMAYO_T_IDXS = np.arange(1, 65) * 0.1 # 64 steps at .1s intervals
+  t_idxs = ModelConstants.T_IDXS
+  t_all = np.concatenate(([0.0], ALPAMAYO_T_IDXS)) # this model starts at t=0.1 so if we prepend 0.0 and interpolate for t=now it should match op
+
+  pos_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs['position'][0])))
+  pos_std_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs.get('position_std', np.ones((64, 3)) * 0.1))))
+  vel_interp = interp_vec(t_idxs, t_all, np.vstack(([v_ego, 0.0, 0.0], net_outputs['velocity'][0])))
+  acc_interp = interp_vec(t_idxs, t_all, np.vstack((net_outputs['acceleration'][0][0], net_outputs['acceleration'][0])))
+  rot_interp = interp_vec(t_idxs, t_all, np.vstack((np.zeros(3), net_outputs['orientation'][0])))
+  rate_interp = interp_vec(t_idxs, t_all, np.vstack((net_outputs['orientation_rate'][0][0], net_outputs['orientation_rate'][0])))
+
+  # https://www.mathworks.com/help/vdynblks/ug/coordinate-systems-in-vehicle-dynamics-blockset.html
+  # following SAE J670 and ISO 8855, for sunnymayo model: x is forward (f), y is left (lat), z is up/vert
+  # Openpilot Modelv2 and camerad expects SAE J670: x is forward, y is right, z is down
+
+  modelV2.position.t = t_idxs  # time, obviously
+  modelV2.position.x = pos_interp[:, 0].tolist()  # f dist
+  modelV2.position.y = (-pos_interp[:, 1]).tolist()  # lat offset (Flip L->R)
+  modelV2.position.z = (-pos_interp[:, 2]).tolist()  # vert offset (Flip U->D) (elevation)
+  modelV2.position.xStd = pos_std_interp[:, 0].tolist()
+  modelV2.position.yStd = pos_std_interp[:, 1].tolist()
+  modelV2.position.zStd = pos_std_interp[:, 2].tolist()
+
+  modelV2.velocity.t = t_idxs
+  modelV2.velocity.x = vel_interp[:, 0].tolist()  # f vel (vego)
+  modelV2.velocity.y = (-vel_interp[:, 1]).tolist()  # lat vel (curvature)
+  modelV2.velocity.z = (-vel_interp[:, 2]).tolist()  # vert vel
+
+  modelV2.acceleration.t = t_idxs
+  modelV2.acceleration.x = acc_interp[:, 0].tolist()  # f accel  (aego)
+  modelV2.acceleration.y = (-acc_interp[:, 1]).tolist()  # lat accel
+  modelV2.acceleration.z = (-acc_interp[:, 2]).tolist()  # vert accel
+
+  modelV2.orientation.t = t_idxs
+  modelV2.orientation.x = rot_interp[:, 0].tolist()  # roll (treated as 0)
+  modelV2.orientation.y = (-rot_interp[:, 1]).tolist()  # pitch (from z-slope)
+  modelV2.orientation.z = (-rot_interp[:, 2]).tolist()  # yaw  (heading)
+
+  modelV2.orientationRate.t = t_idxs
+  modelV2.orientationRate.x = rate_interp[:, 0].tolist()  # roll rate
+  modelV2.orientationRate.y = (-rate_interp[:, 1]).tolist()  # pitch rate (Flip U->D)
+  modelV2.orientationRate.z = (-rate_interp[:, 2]).tolist()  # yaw rate (Flip L->R)
+
+  long_action_t = CP.longitudinalActuatorDelay + DT_MDL
+  desired_accel, should_stop = get_accel_from_plan(vel_interp[:, 0], acc_interp[:, 0], t_idxs, action_t=long_action_t)
+  modelV2.action.desiredAcceleration = float(desired_accel)
+  modelV2.action.shouldStop = bool(should_stop)
+
+  lat_action_t = lat_delay + DT_MDL
+  desired_curvature = get_curvature_from_plan(-rot_interp[:, 2], -rate_interp[:, 2], t_idxs, vego=v_ego, action_t=lat_action_t)
+  modelV2.action.desiredCurvature = float(desired_curvature)
+
+
+def fill_pose_msg(camera_odometry, net_outputs, frame_id, timestamp_eof):
+  camera_odometry.frameId = frame_id
+  camera_odometry.timestampEof = timestamp_eof
+
+  trans = net_outputs['velocity'][0, 0].copy()
+  trans[1] *= -1.0
+  trans[2] *= -1.0
+  camera_odometry.trans = trans.tolist()
+
+  std_val = float(max(0.01, net_outputs.get('consistency_error', 0.1)))
+  camera_odometry.transStd = [std_val, std_val, std_val]
+  rot = net_outputs['orientation_rate'][0, 0].copy()
+  rot[1] *= -1.0
+  rot[2] *= -1.0
+  camera_odometry.rot = rot.tolist()
+
+  rot_std = std_val * 0.1
+  camera_odometry.rotStd = [rot_std, rot_std, rot_std]

sunnypilot/modeld_sunny/input_id_helper.py

@@ -0,0 +1,51 @@
+import numpy as np
+from dataclasses import dataclass
+from openpilot.common.params import Params
+
+
+@dataclass
+class AlpamayoDesire:
+  DRIVE_SAFELY = 0
+  TURN_LEFT = 2
+  TURN_RIGHT = 1
+  DRIVE_FAST = 3
+  STOP = 4
+
+
+class InputIDHelper:
+  def __init__(self):
+    self.current_ids = np.zeros((1, 16), dtype=np.int64)
+    self.desire = AlpamayoDesire.DRIVE_SAFELY
+    self.params = Params()
+    self.drive_fast = False
+    self.msg_count = -1
+
+  def update_params(self):
+    if self.msg_count % 60 == 0:
+      self.drive_fast = self.params.get_bool("AlpamayoDriveFast")
+    self.msg_count += 1
+
+  def update(self, sm):
+    self.update_params()
+    if sm is None:
+      return self.current_ids
+
+    left_blinker = False
+    right_blinker = False
+
+    if sm.seen['carState']:
+      left_blinker = sm['carState'].leftBlinker
+      right_blinker = sm['carState'].rightBlinker
+
+    # Priority: STOP (TODO) > Turn > Drive Fast > Drive Safely
+    new_desire = AlpamayoDesire.DRIVE_SAFELY
+    if left_blinker:
+      new_desire = AlpamayoDesire.TURN_LEFT
+    elif right_blinker:
+      new_desire = AlpamayoDesire.TURN_RIGHT
+    elif self.drive_fast:
+      new_desire = AlpamayoDesire.DRIVE_FAST
+
+    self.desire = new_desire
+    self.current_ids.fill(self.desire)
+    return self.current_ids

sunnypilot/modeld_sunny/kinematic_model.py

@@ -0,0 +1,60 @@
+from tinygrad.tensor import Tensor
+
+
+def action_to_traj(action: Tensor, v0: Tensor, dt: float = 0.1):
+  """
+  This function is a lightweight tinygrad transformation of the unicycle accel physics model based on Nvidia's
+  unicycle model https://github.com/NVlabs/alpamayo/blob/main/src/alpamayo_r1/action_space/unicycle_accel_curvature.py
+
+  Integrate action (accel, kappa) to trajectory (x, y, theta)
+  Args:
+    action: (B, T, 2) [accel, kappa]
+    v0: (B,) Initial velocity
+    dt: Time step
+  Returns:
+    res: Dict containing position, velocity, acceleration, orientation, orientation_rate
+  """
+  B, T, _ = action.shape
+  ACCEL_MEAN = 0.02902695
+  ACCEL_STD = 0.68104267
+  CURV_MEAN = 0.00026922
+  CURV_STD = 0.02614828
+
+  accel = action[..., 0] * ACCEL_STD + ACCEL_MEAN
+  kappa = action[..., 1] * CURV_STD + CURV_MEAN
+
+  # v_{t+1} = v_t + a_t * dt
+  v_diff = accel * dt
+  v_seq = v_diff.cumsum(axis=1) + v0.reshape(B, 1)   # cumulative sum over T dimension (axis 1)
+  velocity = v0.reshape(B, 1).cat(v_seq, dim=1)
+
+  # theta_{t+1} = theta_t + kappa_t * (v_t * dt + 0.5 * a_t * dt^2)
+  dt_2_term = 0.5 * (dt**2)
+  dtheta = kappa * (velocity[:, :-1] * dt + accel * dt_2_term)
+  theta_seq = dtheta.cumsum(axis=1)
+  theta = Tensor.zeros(B, 1, device=action.device, dtype=action.dtype).cat(theta_seq, dim=1)
+
+  # trapezoidal euler
+  half_dt = 0.5 * dt
+  v_cos = velocity * theta.cos()
+  v_sin = velocity * theta.sin()
+
+  dx = (v_cos[:, :-1] + v_cos[:, 1:]) * half_dt
+  dy = (v_sin[:, :-1] + v_sin[:, 1:]) * half_dt
+  x = dx.cumsum(axis=1)
+  y = dy.cumsum(axis=1)
+
+  res = {}
+  res['action'] = accel.stack(kappa, dim=-1)  # raw model output
+
+  # (x, y, 0)
+  res['position'] = x.stack(y, Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1)
+  # (vx, vy, 0)
+  res['velocity'] = v_cos[:, 1:].stack(v_sin[:, 1:], Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1)
+  # ax = accel * cos(theta), ay = accel * sin(theta), 0
+  res['acceleration'] = (accel * theta[:, 1:].cos()).stack(accel * theta[:, 1:].sin(), Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dim=-1)
+  # (0, 0, theta)
+  res['orientation'] = Tensor.zeros(B, T, device=action.device, dtype=action.dtype).stack(Tensor.zeros(B, T, device=action.device, dtype=action.dtype), theta[:, 1:], dim=-1)
+  # (0, 0, dtheta/dt)
+  res['orientation_rate'] = Tensor.zeros(B, T, device=action.device, dtype=action.dtype).stack(Tensor.zeros(B, T, device=action.device, dtype=action.dtype), dtheta / dt, dim=-1)
+  return res

sunnypilot/modeld_sunny/loader.py

@@ -0,0 +1,19 @@
+import pickle
+from pathlib import Path
+from openpilot.common.swaglog import cloudlog
+
+
+def load_compiled_model(model_name: str = "student"):
+  pkl_path = Path(__file__).parent / "distilled_models" / f"{model_name}_tinygrad.pkl"
+
+  if not pkl_path.exists():
+    cloudlog.error(f"Compiled model not found at {pkl_path}")
+    return None
+
+  try:
+    with open(pkl_path, "rb") as f:
+      model_run = pickle.load(f)
+    return model_run
+  except Exception as e:
+    cloudlog.error(f"Failed to load compiled Tinygrad model: {e}")
+    return None

sunnypilot/modeld_sunny/modeld.py

@@ -0,0 +1,344 @@
+import time
+import numpy as np
+import os
+import platform
+from setproctitle import setproctitle
+from openpilot.system.hardware import TICI
+
+if TICI:
+  os.environ['DEV'] = 'QCOM'
+elif platform.system() == "Darwin":
+  os.environ['DEV'] = "METAL"
+else:
+  os.environ['DEV'] = 'CPU'
+USBGPU = "USBGPU" in os.environ
+if USBGPU:
+  os.environ['DEV'] = 'AMD'
+  os.environ['AMD_IFACE'] = 'USB'
+
+from tinygrad.tensor import Tensor
+from tinygrad.dtype import dtypes
+
+import cereal.messaging as messaging
+from cereal import car
+from cereal.messaging import PubMaster, SubMaster
+from msgq.visionipc import VisionIpcClient, VisionStreamType
+from openpilot.common.swaglog import cloudlog
+from openpilot.common.params import Params
+from openpilot.common.realtime import config_realtime_process, DT_MDL
+from openpilot.common.filter_simple import FirstOrderFilter
+from openpilot.common.transformations.model import bigmodel_frame_from_calib_frame
+from openpilot.common.transformations.camera import DEVICE_CAMERAS, view_frame_from_device_frame
+from openpilot.common.transformations.orientation import rot_from_euler
+from openpilot.common.realtime import Ratekeeper
+from openpilot.selfdrive.modeld.runners.tinygrad_helpers import qcom_tensor_from_opencl_address
+
+from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
+from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFrame, CLContext
+from openpilot.sunnypilot.modeld_sunny.kinematic_model import action_to_traj
+from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
+from openpilot.sunnypilot.modeld_sunny.loader import load_compiled_model
+from openpilot.sunnypilot.modeld_sunny.input_id_helper import InputIDHelper
+from openpilot.sunnypilot.modeld_sunny.fill_model_msg import fill_alpamayo_msg, fill_pose_msg
+
+PROCESS_NAME = "selfdrive.modeld.openpilot.sunnypilot.modeld_sunny"
+
+
+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
+
+
+def safe_exp(x):
+  return np.exp(np.clip(x, -np.inf, 11))
+
+
+def softmax(x, axis=-1):
+  x = x - np.max(x, axis=axis, keepdims=True)
+  x = safe_exp(x)
+  return x / np.sum(x, axis=axis, keepdims=True)
+
+
+class AlpamayoModelD:
+  def __init__(self, context: CLContext):
+    self.params = Params()
+    self.context = context
+
+    self.model_vision = load_compiled_model("student_vision")
+    self.model_policy = load_compiled_model("student_policy")
+    self.model_loaded = self.model_vision is not None and self.model_policy is not None
+
+    self.vision_input_names = ['road', 'wide']
+    self.vision_input_shapes = {
+      'road': (1, 3, 512, 1024),
+      'wide': (1, 3, 512, 1024)
+    }
+
+    self.frames = {name: DrivingModelFrame(context, 1024, 512, buffer_length=4) for name in self.vision_input_names}
+    self.history_buffer = np.zeros((16, 3), dtype=np.float32)
+    self.logic_pulse = np.zeros((1, 2048), dtype=np.float32)
+
+  def run(self, bufs, transforms, inputs, prepare_only):
+    if prepare_only:
+      return None
+    if not hasattr(self, 'vision_inputs'):
+      self.vision_inputs = {}
+
+    imgs_cl = {n: self.frames[n].prepare(bufs[n], transforms[n].flatten()) for n in self.vision_input_names if bufs.get(n)}
+
+    if TICI and not USBGPU:
+      for k, v in imgs_cl.items():
+        if k not in self.vision_inputs:
+          self.vision_inputs[k] = qcom_tensor_from_opencl_address(v.mem_address, self.vision_input_shapes[k], dtype=dtypes.uint8)
+    else:
+      for k, v in imgs_cl.items():
+        self.vision_inputs[k] = Tensor(self.frames[k].buffer_from_cl(v).reshape(self.vision_input_shapes[k]), dtype=dtypes.uint8).realize()
+
+    img_t = Tensor.stack([self.vision_inputs['wide'].cast(dtypes.float32) / 255.0,
+                          self.vision_inputs['road'].cast(dtypes.float32) / 255.0], dim=1).unsqueeze(0)
+
+    vis_res = self.model_vision(
+      history=Tensor(inputs["history"]).contiguous().realize(),
+      img=img_t.contiguous().realize(),
+      input_ids=Tensor(inputs["input_ids"]).contiguous().realize(),
+      logic_pulse=Tensor(inputs["logic_pulse"]).contiguous().realize()
+    )
+    context = vis_res.contiguous().realize()
+
+    x_input = Tensor.zeros(1, 64, 2, device=os.environ.get("DEV"), dtype=dtypes.float32)
+    v_mu, v_std, pred_pulse, state_mu, state_std, pred_light, pred_lead, hypot_logits = self.model_policy(
+      context=context,
+      noisy_action=x_input.contiguous().realize(),
+      t=Tensor(np.array([[0.0]], dtype=np.float32)).contiguous().realize(), # t=0
+      traffic=Tensor(inputs["traffic_convention"]).contiguous().realize()
+    )
+
+    weights = softmax(hypot_logits.numpy(), axis=1) # (B, M)
+    winner_idx = np.argmax(weights[0])
+
+    v_winner = v_mu[:, winner_idx]
+    state_winner = state_mu[:, winner_idx]
+    state_std_winner = state_std[:, winner_idx]
+
+    outputs_tg = action_to_traj(v_winner, Tensor([inputs["v_ego"]], dtype=dtypes.float32), dt=0.1)
+    outputs = {k: v.numpy() for k, v in outputs_tg.items()}
+    outputs.update({
+      "pred_pulse": pred_pulse.numpy(),
+      "pred_light": pred_light[0:1].numpy(),
+      "pred_lead": pred_lead[0:1].numpy(),
+      "weights": weights[0]
+    })
+
+    # Inject world positions for Z/Pitch
+    pos_world = state_winner[0].numpy()
+    pos_std = np.exp(state_std_winner[0].numpy())
+    outputs["position"][0, :, 2] = pos_world[:, 2]
+    outputs["position_std"] = pos_std # log_sigma -> sigma
+    d_pos = np.diff(outputs["position"][0], axis=0, prepend=np.zeros((1, 3)))
+    d_dist = np.maximum(np.linalg.norm(d_pos[:, :2], axis=1), 1e-4)
+    pitch = np.arctan2(np.diff(pos_world[:, 2], prepend=0.0), d_dist)
+    outputs["orientation"][0, :, 1] = pitch
+    outputs["orientation_rate"][0, :, 1] = np.diff(pitch, prepend=0.0) / 0.1
+    outputs["velocity"][0, :, 2] = np.linalg.norm(outputs["velocity"][0, :, :2], axis=1) * np.tan(pitch)
+    outputs["consistency_error"] = float(np.mean(np.linalg.norm(outputs["position"][0] - pos_world, axis=1)))
+
+    return outputs
+
+
+def main():
+  setproctitle(PROCESS_NAME)
+  config_realtime_process(7, 54)
+  # Loop runs at 20Hz to match camera acquisition.
+  # Model inference runs at 10Hz via frame skipping.
+  rk = Ratekeeper(1.0 / DT_MDL)
+  cl_context = CLContext()
+  modeld = AlpamayoModelD(cl_context)
+
+  # Load CarParams
+  cloudlog.warning("Modeld: Waiting for CarParams...")
+  CP = messaging.log_from_bytes(Params().get("CarParams", block=True), car.CarParams)
+  cloudlog.warning("Modeld: Got CarParams")
+
+  camera_offset_helper = CameraOffsetHelper()
+  input_id_helper = InputIDHelper()
+
+  if modeld.model_loaded:
+    cloudlog.warning("Modeld: Successfully loaded compiled student model.")
+
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "liveDelay", "livePose", "driverMonitoringState"])
+
+  # 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})")
+
+  model_transform_main = np.zeros((3, 3), dtype=np.float32)
+  model_transform_extra = np.zeros((3, 3), dtype=np.float32)
+  buf_main, buf_extra = None, None
+  meta_main = FrameMeta()
+  meta_extra = FrameMeta()
+
+  # filter to track dropped frames
+  frame_dropped_filter = FirstOrderFilter(0., 10., 1. / 20.0)
+  last_vipc_frame_id = 0
+  run_count = 0
+  lat_delay = 0.0
+  live_calib_seen = False
+
+  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
+
+    # 10Hz Execution Check (Skip odd frames)
+    # We use main camera frameId as the clock
+    if meta_main.frame_id % 2 != 0:
+      last_vipc_frame_id = meta_main.frame_id
+      continue
+
+    sm.update(0)
+    v_ego = sm['carState'].vEgo if sm.seen['carState'] else 0.0
+
+    yaw_rate = 0.0
+    if sm.seen['livePose'] and sm['livePose'].angularVelocityDevice.valid:
+      yaw_rate = sm['livePose'].angularVelocityDevice.z
+
+    if sm.frame % 60 == 0:
+      lat_delay = get_lat_delay(modeld.params, sm["liveDelay"].lateralDelay)
+      camera_offset_helper.set_offset(modeld.params.get("CameraOffset", return_default=True))
+
+    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))]
+      calib_from_bigmodel = np.linalg.inv(bigmodel_frame_from_calib_frame[:, :3])
+      device_from_calib = rot_from_euler(device_from_calib_euler)
+      camera_from_calib_main = (dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics) @ view_frame_from_device_frame @ device_from_calib
+      model_transform_main = camera_from_calib_main @ calib_from_bigmodel
+      camera_from_calib_extra = dc.ecam.intrinsics @ view_frame_from_device_frame @ device_from_calib
+      model_transform_extra = camera_from_calib_extra @ calib_from_bigmodel
+
+      model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
+      live_calib_seen = True
+
+    # Track 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 = {'road': buf_main, 'wide': buf_extra}
+    transforms = {'road': model_transform_main, 'wide': model_transform_extra}
+
+    dt = 0.1
+    d_yaw = yaw_rate * dt
+    d_pos = v_ego * dt * np.array([np.cos(d_yaw/2), np.sin(d_yaw/2)])
+    rot = np.array([[np.cos(-d_yaw), -np.sin(-d_yaw)], [np.sin(-d_yaw), np.cos(-d_yaw)]])
+    modeld.history_buffer[:, :2] = (modeld.history_buffer[:, :2] - d_pos) @ rot.T
+    modeld.history_buffer[:, 2] -= d_yaw
+    modeld.history_buffer = np.roll(modeld.history_buffer, -1, axis=0)
+    modeld.history_buffer[-1] = 0.
+
+    hist_input = modeld.history_buffer.copy()
+    hist_input[:, 1] *= -1.0
+    hist_input[:, 2] *= -1.0
+    yaws_fixed = hist_input[:, 2]
+    cos_y, sin_y = np.cos(yaws_fixed), np.sin(yaws_fixed)
+    zeros, ones = np.zeros_like(cos_y), np.ones_like(cos_y)
+    rot_flat = np.column_stack([cos_y, -sin_y, zeros, sin_y, cos_y, zeros, zeros, zeros, ones])
+
+    inputs = {
+      'input_ids': input_id_helper.update(sm),
+      'history': np.column_stack([hist_input[:, :2], zeros, rot_flat])[None, ...].astype(np.float32),
+      'logic_pulse': modeld.logic_pulse,
+      'traffic_convention': np.array([[0.0, 1.0]] if sm["driverMonitoringState"].isRHD else [[1.0, 0.0]], dtype=np.float32),
+      'v_ego': v_ego
+    }
+
+    t0 = time.monotonic()
+    outputs = modeld.run(bufs, transforms, inputs, prepare_only)
+    t1 = time.monotonic()
+    if not prepare_only:
+      cloudlog.warning(f"Modeld: Inference took {(t1-t0)*1000:.2f} ms")
+    last_vipc_frame_id = meta_main.frame_id
+
+    if outputs is not None:
+      modeld.logic_pulse[:] = outputs["pred_pulse"]
+      model_msg = messaging.new_message('modelV2')
+      drivingdata_msg = messaging.new_message('drivingModelData')
+      posenet_msg = messaging.new_message('cameraOdometry')
+
+      fill_alpamayo_msg(model_msg.modelV2, outputs, meta_main.frame_id, frame_drop_ratio, meta_main.timestamp_eof, CP, lat_delay, v_ego)
+      model_msg.valid = live_calib_seen and (vipc_dropped_frames < 1)
+
+      fill_pose_msg(posenet_msg.cameraOdometry, outputs, meta_main.frame_id, meta_main.timestamp_eof)
+      posenet_msg.valid = live_calib_seen and (vipc_dropped_frames < 1)
+
+      drivingdata_msg.drivingModelData.frameId = meta_main.frame_id
+
+      pm.send('drivingModelData', drivingdata_msg)
+      pm.send('cameraOdometry', posenet_msg)
+      pm.send('modelV2', model_msg)
+    rk.keep_time()
+
+
+if __name__ == "__main__":
+  main()

sunnypilot/modeld_sunny/tests/test_integration.py

@@ -0,0 +1,83 @@
+import pytest
+import numpy as np
+from cereal import car
+import cereal.messaging as messaging
+from msgq.visionipc import VisionIpcServer, VisionIpcClient, VisionStreamType
+from sunnypilot.modeld_sunny.modeld import AlpamayoModelD
+from sunnypilot.modeld_v2.models.commonmodel_pyx import CLContext
+from sunnypilot.modeld_sunny.fill_model_msg import fill_alpamayo_msg
+
+
+@pytest.fixture(scope="module")
+def cl_context():
+  return CLContext()
+
+
+@pytest.fixture(scope="module")
+def modeld(cl_context):
+  print("Initializing AlpamayoModelD...")
+  return AlpamayoModelD(cl_context)
+
+
+@pytest.fixture(scope="function")
+def vipc_server():
+  server_name = "camerad_test"
+  server = VisionIpcServer(server_name)
+  server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 1, 1024, 512)
+  server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 1, 1024, 512)
+  server.start_listener()
+  yield server
+
+
+def test_modeld(cl_context, modeld, vipc_server):
+  v_ego = 20.0
+  inputs = {
+    'input_ids': np.zeros((1, 16), dtype=np.int64),
+    'history': np.zeros((1, 16, 12), dtype=np.float32),
+    'logic_pulse': modeld.logic_pulse,
+    'traffic_convention': np.array([[1.0, 0.0]], dtype=np.float32),
+    'v_ego': v_ego
+  }
+
+  server_name = "camerad_test"
+  client_road = VisionIpcClient(server_name, VisionStreamType.VISION_STREAM_ROAD, False, cl_context)
+  assert client_road.connect(True), "Road client failed to connect"
+  client_wide = VisionIpcClient(server_name, VisionStreamType.VISION_STREAM_WIDE_ROAD, False, cl_context)
+  assert client_wide.connect(True), "Wide client failed to connect"
+
+  # NV12 size for 1024x512 = 1024*512 * 1.5 = 786432
+  yuv_data = b'\x00' * 786432
+  vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, yuv_data)
+  vipc_server.send(VisionStreamType.VISION_STREAM_WIDE_ROAD, yuv_data)
+  buf_road = client_road.recv()
+  buf_wide = client_wide.recv()
+  assert buf_road is not None
+  assert buf_wide is not None
+
+  bufs = {'road': buf_road, 'wide': buf_wide}
+  transforms = {'road': np.eye(3, dtype=np.float32), 'wide': np.eye(3, dtype=np.float32)}
+  outputs = modeld.run(bufs, transforms, inputs, False)
+
+  assert outputs is not None
+  assert outputs["position"].shape == (1, 64, 3)
+  assert outputs["velocity"].shape == (1, 64, 3)
+  assert outputs["acceleration"].shape == (1, 64, 3)
+  assert outputs["orientation"].shape == (1, 64, 3)
+  assert "pred_pulse" in outputs
+  assert "pred_light" in outputs
+  assert "pred_lead" in outputs
+
+  assert np.all(np.isfinite(outputs["position"])), "Position contains NaN/Inf"
+  assert np.all(np.isfinite(outputs["velocity"])), "Velocity contains NaN/Inf"
+  assert "consistency_error" in outputs
+  assert outputs["consistency_error"] >= 0.0
+
+  model = messaging.new_message('modelV2')
+  CP = car.CarParams.new_message()
+  CP.longitudinalActuatorDelay = 0.2
+  fill_alpamayo_msg(model.modelV2, outputs, 12345, 0.0, 1e9, CP, 0.1, v_ego)
+  # these just ensure that the model should outputs same action for the same black pixels
+  assert model.modelV2.action.desiredAcceleration == pytest.approx(-6.75, abs=1e-2)
+  assert model.modelV2.action.desiredCurvature == pytest.approx(-0.05, abs=1e-2)
+  assert not model.modelV2.action.shouldStop
+  assert model.modelV2.frameId == 12345

sunnypilot/modeld_v2/modeld.py

@@ -66,7 +66,7 @@ class ModelState(ModelStateBase):
     self.PLANPLUS_CONTROL: float = 1.0
 
     buffer_length = 5 if self.model_runner.is_20hz else 2
-    self.frames = {name: DrivingModelFrame(context, buffer_length) for name in self.model_runner.vision_input_names}
+    self.frames = {name: DrivingModelFrame(context, 512, 256, buffer_length) for name in self.model_runner.vision_input_names}
     self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
 
     # img buffers are managed in openCL transform code

sunnypilot/modeld_v2/models/commonmodel.cc

@@ -5,7 +5,11 @@
 
 #include "common/clutil.h"
 
-DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length) : ModelFrame(device_id, context), buffer_length(buffer_length) {
+DrivingModelFrame::DrivingModelFrame(cl_device_id device_id, cl_context context, int width, int height, uint8_t buffer_length) : ModelFrame(device_id, context), MODEL_WIDTH(width), MODEL_HEIGHT(height), buffer_length(buffer_length) {
+  MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
+  buf_size = MODEL_FRAME_SIZE * 2;
+  frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
+
   input_frames = std::make_unique<uint8_t[]>(buf_size);
   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, buffer_length*frame_size_bytes, NULL, &err));

sunnypilot/modeld_v2/models/commonmodel.h

@@ -64,15 +64,15 @@ protected:
 
 class DrivingModelFrame : public ModelFrame {
 public:
-  DrivingModelFrame(cl_device_id device_id, cl_context context, uint8_t buffer_length);
+  DrivingModelFrame(cl_device_id device_id, cl_context context, int width, int height, uint8_t buffer_length);
   ~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;
-  const size_t frame_size_bytes = MODEL_FRAME_SIZE * sizeof(uint8_t);
+  int MODEL_WIDTH;
+  int MODEL_HEIGHT;
+  int MODEL_FRAME_SIZE;
+  int buf_size;
+  size_t frame_size_bytes;
   const uint8_t buffer_length;
 
 private:

sunnypilot/modeld_v2/models/commonmodel.pxd

@@ -20,7 +20,7 @@ cdef extern from "sunnypilot/modeld_v2/models/commonmodel.h":
 
   cppclass DrivingModelFrame:
     int buf_size
-    DrivingModelFrame(cl_device_id, cl_context, unsigned char)
+    DrivingModelFrame(cl_device_id, cl_context, int, int, unsigned char)
 
   cppclass MonitoringModelFrame:
     int buf_size

sunnypilot/modeld_v2/models/commonmodel_pyx.pyx

@@ -59,8 +59,8 @@ cdef class ModelFrame:
 cdef class DrivingModelFrame(ModelFrame):
   cdef cppDrivingModelFrame * _frame
 
-  def __cinit__(self, CLContext context, int buffer_length=2):
-    self._frame = new cppDrivingModelFrame(context.device_id, context.context, buffer_length)
+  def __cinit__(self, CLContext context, int width, int height, int buffer_length=2):
+    self._frame = new cppDrivingModelFrame(context.device_id, context.context, width, height, buffer_length)
     self.frame = <cppModelFrame*>(self._frame)
     self.buf_size = self._frame.buf_size
 

system/manager/manager.py

@@ -222,7 +222,8 @@ def main() -> None:
 
 
 if __name__ == "__main__":
-  unblock_stdout()
+  if sys.platform != 'darwin':
+    unblock_stdout()
 
   try:
     main()

system/manager/process_config.py

@@ -80,17 +80,21 @@ def use_sunnylink_uploader_shim(started, params, CP: car.CarParams) -> bool:
   """Shim for use_sunnylink_uploader to match the process manager signature."""
   return use_sunnylink_uploader(params)
 
+def is_modeld_sunny(started: bool, params: Params, CP: car.CarParams) -> bool:
+  """Check if the active model is modeld_sunny."""
+  return bool(params.get_bool("ModeldSunny"))
+
 def is_snpe_model(started, params, CP: car.CarParams) -> bool:
   """Check if the active model runner is SNPE."""
-  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.snpe)
+  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.snpe and not is_modeld_sunny(started, params, CP))
 
 def is_tinygrad_model(started, params, CP: car.CarParams) -> bool:
   """Check if the active model runner is SNPE."""
-  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad)
+  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.tinygrad and not is_modeld_sunny(started, params, CP))
 
 def is_stock_model(started, params, CP: car.CarParams) -> bool:
   """Check if the active model runner is stock."""
-  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock)
+  return bool(get_active_model_runner(params, not started) == custom.ModelManagerSP.Runner.stock and not is_modeld_sunny(started, params, CP))
 
 def mapd_ready(started: bool, params: Params, CP: car.CarParams) -> bool:
   return bool(os.path.exists(Paths.mapd_root()))
@@ -172,6 +176,7 @@ procs += [
   PythonProcess("models_manager", "sunnypilot.models.manager", only_offroad),
   NativeProcess("modeld_snpe", "sunnypilot/modeld", ["./modeld"], and_(only_onroad, is_snpe_model)),
   NativeProcess("modeld_tinygrad", "sunnypilot/modeld_v2", ["./modeld"], and_(only_onroad, is_tinygrad_model)),
+  PythonProcess("modeld_sunny", "sunnypilot.modeld_sunny.modeld", and_(only_onroad, is_modeld_sunny)),
 
   # Backup
   PythonProcess("backup_manager", "sunnypilot.sunnylink.backups.manager", and_(only_offroad, sunnylink_ready_shim)),