FrogPilot 0.9.7

selfdrive/modeld/constants.py

@@ -15,7 +15,8 @@ class ModelConstants:
   # model inputs constants
   MODEL_FREQ = 20
   FEATURE_LEN = 512
-  HISTORY_BUFFER_LEN = 99
+  FULL_HISTORY_BUFFER_LEN = 99
+  HISTORY_BUFFER_LEN = 24
   DESIRE_LEN = 8
   TRAFFIC_CONVENTION_LEN = 2
   LAT_PLANNER_STATE_LEN = 4
@@ -31,7 +32,6 @@ class ModelConstants:
   DISENGAGE_WIDTH = 5
   POSE_WIDTH = 6
   WIDE_FROM_DEVICE_WIDTH = 3
-  SIM_POSE_WIDTH = 6
   LEAD_WIDTH = 4
   LANE_LINES_WIDTH = 2
   ROAD_EDGES_WIDTH = 2
@@ -59,6 +59,8 @@ class ModelConstants:
   RYG_GREEN = 0.01165
   RYG_YELLOW = 0.06157
 
+  POLY_PATH_DEGREE = 4
+
 # model outputs slices
 class Plan:
   POSITION = slice(0, 3)
@@ -70,13 +72,14 @@ class Plan:
 class Meta:
   ENGAGED = slice(0, 1)
   # next 2, 4, 6, 8, 10 seconds
-  GAS_DISENGAGE = slice(1, 36, 7)
-  BRAKE_DISENGAGE = slice(2, 36, 7)
-  STEER_OVERRIDE = slice(3, 36, 7)
-  HARD_BRAKE_3 = slice(4, 36, 7)
-  HARD_BRAKE_4 = slice(5, 36, 7)
-  HARD_BRAKE_5 = slice(6, 36, 7)
-  GAS_PRESS = slice(7, 36, 7)
+  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
-  LEFT_BLINKER = slice(36, 48, 2)
-  RIGHT_BLINKER = slice(37, 48, 2)
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)

selfdrive/modeld/dmonitoringmodeld.py

@@ -14,7 +14,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.realtime import set_realtime_priority
 from openpilot.selfdrive.modeld.runners import ModelRunner, Runtime
-from openpilot.selfdrive.modeld.models.commonmodel_pyx import sigmoid
+from openpilot.selfdrive.modeld.parse_model_outputs import sigmoid
 
 CALIB_LEN = 3
 REG_SCALE = 0.25
@@ -76,8 +76,8 @@ class ModelState:
     input_data = self.inputs['input_img'].reshape(MODEL_HEIGHT, MODEL_WIDTH)
     input_data[:] = buf_data[v_offset:v_offset+MODEL_HEIGHT, h_offset:h_offset+MODEL_WIDTH]
 
-    t1 = time.perf_counter()
     self.model.setInputBuffer("input_img", self.inputs['input_img'].view(np.float32))
+    t1 = time.perf_counter()
     self.model.execute()
     t2 = time.perf_counter()
     return self.output, t2 - t1
@@ -88,15 +88,15 @@ def fill_driver_state(msg, ds_result: DriverStateResult):
   msg.faceOrientationStd = [math.exp(x) for x in ds_result.face_orientation_std]
   msg.facePosition = [x * REG_SCALE for x in ds_result.face_position[:2]]
   msg.facePositionStd = [math.exp(x) for x in ds_result.face_position_std[:2]]
-  msg.faceProb = sigmoid(ds_result.face_prob)
-  msg.leftEyeProb = sigmoid(ds_result.left_eye_prob)
-  msg.rightEyeProb = sigmoid(ds_result.right_eye_prob)
-  msg.leftBlinkProb = sigmoid(ds_result.left_blink_prob)
-  msg.rightBlinkProb = sigmoid(ds_result.right_blink_prob)
-  msg.sunglassesProb = sigmoid(ds_result.sunglasses_prob)
-  msg.occludedProb = sigmoid(ds_result.occluded_prob)
-  msg.readyProb = [sigmoid(x) for x in ds_result.ready_prob]
-  msg.notReadyProb = [sigmoid(x) for x in ds_result.not_ready_prob]
+  msg.faceProb = float(sigmoid(ds_result.face_prob))
+  msg.leftEyeProb = float(sigmoid(ds_result.left_eye_prob))
+  msg.rightEyeProb = float(sigmoid(ds_result.right_eye_prob))
+  msg.leftBlinkProb = float(sigmoid(ds_result.left_blink_prob))
+  msg.rightBlinkProb = float(sigmoid(ds_result.right_blink_prob))
+  msg.sunglassesProb = float(sigmoid(ds_result.sunglasses_prob))
+  msg.occludedProb = float(sigmoid(ds_result.occluded_prob))
+  msg.readyProb = [float(sigmoid(x)) for x in ds_result.ready_prob]
+  msg.notReadyProb = [float(sigmoid(x)) for x in ds_result.not_ready_prob]
 
 def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts: int, execution_time: float, dsp_execution_time: float):
   model_result = ctypes.cast(model_output.ctypes.data, ctypes.POINTER(DMonitoringModelResult)).contents
@@ -105,8 +105,8 @@ def get_driverstate_packet(model_output: np.ndarray, frame_id: int, location_ts:
   ds.frameId = frame_id
   ds.modelExecutionTime = execution_time
   ds.dspExecutionTime = dsp_execution_time
-  ds.poorVisionProb = sigmoid(model_result.poor_vision_prob)
-  ds.wheelOnRightProb = sigmoid(model_result.wheel_on_right_prob)
+  ds.poorVisionProb = float(sigmoid(model_result.poor_vision_prob))
+  ds.wheelOnRightProb = float(sigmoid(model_result.wheel_on_right_prob))
   ds.rawPredictions = model_output.tobytes() if SEND_RAW_PRED else b''
   fill_driver_state(ds.leftDriverData, model_result.driver_state_lhd)
   fill_driver_state(ds.rightDriverData, model_result.driver_state_rhd)

selfdrive/modeld/fill_model_msg.py

@@ -3,11 +3,22 @@ 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 MIN_SPEED
 
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
 ConfidenceClass = log.ModelDataV2.ConfidenceClass
 
+def curv_from_psis(psi_target, psi_rate, vego, delay):
+  vego = np.clip(vego, MIN_SPEED, np.inf)
+  curv_from_psi = psi_target / (vego * delay)  # epsilon to prevent divide-by-zero
+  return 2*curv_from_psi - psi_rate / vego
+
+def get_curvature_from_plan(plan, vego, delay):
+  psi_target = np.interp(delay, ModelConstants.T_IDXS, plan[:, Plan.T_FROM_CURRENT_EULER][:, 2])
+  psi_rate = plan[:, Plan.ORIENTATION_RATE][0, 2]
+  return curv_from_psis(psi_target, psi_rate, vego, delay)
+
 class PublishState:
   def __init__(self):
     self.disengage_buffer = np.zeros(ModelConstants.CONFIDENCE_BUFFER_LEN*ModelConstants.DISENGAGE_WIDTH, dtype=np.float32)
@@ -41,17 +52,46 @@ def fill_xyvat(builder, t, x, y, v, a, x_std=None, y_std=None, v_std=None, a_std
   if a_std is not None:
     builder.aStd = a_std.tolist()
 
-def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str, np.ndarray], 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
-  msg.valid = valid
+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()
 
-  modelV2 = msg.modelV2
+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], v_ego: float, delay: float,
+                   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, planner_curves: 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
+
+  desired_curv = float(get_curvature_from_plan(net_output_data['plan'][0], v_ego, delay))
+
+  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
+
+  action = driving_model_data.action
+  action.desiredCurvature = desired_curv if planner_curves else float(net_output_data['desired_curvature'][0,0])
+
+  modelV2 = extended_msg.modelV2
   modelV2.frameId = vipc_frame_id
   modelV2.frameIdExtra = vipc_frame_id_extra
   modelV2.frameAge = frame_age
-  modelV2.frameDropPerc = frame_drop * 100
+  modelV2.frameDropPerc = frame_drop_perc
   modelV2.timestampEof = timestamp_eof
   modelV2.modelExecutionTime = model_execution_time
 
@@ -67,9 +107,20 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
   orientation_rate = modelV2.orientationRate
   fill_xyzt(orientation_rate, ModelConstants.T_IDXS, *net_output_data['plan'][0,:,Plan.ORIENTATION_RATE].T)
 
+  # temporal pose
+  temporal_pose = modelV2.temporalPose
+  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()
+
+  # poly path
+  poly_path = driving_model_data.path
+  fill_xyz_poly(poly_path, ModelConstants.POLY_PATH_DEGREE, *net_output_data['plan'][0,:,Plan.POSITION].T)
+
   # lateral planning
   action = modelV2.action
-  action.desiredCurvature = float(net_output_data['desired_curvature'][0,0])
+  action.desiredCurvature = desired_curv if planner_curves else float(net_output_data['desired_curvature'][0,0])
 
   # times at X_IDXS according to model plan
   PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N
@@ -91,12 +142,31 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
     PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx]
 
   # lane lines
-  modelV2.init('laneLines', 4)
-  for i in range(4):
+  modelV2.init('laneLines', 6)
+  lane_probs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+  for i in range(6):
     lane_line = modelV2.laneLines[i]
-    fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1])
+    if i < 4:
+      fill_xyzt(lane_line, PLAN_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:
+      if lane_probs[0] > 0:
+        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, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), leftLane_x, leftLane_y)
+      else:
+        fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
+    elif i == 5:
+      if lane_probs[3] > 0:
+        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, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), rightLane_x, rightLane_y)
+      else:
+        fill_xyzt(lane_line, PLAN_T_IDXS, np.empty((0,)), np.empty((0,)), np.empty((0,)))
   modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist()
-  modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist()
+  modelV2.laneLineProbs = lane_probs
+
+  lane_line_meta = driving_model_data.laneLineMeta
+  fill_lane_line_meta(lane_line_meta, modelV2.laneLines, modelV2.laneLineProbs)
 
   # road edges
   modelV2.init('roadEdges', 2)
@@ -127,6 +197,8 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
   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]
@@ -136,13 +208,6 @@ def fill_model_msg(msg: capnp._DynamicStructBuilder, net_output_data: dict[str,
     (publish_state.prev_brake_3ms2_probs > ModelConstants.FCW_THRESHOLDS_3MS2).all()
   meta.hardBrakePredicted = hard_brake_predicted.item()
 
-  # temporal pose
-  temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
-
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:
     # any disengage prob

selfdrive/modeld/modeld.py

@@ -24,6 +24,8 @@ from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_
 from openpilot.selfdrive.modeld.constants import ModelConstants
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext
 
+from openpilot.selfdrive.frogpilot.frogpilot_variables import METADATAS_PATH, MODELS_PATH, get_frogpilot_toggles
+
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -31,8 +33,6 @@ MODEL_PATHS = {
   ModelRunner.THNEED: Path(__file__).parent / 'models/supercombo.thneed',
   ModelRunner.ONNX: Path(__file__).parent / 'models/supercombo.onnx'}
 
-METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
-
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -50,21 +50,32 @@ class ModelState:
   prev_desire: np.ndarray  # for tracking the rising edge of the pulse
   model: ModelRunner
 
-  def __init__(self, context: CLContext):
+  def __init__(self, context: CLContext, model: str, model_version: str):
+    # FrogPilot variables
+    MODEL_PATHS[ModelRunner.THNEED] = MODELS_PATH / f'{model}.thneed'
+
+    with open(METADATAS_PATH / f'supercombo_metadata_{model_version}.pkl', 'rb') as f:
+      model_metadata = pickle.load(f)
+
+    input_shapes = model_metadata.get('input_shapes')
+    self.use_desired_curvature = 'lateral_control_params' in input_shapes and 'prev_desired_curv' in input_shapes
+
     self.frame = ModelFrame(context)
     self.wide_frame = ModelFrame(context)
     self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
+    self.full_features_20Hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN, ModelConstants.FEATURE_LEN), dtype=np.float32)
+    self.desire_20Hz =  np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.DESIRE_LEN), dtype=np.float32)
+    self.prev_desired_curv_20hz = np.zeros((ModelConstants.FULL_HISTORY_BUFFER_LEN + 1, ModelConstants.PREV_DESIRED_CURV_LEN), dtype=np.float32)
+
+    # img buffers are managed in openCL transform code
     self.inputs = {
       'desire': np.zeros(ModelConstants.DESIRE_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
       'traffic_convention': np.zeros(ModelConstants.TRAFFIC_CONVENTION_LEN, dtype=np.float32),
-      'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32),
-      'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32),
+      **({'lateral_control_params': np.zeros(ModelConstants.LATERAL_CONTROL_PARAMS_LEN, dtype=np.float32)} if self.use_desired_curvature else {}),
+      **({'prev_desired_curv': np.zeros(ModelConstants.PREV_DESIRED_CURV_LEN * (ModelConstants.HISTORY_BUFFER_LEN+1), dtype=np.float32)} if self.use_desired_curvature else {}),
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
     }
 
-    with open(METADATA_PATH, 'rb') as f:
-      model_metadata = pickle.load(f)
-
     self.output_slices = model_metadata['output_slices']
     net_output_size = model_metadata['output_shapes']['outputs'][1]
     self.output = np.zeros(net_output_size, dtype=np.float32)
@@ -86,17 +97,19 @@ class ModelState:
                 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'][0] = 0
-    self.inputs['desire'][:-ModelConstants.DESIRE_LEN] = self.inputs['desire'][ModelConstants.DESIRE_LEN:]
-    self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
+    new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
     self.prev_desire[:] = inputs['desire']
 
+    self.desire_20Hz[:-1] = self.desire_20Hz[1:]
+    self.desire_20Hz[-1] = new_desire
+    self.inputs['desire'][:] = self.desire_20Hz.reshape((25,4,-1)).max(axis=1).flatten()
+
     self.inputs['traffic_convention'][:] = inputs['traffic_convention']
-    self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
+    if self.use_desired_curvature:
+      self.inputs['lateral_control_params'][:] = inputs['lateral_control_params']
 
-    # if getCLBuffer is not None, frame will be None
     self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
-    if wbuf is not None:
-      self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
+    self.model.setInputBuffer("big_input_imgs", self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs")))
 
     if prepare_only:
       return None
@@ -104,10 +117,18 @@ class ModelState:
     self.model.execute()
     outputs = self.parser.parse_outputs(self.slice_outputs(self.output))
 
-    self.inputs['features_buffer'][:-ModelConstants.FEATURE_LEN] = self.inputs['features_buffer'][ModelConstants.FEATURE_LEN:]
-    self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
-    self.inputs['prev_desired_curv'][:-ModelConstants.PREV_DESIRED_CURV_LEN] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:]
-    self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :]
+    self.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
+    self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
+
+    if self.use_desired_curvature:
+      self.prev_desired_curv_20hz[:-1] = self.prev_desired_curv_20hz[1:]
+      self.prev_desired_curv_20hz[-1] = outputs['desired_curvature'][0, :]
+
+    idxs = np.arange(-4,-100,-4)[::-1]
+    self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
+    if self.use_desired_curvature:
+      # TODO model only uses last value now, once that changes we need to input strided action history buffer
+      self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = 0. * self.prev_desired_curv_20hz[-4, :]
     return outputs
 
 
@@ -122,7 +143,16 @@ def main(demo=False):
   cloudlog.warning("setting up CL context")
   cl_context = CLContext()
   cloudlog.warning("CL context ready; loading model")
-  model = ModelState(cl_context)
+
+  # FrogPilot variables
+  frogpilot_toggles = get_frogpilot_toggles()
+
+  model_name = frogpilot_toggles.model
+  model_version = frogpilot_toggles.model_version
+
+  planner_curves = frogpilot_toggles.planner_curvature_model
+
+  model = ModelState(cl_context, model_name, model_version)
   cloudlog.warning("models loaded, modeld starting")
 
   # visionipc clients
@@ -149,8 +179,8 @@ 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", "cameraOdometry"])
-  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl"])
+  pm = PubMaster(["modelV2", "drivingModelData", "cameraOdometry"])
+  sm = SubMaster(["deviceState", "carState", "roadCameraState", "liveCalibration", "driverMonitoringState", "carControl", "frogpilotPlan"])
 
   publish_state = PublishState()
   params = Params()
@@ -218,7 +248,9 @@ def main(demo=False):
     desire = DH.desire
     is_rhd = sm["driverMonitoringState"].isRHD
     frame_id = sm["roadCameraState"].frameId
-    lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
+    v_ego = max(sm["carState"].vEgo, 0.)
+    if model.use_desired_curvature:
+      lateral_control_params = np.array([v_ego, steer_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))]
@@ -249,7 +281,7 @@ def main(demo=False):
     inputs:dict[str, np.ndarray] = {
       'desire': vec_desire,
       'traffic_convention': traffic_convention,
-      'lateral_control_params': lateral_control_params,
+      **({'lateral_control_params': lateral_control_params} if model.use_desired_curvature else {}),
       }
 
     mt1 = time.perf_counter()
@@ -259,24 +291,35 @@ def main(demo=False):
 
     if model_output is not None:
       modelv2_send = messaging.new_message('modelV2')
+      drivingdata_send = messaging.new_message('drivingModelData')
       posenet_send = messaging.new_message('cameraOdometry')
-      fill_model_msg(modelv2_send, model_output, 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)
+      fill_model_msg(drivingdata_send, modelv2_send, model_output, v_ego, steer_delay,
+                     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,
+                     planner_curves)
 
       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)
+      DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob, sm['frogpilotPlan'], frogpilot_toggles)
       modelv2_send.modelV2.meta.laneChangeState = DH.lane_change_state
       modelv2_send.modelV2.meta.laneChangeDirection = DH.lane_change_direction
+      modelv2_send.modelV2.meta.turnDirection = DH.turn_direction
+      drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
+      drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction
+      drivingdata_send.drivingModelData.meta.turnDirection = DH.turn_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('drivingModelData', drivingdata_send)
       pm.send('cameraOdometry', posenet_send)
 
     last_vipc_frame_id = meta_main.frame_id
 
+    # Update FrogPilot parameters
+    if sm['frogpilotPlan'].togglesUpdated:
+      frogpilot_toggles = get_frogpilot_toggles()
 
 if __name__ == "__main__":
   try:

selfdrive/modeld/models/commonmodel.cc

@@ -7,32 +7,39 @@
 #include "common/clutil.h"
 
 ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
-  input_frames = std::make_unique<float[]>(buf_size);
+  input_frames = std::make_unique<uint8_t[]>(buf_size);
 
   q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
   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));
-  net_input_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, MODEL_FRAME_SIZE * sizeof(float), NULL, &err));
+  img_buffer_20hz_cl = CL_CHECK_ERR(clCreateBuffer(context, CL_MEM_READ_WRITE, 5*frame_size_bytes, NULL, &err));
+  region.origin = 4 * 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));
 
   transform_init(&transform, context, device_id);
   loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
 }
 
-float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
+uint8_t* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int frame_stride, int frame_uv_offset, const mat3 &projection, cl_mem *output) {
   transform_queue(&this->transform, q,
-                  yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
-                  y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
+                yuv_cl, frame_width, frame_height, frame_stride, frame_uv_offset,
+                y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, projection);
 
+  for (int i = 0; i < 4; 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);
   if (output == NULL) {
-    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
-
-    std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
-    CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
+    CL_CHECK(clEnqueueReadBuffer(q, img_buffer_20hz_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[0], 0, nullptr, nullptr));
+    CL_CHECK(clEnqueueReadBuffer(q, last_img_cl, CL_TRUE, 0, frame_size_bytes, &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
     clFinish(q);
     return &input_frames[0];
   } else {
-    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
+    copy_queue(&loadyuv, q, img_buffer_20hz_cl, *output, 0, 0, frame_size_bytes);
+    copy_queue(&loadyuv, q, last_img_cl, *output, 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 NULL;
@@ -42,13 +49,10 @@ float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, int
 ModelFrame::~ModelFrame() {
   transform_destroy(&transform);
   loadyuv_destroy(&loadyuv);
-  CL_CHECK(clReleaseMemObject(net_input_cl));
+  CL_CHECK(clReleaseMemObject(img_buffer_20hz_cl));
+  CL_CHECK(clReleaseMemObject(last_img_cl));
   CL_CHECK(clReleaseMemObject(v_cl));
   CL_CHECK(clReleaseMemObject(u_cl));
   CL_CHECK(clReleaseMemObject(y_cl));
   CL_CHECK(clReleaseCommandQueue(q));
-}
-
-float sigmoid(float input) {
-  return 1 / (1 + expf(-input));
-}
+}

selfdrive/modeld/models/commonmodel.h

@@ -16,23 +16,23 @@
 #include "selfdrive/modeld/transforms/loadyuv.h"
 #include "selfdrive/modeld/transforms/transform.h"
 
-float sigmoid(float input);
-
 class ModelFrame {
 public:
   ModelFrame(cl_device_id device_id, cl_context context);
   ~ModelFrame();
-  float* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
+  uint8_t* prepare(cl_mem yuv_cl, int width, int height, int frame_stride, int frame_uv_offset, const mat3& transform, cl_mem *output);
 
   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);
 
 private:
   Transform transform;
   LoadYUVState loadyuv;
   cl_command_queue q;
-  cl_mem y_cl, u_cl, v_cl, net_input_cl;
-  std::unique_ptr<float[]> input_frames;
-};
+  cl_mem y_cl, u_cl, v_cl, img_buffer_20hz_cl, last_img_cl;
+  cl_buffer_region region;
+  std::unique_ptr<uint8_t[]> input_frames;
+};

selfdrive/modeld/models/commonmodel.pxd

@@ -12,9 +12,7 @@ cdef extern from "common/clutil.h":
   cl_context cl_create_context(cl_device_id)
 
 cdef extern from "selfdrive/modeld/models/commonmodel.h":
-  float sigmoid(float)
-
   cppclass ModelFrame:
     int buf_size
     ModelFrame(cl_device_id, cl_context)
-    float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)
+    unsigned char * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -1,5 +1,5 @@
 # distutils: language = c++
-# cython: c_string_encoding=ascii
+# cython: c_string_encoding=ascii, language_level=3
 
 import numpy as np
 cimport numpy as cnp
@@ -8,10 +8,8 @@ from libc.string cimport memcpy
 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
-from .commonmodel cimport mat3, sigmoid as cppSigmoid, ModelFrame as cppModelFrame
+from .commonmodel cimport mat3, ModelFrame as cppModelFrame
 
-def sigmoid(x):
-  return cppSigmoid(x)
 
 cdef class CLContext(BaseCLContext):
   def __cinit__(self):
@@ -37,11 +35,11 @@ cdef class ModelFrame:
   def prepare(self, VisionBuf buf, float[:] projection, CLMem output):
     cdef mat3 cprojection
     memcpy(cprojection.v, &projection[0], 9*sizeof(float))
-    cdef float * data
+    cdef unsigned char * data
     if output is None:
       data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, NULL)
     else:
       data = self.frame.prepare(buf.buf.buf_cl, buf.width, buf.height, buf.stride, buf.uv_offset, cprojection, output.mem)
     if not data:
       return None
-    return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)
+    return np.asarray(<cnp.uint8_t[:self.frame.buf_size]> data)

selfdrive/modeld/parse_model_outputs.py

@@ -1,15 +1,19 @@
 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. + np.exp(-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:
-    np.exp(x, out=x)
+    safe_exp(x, out=x)
   else:
-    x = np.exp(x)
+    x = safe_exp(x)
   x /= np.sum(x, axis=axis, keepdims=True)
   return x
 
@@ -42,10 +46,9 @@ class Parser:
     raw = outs[name]
     raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
 
-    pred_mu = raw[:,:,:(raw.shape[2] - out_N)//2]
     n_values = (raw.shape[2] - out_N)//2
     pred_mu = raw[:,:,:n_values]
-    pred_std = np.exp(raw[:,:,n_values: 2*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)

selfdrive/modeld/runners/onnxmodel.py

@@ -1,33 +1,12 @@
-import onnx
-import itertools
 import os
+import onnx
 import sys
 import numpy as np
 from typing import Any
 
 from openpilot.selfdrive.modeld.runners.runmodel_pyx import RunModel
+from openpilot.selfdrive.modeld.runners.ort_helpers import convert_fp16_to_fp32, ORT_TYPES_TO_NP_TYPES
 
-ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
-
-def attributeproto_fp16_to_fp32(attr):
-  float32_list = np.frombuffer(attr.raw_data, dtype=np.float16)
-  attr.data_type = 1
-  attr.raw_data = float32_list.astype(np.float32).tobytes()
-
-def convert_fp16_to_fp32(path):
-  model = onnx.load(path)
-  for i in model.graph.initializer:
-    if i.data_type == 10:
-      attributeproto_fp16_to_fp32(i)
-  for i in itertools.chain(model.graph.input, model.graph.output):
-    if i.type.tensor_type.elem_type == 10:
-      i.type.tensor_type.elem_type = 1
-  for i in model.graph.node:
-    for a in i.attribute:
-      if hasattr(a, 't'):
-        if a.t.data_type == 10:
-          attributeproto_fp16_to_fp32(a.t)
-  return model.SerializeToString()
 
 def create_ort_session(path, fp16_to_fp32):
   os.environ["OMP_NUM_THREADS"] = "4"
@@ -43,14 +22,14 @@ def create_ort_session(path, fp16_to_fp32):
     provider = 'OpenVINOExecutionProvider'
   elif 'CUDAExecutionProvider' in ort.get_available_providers() and 'ONNXCPU' not in os.environ:
     options.intra_op_num_threads = 2
-    provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'DEFAULT'})
+    provider = ('CUDAExecutionProvider', {'cudnn_conv_algo_search': 'EXHAUSTIVE'})
   else:
     options.intra_op_num_threads = 2
     options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
     options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
     provider = 'CPUExecutionProvider'
 
-  model_data = convert_fp16_to_fp32(path) if fp16_to_fp32 else path
+  model_data = convert_fp16_to_fp32(onnx.load(path)) if fp16_to_fp32 else path
   print("Onnx selected provider: ", [provider], file=sys.stderr)
   ort_session = ort.InferenceSession(model_data, options, providers=[provider])
   print("Onnx using ", ort_session.get_providers(), file=sys.stderr)
@@ -61,7 +40,6 @@ class ONNXModel(RunModel):
   def __init__(self, path, output, runtime, use_tf8, cl_context):
     self.inputs = {}
     self.output = output
-    self.use_tf8 = use_tf8
 
     self.session = create_ort_session(path, fp16_to_fp32=True)
     self.input_names = [x.name for x in self.session.get_inputs()]
@@ -85,7 +63,7 @@ class ONNXModel(RunModel):
     return None
 
   def execute(self):
-    inputs = {k: (v.view(np.uint8) / 255. if self.use_tf8 and k == 'input_img' else v) for k,v in self.inputs.items()}
+    inputs = {k: v.view(self.input_dtypes[k]) for k,v in self.inputs.items()}
     inputs = {k: v.reshape(self.input_shapes[k]).astype(self.input_dtypes[k]) for k,v in inputs.items()}
     outputs = self.session.run(None, inputs)
     assert len(outputs) == 1, "Only single model outputs are supported"

selfdrive/modeld/runners/ort_helpers.py

@@ -0,0 +1,36 @@
+import onnx
+import onnxruntime as ort
+import numpy as np
+import itertools
+
+ORT_TYPES_TO_NP_TYPES = {'tensor(float16)': np.float16, 'tensor(float)': np.float32, 'tensor(uint8)': np.uint8}
+
+def attributeproto_fp16_to_fp32(attr):
+  float32_list = np.frombuffer(attr.raw_data, dtype=np.float16)
+  attr.data_type = 1
+  attr.raw_data = float32_list.astype(np.float32).tobytes()
+
+def convert_fp16_to_fp32(model):
+  for i in model.graph.initializer:
+    if i.data_type == 10:
+      attributeproto_fp16_to_fp32(i)
+  for i in itertools.chain(model.graph.input, model.graph.output):
+    if i.type.tensor_type.elem_type == 10:
+      i.type.tensor_type.elem_type = 1
+  for i in model.graph.node:
+    if i.op_type == 'Cast' and i.attribute[0].i == 10:
+      i.attribute[0].i = 1
+    for a in i.attribute:
+      if hasattr(a, 't'):
+        if a.t.data_type == 10:
+          attributeproto_fp16_to_fp32(a.t)
+  return model.SerializeToString()
+
+
+def make_onnx_cpu_runner(model_path):
+  options = ort.SessionOptions()
+  options.intra_op_num_threads = 4
+  options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL
+  options.graph_optimization_level = ort.GraphOptimizationLevel.ORT_ENABLE_ALL
+  model_data = convert_fp16_to_fp32(onnx.load(model_path))
+  return ort.InferenceSession(model_data,  options, providers=['CPUExecutionProvider'])

selfdrive/modeld/runners/runmodel_pyx.pyx

@@ -1,5 +1,5 @@
 # distutils: language = c++
-# cython: c_string_encoding=ascii
+# cython: c_string_encoding=ascii, language_level=3
 
 from libcpp.string cimport string
 

selfdrive/modeld/runners/snpemodel_pyx.pyx

@@ -1,5 +1,5 @@
 # distutils: language = c++
-# cython: c_string_encoding=ascii
+# cython: c_string_encoding=ascii, language_level=3
 
 import os
 from libcpp cimport bool

selfdrive/modeld/runners/thneedmodel_pyx.pyx

@@ -1,5 +1,5 @@
 # distutils: language = c++
-# cython: c_string_encoding=ascii
+# cython: c_string_encoding=ascii, language_level=3
 
 from libcpp cimport bool
 from libcpp.string cimport string

selfdrive/modeld/tests/test_modeld.py

@@ -3,9 +3,10 @@ import random
 
 import cereal.messaging as messaging
 from msgq.visionipc import VisionIpcServer, VisionStreamType
+from opendbc.car.car_helpers import get_demo_car_params
+from openpilot.common.params import Params
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.realtime import DT_MDL
-from openpilot.selfdrive.car.car_helpers import write_car_param
 from openpilot.system.manager.process_config import managed_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
 
@@ -18,11 +19,11 @@ class TestModeld:
 
   def setup_method(self):
     self.vipc_server = VisionIpcServer("camerad")
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, CAM.width, CAM.height)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, CAM.width, CAM.height)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, CAM.width, CAM.height)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, CAM.width, CAM.height)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, CAM.width, CAM.height)
     self.vipc_server.start_listener()
-    write_car_param()
+    Params().put("CarParams", get_demo_car_params().to_bytes())
 
     self.sm = messaging.SubMaster(['modelV2', 'cameraOdometry'])
     self.pm = messaging.PubMaster(['roadCameraState', 'wideRoadCameraState', 'liveCalibration'])

selfdrive/modeld/tests/tf_test/build.sh

@@ -1,2 +1,2 @@
-#!/bin/bash
+#!/usr/bin/env bash
 clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow

selfdrive/modeld/transforms/loadyuv.cc

@@ -33,17 +33,8 @@ 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, bool do_shift) {
+                   cl_mem out_cl) {
   cl_int global_out_off = 0;
-  if (do_shift) {
-    // shift the image in slot 1 to slot 0, then place the new image in slot 1
-    global_out_off += (s->width*s->height) + (s->width/2)*(s->height/2)*2;
-    CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &out_cl));
-    CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_int), &global_out_off));
-    const size_t copy_work_size = global_out_off/8;
-    CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
-                                &copy_work_size, NULL, 0, 0, NULL));
-  }
 
   CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
   CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
@@ -72,3 +63,14 @@ void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
   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));
+}

selfdrive/modeld/transforms/loadyuv.cl

@@ -1,7 +1,7 @@
 #define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
 
 __kernel void loadys(__global uchar8 const * const Y,
-                     __global float * out,
+                     __global uchar * out,
                      int out_offset)
 {
     const int gid = get_global_id(0);
@@ -10,13 +10,12 @@ __kernel void loadys(__global uchar8 const * const Y,
     const int ox = ois % TRANSFORMED_WIDTH;
 
     const uchar8 ys = Y[gid];
-    const float8 ysf = convert_float8(ys);
 
     // 02
     // 13
 
-    __global float* outy0;
-    __global float* outy1;
+    __global uchar* outy0;
+    __global uchar* outy1;
     if ((oy & 1) == 0) {
       outy0 = out + out_offset; //y0
       outy1 = out + out_offset + UV_SIZE*2; //y2
@@ -25,23 +24,24 @@ __kernel void loadys(__global uchar8 const * const Y,
       outy1 = out + out_offset + UV_SIZE*3; //y3
     }
 
-    vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
-    vstore4(ysf.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
+    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 float8 * out,
+                     __global uchar8 * out,
                      int out_offset)
 {
   const int gid = get_global_id(0);
   const uchar8 inv = in[gid];
-  const float8 outv  = convert_float8(inv);
-  out[gid + out_offset / 8] = outv;
+  out[gid + out_offset / 8] = inv;
 }
 
-__kernel void copy(__global float8 * inout,
-                   int in_offset)
+__kernel void copy(__global uchar8 * in,
+                   __global uchar8 * out,
+                   int in_offset,
+                   int out_offset)
 {
   const int gid = get_global_id(0);
-  inout[gid] = inout[gid + in_offset / 8];
+  out[gid + out_offset / 8] = in[gid + in_offset / 8];
 }

selfdrive/modeld/transforms/loadyuv.h

@@ -13,4 +13,8 @@ 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, bool do_shift = false);
+                   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);