Merge branch 'dragonpilot-community:beta3-sgo' into beta3-sgo

CHANGELOGS.md

@@ -1,4 +1,4 @@
-dragonpilot beta3 0.9.8
+dragonpilot beta3-sgo 0.9.8
 =======================
 * Up to comma.ai openpilot master branch commit cbee4421da690fa7b260f62c4a2c72ccacc6c902 (2024-07-14)
 * DP HIGHLIGHT:

body/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[8] = "26f4dbe6";
+const uint8_t gitversion[8] = "e66cc337";

dp_ext/selfdrive/tetood/lib/overpass_api_helper.py

@@ -20,18 +20,10 @@ class OverpassAPIHelper:
         (
           way[highway][highway!~"^({excl_way_types})$"];
         ) -> .allways;
-        (
+        (.allways;>;);
-          way[highway][highway!~"^({excl_way_types}|motorway_link)$"][!name];
-        ) -> .no_name_ways;
-        (
-          way[highway][highway!~"^({excl_way_types})$"][service][service~"^(driveway)$"];
-        ) -> .service_ways;
-        (.allways; - .no_name_ways;) -> .way_result_1;
-        (.way_result_1; - .service_ways;) -> .way_result_final;
-        (.way_result_final;>;);
         out body;
         """
-        # print(overpass_query)
+        print(overpass_query)
         try:
           response = requests.get(self.url, params={'data': overpass_query}, headers=self.headers)
         except:

panda/board/obj/gitversion.h

@@ -1 +1 @@
-const uint8_t gitversion[] = "DEV-26f4dbe6-DEBUG";
+const uint8_t gitversion[] = "DEV-e66cc337-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-26f4dbe6-DEBUG
+DEV-e66cc337-DEBUG

selfdrive/controls/lib/longitudinal_planner.py

@@ -12,7 +12,7 @@ from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX
 from openpilot.selfdrive.controls.lib.longcontrol import LongCtrlState
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
-from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N, get_speed_error
+from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N#, get_speed_error
 from openpilot.common.swaglog import cloudlog
 
 # dp
@@ -52,21 +52,22 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 
 
 def get_accel_from_plan(CP, speeds, accels):
-  if len(speeds) == CONTROL_N:
+    if len(speeds) == CONTROL_N:
-    v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
+      v_target_now = interp(DT_MDL, CONTROL_N_T_IDX, speeds)
-    a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
+      a_target_now = interp(DT_MDL, CONTROL_N_T_IDX, accels)
 
-    v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
+      v_target = interp(CP.longitudinalActuatorDelay + DT_MDL, CONTROL_N_T_IDX, speeds)
-    a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
+      a_target = 2 * (v_target - v_target_now) / CP.longitudinalActuatorDelay - a_target_now
 
-    v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
+      v_target_1sec = interp(CP.longitudinalActuatorDelay + DT_MDL + 1.0, CONTROL_N_T_IDX, speeds)
-  else:
+    else:
-    v_target = 0.0
+      v_target = 0.0
-    v_target_1sec = 0.0
+      v_target_now = 0.0
-    a_target = 0.0
+      v_target_1sec = 0.0
-  should_stop = (v_target < CP.vEgoStopping and
+      a_target = 0.0
-                 v_target_1sec < CP.vEgoStopping)
+    should_stop = (v_target < CP.vEgoStopping and
-  return a_target, should_stop
+                    v_target_1sec < CP.vEgoStopping)
+    return a_target, should_stop
 
 
 class LongitudinalPlanner:
@@ -96,8 +97,8 @@ class LongitudinalPlanner:
   @staticmethod
   def parse_model(model_msg, model_error):
     if (len(model_msg.position.x) == ModelConstants.IDX_N and
-      len(model_msg.velocity.x) == ModelConstants.IDX_N and
+       len(model_msg.velocity.x) == ModelConstants.IDX_N and
-      len(model_msg.acceleration.x) == ModelConstants.IDX_N):
+       len(model_msg.acceleration.x) == ModelConstants.IDX_N):
       x = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
       v = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.velocity.x) - model_error
       a = np.interp(T_IDXS_MPC, ModelConstants.T_IDXS, model_msg.acceleration.x)
@@ -111,22 +112,9 @@ class LongitudinalPlanner:
 
   def update(self, sm):
     if self._frame % 50 == 0:
-      self._dynamic_endtoend_controller.set_enabled(self.params.get_bool("dp_long_de2e"))
       self._pac.set_enabled(self.params.get_bool("dp_long_pac"))
-
     self._frame += 1
-
+    self.mpc.mode = 'acc'
-    if self._dynamic_endtoend_controller.is_enabled():
-      self._dynamic_endtoend_controller.set_mpc_fcw_crash_cnt(self.mpc.crash_cnt)
-      self._dynamic_endtoend_controller.update(self.CP.radarUnavailable,
-                                                                     sm['carState'],
-                                                                     sm['radarState'].leadOne,
-                                                                     sm['modelV2'],
-                                                                     sm['controlsState']
-                                                                     ) #, sm['navInstruction'].maneuverDistance)
-      self.mpc.mode = self._dynamic_endtoend_controller.get_mpc_mode()
-    else:
-      self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc'
 
     v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['controlsState'].vCruise, V_CRUISE_MAX)
@@ -141,7 +129,7 @@ class LongitudinalPlanner:
     # No change cost when user is controlling the speed, or when standstill
     prev_accel_constraint = not (reset_state or sm['carState'].standstill)
 
-    if self.mpc.mode == 'acc':
+    if not sm['controlsState'].experimentalMode:
       self._pac.update(sm['carState'], sm['radarState'].leadOne.status and v_ego > 0. and sm['radarState'].leadOne.dRel / v_ego < 2.5)
       # accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
       accel_limits = [A_CRUISE_MIN, self._pac.get_max_accel(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)]
@@ -158,7 +146,7 @@ class LongitudinalPlanner:
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
     # Compute model v_ego error
-    self.v_model_error = get_speed_error(sm['modelV2'], v_ego)
+    self.v_model_error = 0. # get_speed_error(sm['modelV2'], v_ego)
 
     if force_slow_decel:
       v_cruise = 0.0
@@ -169,20 +157,17 @@ class LongitudinalPlanner:
     # dp
     self._adp_controller.update(v_ego)
     personality = self._adp_controller.get_personality(sm['controlsState'].personality)
-
     self.mpc.set_weights(prev_accel_constraint, personality=personality)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
-    # dp
+    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['controlsState'].personality)
-    self._curve_speed_limiter.update(v_ego, sm['modelV2'].orientationRate.z, v)
-    v = self._curve_speed_limiter.get_v(v)
 
-    self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=personality)
+    self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution)
-
+    self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution)
-    self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)
+    self.v_desired_trajectory = self.v_desired_trajectory_full[:CONTROL_N]
-    self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
+    self.a_desired_trajectory = self.a_desired_trajectory_full[:CONTROL_N]
-    self.j_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC[:-1], self.mpc.j_solution)
+    self.j_desired_trajectory = np.interp(ModelConstants.T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 
     # TODO counter is only needed because radar is glitchy, remove once radar is gone
     self.fcw = self.mpc.crash_cnt > 2 and not sm['carState'].standstill
@@ -191,7 +176,7 @@ class LongitudinalPlanner:
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
-    self.a_desired = float(interp(self.dt, CONTROL_N_T_IDX, self.a_desired_trajectory))
+    self.a_desired = float(interp(self.dt, ModelConstants.T_IDXS[:CONTROL_N], self.a_desired_trajectory))
     self.v_desired_filter.x = self.v_desired_filter.x + self.dt * (self.a_desired + a_prev) / 2.0
 
   def publish(self, sm, pm):
@@ -204,6 +189,9 @@ class LongitudinalPlanner:
     longitudinalPlan.processingDelay = (plan_send.logMonoTime / 1e9) - sm.logMonoTime['modelV2']
     longitudinalPlan.solverExecutionTime = self.mpc.solve_time
 
+    longitudinalPlan.allowBrake = True
+    longitudinalPlan.allowThrottle = True
+
     longitudinalPlan.speeds = self.v_desired_trajectory.tolist()
     longitudinalPlan.accels = self.a_desired_trajectory.tolist()
     longitudinalPlan.jerks = self.j_desired_trajectory.tolist()
@@ -212,9 +200,20 @@ class LongitudinalPlanner:
     longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
-    a_target, should_stop = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
+    a_target_mpc, should_stop_mpc = get_accel_from_plan(self.CP, longitudinalPlan.speeds, longitudinalPlan.accels)
-    longitudinalPlan.aTarget = a_target
+
-    longitudinalPlan.shouldStop = should_stop
+    if sm['controlsState'].experimentalMode:
+      model_speeds = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].velocity.x)
+      model_accels = np.interp(CONTROL_N_T_IDX, ModelConstants.T_IDXS, sm['modelV2'].acceleration.x)
+      a_target_model, should_stop_model = get_accel_from_plan(self.CP, model_speeds, model_accels)
+      a_target = min(a_target_mpc, a_target_model)
+      should_stop = should_stop_mpc or should_stop_model
+    else:
+      a_target = a_target_mpc
+      should_stop = should_stop_mpc
+
+    longitudinalPlan.aTarget = float(a_target)
+    longitudinalPlan.shouldStop = bool(should_stop)
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = True
 

selfdrive/controls/radard.py

@@ -251,10 +251,10 @@ class RadarD:
     self.radar_state.radarErrors = list(radar_errors)
     self.radar_state.carStateMonoTime = sm.logMonoTime['carState']
 
-    if len(sm['modelV2'].temporalPose.trans):
+    #if len(sm['modelV2'].temporalPose.trans):
-      model_v_ego = sm['modelV2'].temporalPose.trans[0]
+    #  model_v_ego = sm['modelV2'].temporalPose.trans[0]
-    else:
+    #else:
-      model_v_ego = self.v_ego
+    model_v_ego = self.v_ego
     leads_v3 = sm['modelV2'].leadsV3
     if len(leads_v3) > 1:
       self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True)

selfdrive/locationd/calibrationd.py

@@ -286,6 +286,5 @@ def main() -> NoReturn:
     if sm.frame % 5 == 0:
       calibrator.send_data(pm, sm.all_checks())
 
-
 if __name__ == "__main__":
   main()

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

selfdrive/modeld/fill_model_msg.py

@@ -166,11 +166,12 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D
   meta.hardBrakePredicted = hard_brake_predicted.item()
 
   # temporal pose
+  # TODO
   temporal_pose = modelV2.temporalPose
-  temporal_pose.trans = net_output_data['sim_pose'][0,:3].tolist()
+  temporal_pose.trans = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
-  temporal_pose.transStd = net_output_data['sim_pose_stds'][0,:3].tolist()
+  temporal_pose.transStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
-  temporal_pose.rot = net_output_data['sim_pose'][0,3:].tolist()
+  temporal_pose.rot = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
-  temporal_pose.rotStd = net_output_data['sim_pose_stds'][0,3:].tolist()
+  temporal_pose.rotStd = np.zeros((3,), dtype=np.float32).reshape(-1).tolist()
 
   # confidence
   if vipc_frame_id % (2*ModelConstants.MODEL_FREQ) == 0:

selfdrive/modeld/modeld.py

@@ -33,6 +33,10 @@ MODEL_PATHS = {
 
 METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
 
+MODEL_WIDTH = 512
+MODEL_HEIGHT = 256
+MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 // 2
+
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -54,6 +58,8 @@ class ModelState:
     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.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),
@@ -62,6 +68,11 @@ class ModelState:
       'features_buffer': np.zeros(ModelConstants.HISTORY_BUFFER_LEN * ModelConstants.FEATURE_LEN, dtype=np.float32),
     }
 
+    self.input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.big_input_imgs_20hz = np.zeros(MODEL_FRAME_SIZE*5, dtype=np.float32)
+    self.input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+    self.big_input_imgs = np.zeros(MODEL_FRAME_SIZE*2, dtype=np.float32)
+
     with open(METADATA_PATH, 'rb') as f:
       model_metadata = pickle.load(f)
 
@@ -86,17 +97,30 @@ 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:]
+    new_desire = np.where(inputs['desire'] - self.prev_desire > .99, inputs['desire'], 0)
-    self.inputs['desire'][-ModelConstants.DESIRE_LEN:] = 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 getCLBuffer is not None, frame will be None
+    new_img = self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs"))
-    self.model.setInputBuffer("input_imgs", self.frame.prepare(buf, transform.flatten(), self.model.getCLBuffer("input_imgs")))
+    self.input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.input_imgs_20hz[MODEL_FRAME_SIZE:]
+    self.input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_img
+    self.input_imgs[:MODEL_FRAME_SIZE] = self.input_imgs_20hz[:MODEL_FRAME_SIZE]
+    self.input_imgs[MODEL_FRAME_SIZE:] = self.input_imgs_20hz[-MODEL_FRAME_SIZE:]
+    self.model.setInputBuffer("input_imgs", self.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")))
+      new_big_img = self.wide_frame.prepare(wbuf, transform_wide.flatten(), self.model.getCLBuffer("big_input_imgs"))
+      self.big_input_imgs_20hz[:-MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[MODEL_FRAME_SIZE:]
+      self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:] = new_big_img
+      self.big_input_imgs[:MODEL_FRAME_SIZE] = self.big_input_imgs_20hz[:MODEL_FRAME_SIZE]
+      self.big_input_imgs[MODEL_FRAME_SIZE:] = self.big_input_imgs_20hz[-MODEL_FRAME_SIZE:]
+      self.model.setInputBuffer("big_input_imgs", self.big_input_imgs)
+
 
     if prepare_only:
       return None
@@ -104,8 +128,11 @@ 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.full_features_20Hz[:-1] = self.full_features_20Hz[1:]
-    self.inputs['features_buffer'][-ModelConstants.FEATURE_LEN:] = outputs['hidden_state'][0, :]
+    self.full_features_20Hz[-1] = outputs['hidden_state'][0, :]
+    idxs = np.arange(-4,-100,-4)[::-1]
+    self.inputs['features_buffer'][:] = self.full_features_20Hz[idxs].flatten()
+    # 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] = self.inputs['prev_desired_curv'][ModelConstants.PREV_DESIRED_CURV_LEN:]
     self.inputs['prev_desired_curv'][-ModelConstants.PREV_DESIRED_CURV_LEN:] = outputs['desired_curvature'][0, :]
     return outputs

selfdrive/modeld/models/commonmodel.h

@@ -27,12 +27,11 @@ public:
   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;
 
 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;
+  std::unique_ptr<float[]> frame;
 };

selfdrive/modeld/models/commonmodel.pxd

@@ -15,6 +15,6 @@ cdef extern from "selfdrive/modeld/models/commonmodel.h":
   float sigmoid(float)
 
   cppclass ModelFrame:
-    int buf_size
+    int MODEL_FRAME_SIZE
     ModelFrame(cl_device_id, cl_context)
     float * prepare(cl_mem, int, int, int, int, mat3, cl_mem*)

selfdrive/modeld/models/commonmodel_pyx.cpp

@@ -21095,7 +21095,7 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
  *       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.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)
  */
   __pyx_t_3 = (!(__pyx_v_data != 0));
   if (__pyx_t_3) {
@@ -21104,7 +21104,7 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
  *       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.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)
  */
     __Pyx_XDECREF(__pyx_r);
     __pyx_r = Py_None; __Pyx_INCREF(Py_None);
@@ -21115,14 +21115,14 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
  *       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.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)
  */
   }
 
   /* "selfdrive/modeld/models/commonmodel_pyx.pyx":47
  *     if not data:
  *       return None
- *     return np.asarray(<cnp.float32_t[:self.frame.buf_size]> data)             # <<<<<<<<<<<<<<
+ *     return np.asarray(<cnp.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)             # <<<<<<<<<<<<<<
  */
   __Pyx_XDECREF(__pyx_r);
   __Pyx_GetModuleGlobalName(__pyx_t_8, __pyx_n_s_np); if (unlikely(!__pyx_t_8)) __PYX_ERR(1, 47, __pyx_L1_error)
@@ -21136,7 +21136,7 @@ static PyObject *__pyx_pf_9selfdrive_6modeld_6models_15commonmodel_pyx_10ModelFr
   }
   __pyx_t_11 = __pyx_format_from_typeinfo(&__Pyx_TypeInfo_nn___pyx_t_5numpy_float32_t); if (unlikely(!__pyx_t_11)) __PYX_ERR(1, 47, __pyx_L1_error)
   __Pyx_GOTREF(__pyx_t_11);
-  __pyx_t_8 = Py_BuildValue((char*) "("  __PYX_BUILD_PY_SSIZE_T  ")", ((Py_ssize_t)__pyx_v_self->frame->buf_size)); if (unlikely(!__pyx_t_8)) __PYX_ERR(1, 47, __pyx_L1_error)
+  __pyx_t_8 = Py_BuildValue((char*) "("  __PYX_BUILD_PY_SSIZE_T  ")", ((Py_ssize_t)__pyx_v_self->frame->MODEL_FRAME_SIZE)); if (unlikely(!__pyx_t_8)) __PYX_ERR(1, 47, __pyx_L1_error)
   __Pyx_GOTREF(__pyx_t_8);
   __pyx_t_10 = __pyx_array_new(__pyx_t_8, sizeof(__pyx_t_5numpy_float32_t), PyBytes_AS_STRING(__pyx_t_11), (char *) "c", (char *) __pyx_v_data); if (unlikely(!__pyx_t_10)) __PYX_ERR(1, 47, __pyx_L1_error)
   __Pyx_GOTREF((PyObject *)__pyx_t_10);

selfdrive/modeld/models/commonmodel_pyx.pyx

@@ -44,4 +44,4 @@ cdef class ModelFrame:
       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.float32_t[:self.frame.MODEL_FRAME_SIZE]> data)

selfdrive/modeld/parse_model_outputs.py

@@ -88,7 +88,7 @@ class Parser:
     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_mdn('pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
     self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
-    self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,))
+    # self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(ModelConstants.POSE_WIDTH,)) # TODO
     self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
     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))
@@ -98,6 +98,6 @@ class Parser:
       self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
     for k in ['lead_prob', 'lane_lines_prob', 'meta']:
       self.parse_binary_crossentropy(k, outs)
-    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    # self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,)) # TODO
     self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN,ModelConstants.DESIRE_PRED_WIDTH))
     return outs