refactor

selfdrive/controls/lib/drive_helpers.py

@@ -19,6 +19,9 @@ def clamp(val, min_val, max_val):
   clamped_val = float(np.clip(val, min_val, max_val))
   return clamped_val, clamped_val != val
 
+def smooth_value(val, prev_val, tau):
+  alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1
+  return alpha * val + (1 - alpha) * prev_val
 
 def clip_curvature(v_ego, prev_curvature, new_curvature, roll):
   # This function respects ISO lateral jerk and acceleration limits + a max curvature

selfdrive/modeld/modeld.py

@@ -26,15 +26,13 @@ from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.system import sentry
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
 from openpilot.selfdrive.modeld.parse_model_outputs import Parser
 from openpilot.selfdrive.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import DrivingModelFrame, CLContext
 
 
-
-
 PROCESS_NAME = "selfdrive.modeld.modeld"
 SEND_RAW_PRED = os.getenv('SEND_RAW_PRED')
 
@@ -46,10 +44,6 @@ POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.p
 LAT_SMOOTH_SECONDS = 0.3
 LONG_SMOOTH_SECONDS = 0.3
 
-def smooth_value(val, prev_val, tau):
-  alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1
-  return alpha * val + (1 - alpha) * prev_val
-
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action: