diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index a024e91113..3021cb5e3c 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -192,8 +192,6 @@ class ModelState(ModelStateBase): 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: - from tinygrad.tensor import Tensor - for key in bufs.keys(): ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data yuv_size = self.frame_buf_params[key][3] @@ -250,6 +248,10 @@ class ModelState(ModelStateBase): buf[0, :-1] = buf[0, 1:] buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0 + if 'feat_q' in self.input_queues: + feat_val = self.input_queues['feat_q'].numpy() + self.input_queues['feat_q'].assign(feat_val).realize() + return outputs def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, @@ -258,7 +260,6 @@ class ModelState(ModelStateBase): plan = model_output['plan'][0] desired_accel, should_stop = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS, action_t=long_action_t) - desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0] if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan) @@ -268,6 +269,8 @@ class ModelState(ModelStateBase): desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2 should_stop = (v_ego < 0.3 and desired_accel < 0.1) + desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, self.LONG_SMOOTH_SECONDS) + if self.generation is not None and self.generation >= 10: # smooth curvature for post FOF models if v_ego > self.MIN_LAT_CONTROL_SPEED: desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, self.LAT_SMOOTH_SECONDS)