diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 576e73b7e6..04d79d3330 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -44,7 +44,7 @@ class Controls(ControlsExt): self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput', - 'driverMonitoringState', 'onroadEvents', 'driverAssistance'] + self.sm_services_ext, + 'driverMonitoringState', 'onroadEvents', 'driverAssistance', 'liveDelay'] + self.sm_services_ext, poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState'] + self.pm_services_ext) @@ -93,6 +93,7 @@ class Controls(ControlsExt): torque_params.frictionCoefficientFiltered) self.LaC.extension.update_model_v2(self.sm['modelV2']) + self.LaC.extension.update_lateral_lag(self.sm['liveDelay'].lateralDelay) long_plan = self.sm['longitudinalPlan'] model_v2 = self.sm['modelV2'] diff --git a/sunnypilot/modeld_v2/SConscript b/sunnypilot/modeld_v2/SConscript index 4ade1469f0..bb4c8d4a50 100644 --- a/sunnypilot/modeld_v2/SConscript +++ b/sunnypilot/modeld_v2/SConscript @@ -13,7 +13,6 @@ common_src = [ "transforms/transform.cc", ] - # OpenCL is a framework on Mac if arch == "Darwin": frameworks += ['OpenCL'] @@ -29,4 +28,3 @@ for pathdef, fn in {'TRANSFORM': 'transforms/transform.cl', 'LOADYUV': 'transfor cython_libs = envCython["LIBS"] + libs commonmodel_lib = lenv.Library('commonmodel', common_src) lenvCython.Program('models/commonmodel_pyx.so', 'models/commonmodel_pyx.pyx', LIBS=[commonmodel_lib, *cython_libs], FRAMEWORKS=frameworks) - diff --git a/sunnypilot/modeld_v2/fill_model_msg.py b/sunnypilot/modeld_v2/fill_model_msg.py index 411b8177f3..4d04d6e5ec 100644 --- a/sunnypilot/modeld_v2/fill_model_msg.py +++ b/sunnypilot/modeld_v2/fill_model_msg.py @@ -118,30 +118,14 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D # action (includes lateral planning now) modelV2.action = action - # times at X_IDXS according to model plan - PLAN_T_IDXS = [np.nan] * ModelConstants.IDX_N - PLAN_T_IDXS[0] = 0.0 - plan_x = net_output_data['plan'][0,:,Plan.POSITION][:,0].tolist() - for xidx in range(1, ModelConstants.IDX_N): - tidx = 0 - # increment tidx until we find an element that's further away than the current xidx - while tidx < ModelConstants.IDX_N - 1 and plan_x[tidx+1] < ModelConstants.X_IDXS[xidx]: - tidx += 1 - if tidx == ModelConstants.IDX_N - 1: - # if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break - PLAN_T_IDXS[xidx] = ModelConstants.T_IDXS[ModelConstants.IDX_N - 1] - break - # interpolate to find `t` for the current xidx - current_x_val = plan_x[tidx] - next_x_val = plan_x[tidx+1] - p = (ModelConstants.X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val) if abs(next_x_val - current_x_val) > 1e-9 else float('nan') - PLAN_T_IDXS[xidx] = p * ModelConstants.T_IDXS[tidx+1] + (1 - p) * ModelConstants.T_IDXS[tidx] + # times at X_IDXS of edges and lines aren't used + LINE_T_IDXS: list[float] = [] # lane lines modelV2.init('laneLines', 4) for i in range(4): lane_line = modelV2.laneLines[i] - fill_xyzt(lane_line, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) + fill_xyzt(lane_line, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['lane_lines'][0,i,:,0], net_output_data['lane_lines'][0,i,:,1]) modelV2.laneLineStds = net_output_data['lane_lines_stds'][0,:,0,0].tolist() modelV2.laneLineProbs = net_output_data['lane_lines_prob'][0,1::2].tolist() @@ -151,7 +135,7 @@ def fill_model_msg(base_msg: capnp._DynamicStructBuilder, extended_msg: capnp._D modelV2.init('roadEdges', 2) for i in range(2): road_edge = modelV2.roadEdges[i] - fill_xyzt(road_edge, PLAN_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) + fill_xyzt(road_edge, LINE_T_IDXS, np.array(ModelConstants.X_IDXS), net_output_data['road_edges'][0,i,:,0], net_output_data['road_edges'][0,i,:,1]) modelV2.roadEdgeStds = net_output_data['road_edges_stds'][0,:,0,0].tolist() # leads diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index fe322f462e..5b9c5ef9e5 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -62,7 +62,7 @@ class ModelState: self.MIN_LAT_CONTROL_SPEED = 0.3 buffer_length = 5 if self.model_runner.is_20hz else 2 - self.frames = {'input_imgs': DrivingModelFrame(context, buffer_length), 'big_input_imgs': DrivingModelFrame(context, buffer_length)} + self.frames = {name: DrivingModelFrame(context, 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 @@ -86,7 +86,7 @@ class ModelState: self.desire_reshape_dims = (self.numpy_inputs['desire'].shape[0], self.numpy_inputs['desire'].shape[1], -1, self.numpy_inputs['desire'].shape[2]) - def run(self, buf: VisionBuf, wbuf: VisionBuf, transform: np.ndarray, transform_wide: np.ndarray, + def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray], inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None: # Model decides when action is completed, so desire input is just a pulse triggered on rising edge inputs['desire'][0] = 0 @@ -110,8 +110,7 @@ class ModelState: if key in inputs and key not in ['desire']: self.numpy_inputs[key][:] = inputs[key] - imgs_cl = {'input_imgs': self.frames['input_imgs'].prepare(buf, transform.flatten()), - 'big_input_imgs': self.frames['big_input_imgs'].prepare(wbuf, transform_wide.flatten())} + imgs_cl = {name: self.frames[name].prepare(bufs[name], transforms[name].flatten()) for name in self.model_runner.vision_input_names} # Prepare inputs using the model runner self.model_runner.prepare_inputs(imgs_cl, self.numpy_inputs, self.frames) @@ -315,6 +314,8 @@ def main(demo=False): if prepare_only: cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames") + bufs = {name: buf_extra if 'big' in name else buf_main for name in model.model_runner.vision_input_names} + transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.model_runner.vision_input_names} inputs:dict[str, np.ndarray] = { 'desire': vec_desire, 'traffic_convention': traffic_convention, @@ -324,7 +325,7 @@ def main(demo=False): inputs['lateral_control_params'] = np.array([v_ego, steer_delay], dtype=np.float32) mt1 = time.perf_counter() - model_output = model.run(buf_main, buf_extra, model_transform_main, model_transform_extra, inputs, prepare_only) + model_output = model.run(bufs, transforms, inputs, prepare_only) mt2 = time.perf_counter() model_execution_time = mt2 - mt1 diff --git a/sunnypilot/models/helpers.py b/sunnypilot/models/helpers.py index 87f00ea5fa..a614447ceb 100644 --- a/sunnypilot/models/helpers.py +++ b/sunnypilot/models/helpers.py @@ -19,7 +19,7 @@ from openpilot.system.hardware import PC from openpilot.system.hardware.hw import Paths from pathlib import Path -CURRENT_SELECTOR_VERSION = 5 +CURRENT_SELECTOR_VERSION = 6 REQUIRED_MIN_SELECTOR_VERSION = 5 USE_ONNX = os.getenv('USE_ONNX', PC) diff --git a/sunnypilot/models/runners/model_runner.py b/sunnypilot/models/runners/model_runner.py index c210d5b87e..7124662791 100644 --- a/sunnypilot/models/runners/model_runner.py +++ b/sunnypilot/models/runners/model_runner.py @@ -12,8 +12,15 @@ CUSTOM_MODEL_PATH = Paths.model_root() # Set QCOM environment variable for TICI devices, potentially enabling hardware acceleration -if TICI: +USBGPU = "USBGPU" in os.environ +if USBGPU: + os.environ['AMD'] = '1' + os.environ['AMD_IFACE'] = 'USB' +elif TICI: os.environ['QCOM'] = '1' +else: + os.environ['LLVM'] = '1' + os.environ['JIT'] = '2' # TODO: This may cause issues class ModelData: @@ -132,6 +139,13 @@ class ModelRunner(ModularRunner): return self._model_data.output_slices raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + @property + def vision_input_names(self) -> list[str]: + """Returns the list of vision input names from the input shapes.""" + if self._model_data: + return list(self._model_data.input_shapes.keys()) + raise ValueError("Model data is not available. Ensure the model is loaded correctly.") + @abstractmethod def prepare_inputs(self, imgs_cl: CLMemDict, numpy_inputs: NumpyDict, frames: FrameDict) -> dict: """ diff --git a/sunnypilot/models/runners/tinygrad/tinygrad_runner.py b/sunnypilot/models/runners/tinygrad/tinygrad_runner.py index 2e4fd45294..270890cace 100644 --- a/sunnypilot/models/runners/tinygrad/tinygrad_runner.py +++ b/sunnypilot/models/runners/tinygrad/tinygrad_runner.py @@ -54,6 +54,11 @@ class TinygradRunner(ModelRunner, SupercomboTinygrad, PolicyTinygrad, VisionTiny self.input_to_dtype[name] = info[2] # dtype self.input_to_device[name] = info[3] # device + @property + def vision_input_names(self) -> list[str]: + """Returns the list of vision input names from the input shapes.""" + return [name for name in self.input_shapes.keys() if 'img' in name] + def prepare_vision_inputs(self, imgs_cl: CLMemDict, frames: FrameDict): """Prepares vision (image) inputs as Tinygrad Tensors.""" for key in imgs_cl: @@ -109,6 +114,11 @@ class TinygradSplitRunner(ModelRunner): vision_output = self.vision_runner.run_model() return {**policy_output, **vision_output} # Combine results + @property + def vision_input_names(self) -> list[str]: + """Returns the list of vision input names from the vision runner.""" + return list(self.vision_runner.vision_input_names) + @property def input_shapes(self) -> ShapeDict: """Returns the combined input shapes from both vision and policy models.""" diff --git a/sunnypilot/neural_network_data b/sunnypilot/neural_network_data index 16946bfd9c..b59ab483c8 160000 --- a/sunnypilot/neural_network_data +++ b/sunnypilot/neural_network_data @@ -1 +1 @@ -Subproject commit 16946bfd9cca1a33e8b609d10ca3d1a0089d38e2 +Subproject commit b59ab483c86cd70d04de91ad878024cd66fe86b6 diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 800bbac67a..8ccd81d8ff 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -11,7 +11,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.modeld.constants import ModelConstants LAT_PLAN_MIN_IDX = 5 - +LATERAL_LAG_MOD = 0.0 # seconds, modifies how far in the future we look ahead for the lateral plan def get_predicted_lateral_jerk(lat_accels, t_diffs): # compute finite difference between subsequent model_v2.acceleration.y values @@ -85,12 +85,15 @@ class LatControlTorqueExtBase: # precompute time differences between ModelConstants.T_IDXS self.t_diffs = np.diff(ModelConstants.T_IDXS) - self.desired_lat_jerk_time = CP.steerActuatorDelay + 0.3 + self.desired_lat_jerk_time = CP.steerActuatorDelay + LATERAL_LAG_MOD def update_model_v2(self, model_v2): self.model_v2 = model_v2 self.model_valid = self.model_v2 is not None and len(self.model_v2.orientation.x) >= CONTROL_N + def update_lateral_lag(self, lag): + self.desired_lat_jerk_time = max(0.01, lag) + LATERAL_LAG_MOD + def update_friction_input(self, val_1, val_2): _error = val_1 - val_2 _value = self.lat_accel_friction_factor * _error + self.lat_jerk_friction_factor * self.lookahead_lateral_jerk diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 218bbb9f6a..2f89594c8f 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -45,9 +45,8 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): self.pitch_last = 0.0 # setup future time offsets - self.nn_time_offset = CP.steerActuatorDelay + 0.2 - future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future - self.nn_future_times = [i + self.nn_time_offset for i in future_times] + self.future_times = [0.3, 0.6, 1.0, 1.5] # seconds in the future + self.nn_future_times = [i + self.desired_lat_jerk_time for i in self.future_times] # setup past time offsets self.past_times = [-0.3, -0.2, -0.1] @@ -58,6 +57,10 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): self.error_deque = deque(maxlen=history_check_frames[0]) self.past_future_len = len(self.past_times) + len(self.nn_future_times) + def update_lateral_lag(self, lag): + super().update_lateral_lag(lag) + self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times] + def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None: if not self.enabled or not self.model_valid or not self.has_nn_model: return diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py index 699327cd81..01ddec68ab 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py @@ -73,17 +73,21 @@ class TestNeuralNetworkLateralControl: controller.extension.model_v2 = model_v2 # Saturate for curvature limited and controller limited + test_lag = 0.3 for _ in range(1000): controller.extension.update_model_v2(model_v2) + controller.extension.update_lateral_lag(test_lag) _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) assert lac_log.saturated for _ in range(1000): controller.extension.update_model_v2(model_v2) + controller.extension.update_lateral_lag(test_lag) _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) assert not lac_log.saturated for _ in range(1000): controller.extension.update_model_v2(model_v2) + controller.extension.update_lateral_lag(test_lag) _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) assert lac_log.saturated