diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9958755577..038061b7e6 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 0ad629f519..1060b36d28 100755 --- a/selfdrive/modeld/modeld.py +++ b/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: