modelsmooth

2026-08-05 16:26:06 +08:00 · 2026-08-02 22:21:52 -05:00
parent a6a7475e7a
commit 9affcc8378
10 changed files with 103 additions and 17 deletions
@@ -447,6 +447,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
    {"ModelDrivesAndScores", {PERSISTENT, JSON, "{}", "{}"}},
    {"ModelReleasedDates", {PERSISTENT, STRING, "", "", 1}},
    {"ModelRandomizer", {PERSISTENT, BOOL, "0", "0", 2}},
+    {"LatSmoothSeconds", {PERSISTENT, FLOAT, "0.1", "0.1", 3}},
+    {"LongSmoothSeconds", {PERSISTENT, FLOAT, "0.3", "0.3", 3}},
    {"ModelSortMode", {PERSISTENT, STRING, "alphabetical", "alphabetical", 1}},
    {"ModelToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}},
    {"ModelUI", {PERSISTENT, BOOL, "1", "0", 2, SETTINGS_SIMPLE}},
@@ -17,6 +17,26 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import LongitudinalPl
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc, soften_far_radar_lead_accel, should_trigger_planner_fcw
 from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from openpilot.selfdrive.modeld.constants import ModelConstants, Plan
+from openpilot.selfdrive.modeld import modeld
+
+
+class _SmoothParams:
+  def __init__(self, value, developer=True, safe=False):
+    self.value = value
+    self.developer = developer
+    self.safe = safe
+
+  def get_bool(self, key):
+    return self.developer if key == "DeveloperUI" else self.safe
+
+  def get_float(self, key, **kwargs):
+    return self.value
+
+
+def test_model_smoothing_is_developer_gated_and_quantized():
+  assert modeld._model_smooth_seconds(_SmoothParams(0.126), "LatSmoothSeconds", 0.1) == pytest.approx(0.125)
+  assert modeld._model_smooth_seconds(_SmoothParams(0.126, developer=False), "LatSmoothSeconds", 0.1) == pytest.approx(0.1)
+  assert modeld._model_smooth_seconds(_SmoothParams(0.126, safe=True), "LatSmoothSeconds", 0.1) == pytest.approx(0.1)


 def make_lead(*, status: bool, d_rel: float = 200.0, v_lead: float = 0.0, a_lead: float = 0.0,
@@ -54,6 +54,13 @@ BUILTIN_MODEL_ALIASES = {BUILTIN_MODEL_KEY, "sc"}

 LAT_SMOOTH_SECONDS = 0.1
 LONG_SMOOTH_SECONDS = 0.3
+SMOOTH_SECONDS_STEP = 0.005
+
+def _model_smooth_seconds(params, key, default):
+  if not params.get_bool("DeveloperUI") or params.get_bool("SafeMode"):
+    return default
+  value = params.get_float(key, return_default=True, default=default)
+  return round(min(max(value, SMOOTH_SECONDS_STEP), 2.0) / SMOOTH_SECONDS_STEP) * SMOOTH_SECONDS_STEP
 MIN_LAT_CONTROL_SPEED = 0.3


@@ -114,7 +121,8 @@ def _canonical_model_id(model_id: str) -> str:

 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                          lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool,
-                          is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles) -> log.ModelDataV2.Action:
+                          is_v9: bool, is_v14: bool, is_v15: bool, starpilot_toggles,
+                          lat_smooth_seconds=LAT_SMOOTH_SECONDS, long_smooth_seconds=LONG_SMOOTH_SECONDS) -> log.ModelDataV2.Action:
    if is_v14 or is_v15:
      desired_curv_unscaled, desired_accel = model_output['action'][0]
      if is_v15:
@@ -123,9 +131,9 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
        desired_curvature = float(desired_curv_unscaled) / 100.0
      should_stop = (v_ego < 0.3 and desired_accel < 0.1)

-      desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
+      desired_accel = smooth_value(float(desired_accel), prev_action.desiredAcceleration, long_smooth_seconds)
      if v_ego > MIN_LAT_CONTROL_SPEED:
-        desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
+        desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth_seconds)
      else:
        desired_curvature = prev_action.desiredCurvature

@@ -144,7 +152,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
                                                                 ModelConstants.T_IDXS,
                                                                 action_t=long_action_t,
                                                                 vEgoStopping=v_ego_stopping)
-    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
+    desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, long_smooth_seconds)

    if is_v9:
      # V9: use desired_curvature if present; otherwise do NOT fall back to plan
@@ -155,7 +163,7 @@ def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.
    else:
      desired_curvature = get_curvature_from_output(model_output, plan, v_ego, lat_action_t, mlsim=mlsim)
    if v_ego > MIN_LAT_CONTROL_SPEED:
-      desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
+      desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, lat_smooth_seconds)
    else:
      desired_curvature = prev_action.desiredCurvature

@@ -523,9 +531,9 @@ def main(demo=False):
    CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
  cloudlog.info("modeld got CarParams: %s", CP.brand)

-  # TODO this needs more thought, use .2s extra for now to estimate other delays
-  # TODO Move smooth seconds to action function
-  long_delay = CP.longitudinalActuatorDelay + LONG_SMOOTH_SECONDS
+  lat_smooth_seconds = _model_smooth_seconds(params, "LatSmoothSeconds", LAT_SMOOTH_SECONDS)
+  long_smooth_seconds = _model_smooth_seconds(params, "LongSmoothSeconds", LONG_SMOOTH_SECONDS)
+  long_delay = CP.longitudinalActuatorDelay + long_smooth_seconds
  prev_action = log.ModelDataV2.Action()

  DH = DesireHelper()
@@ -566,11 +574,14 @@ def main(demo=False):
      meta_extra = meta_main

    sm.update(0)
+    lat_smooth_seconds = _model_smooth_seconds(params, "LatSmoothSeconds", LAT_SMOOTH_SECONDS)
+    long_smooth_seconds = _model_smooth_seconds(params, "LongSmoothSeconds", LONG_SMOOTH_SECONDS)
+    long_delay = CP.longitudinalActuatorDelay + long_smooth_seconds
    desire = DH.desire
    is_rhd = sm["driverMonitoringState"].isRHD
    frame_id = sm["roadCameraState"].frameId
    v_ego = max(sm["carState"].vEgo, 0.)
-    lat_delay = sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
+    lat_delay = sm["liveDelay"].lateralDelay + lat_smooth_seconds
    lateral_control_params = np.array([v_ego, lat_delay], dtype=np.float32)
    if sm.frame % 60 == 0:
      camera_offset.set_target(params.get_float("CameraOffset", return_default=True))
@@ -659,6 +670,7 @@ def main(demo=False):
        lat_action_t,
        long_action_t,
        v_ego, model.mlsim, model.is_v9, model.is_v14, model.is_v15, starpilot_toggles,
+        lat_smooth_seconds, long_smooth_seconds,
      )
      prev_action = action
      fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
@@ -19,6 +19,8 @@ SAFE_MODE_MANAGED_KEYS = (
  "ModelVersion",
  "DrivingModelVersion",
  "ModelRandomizer",
+  "LatSmoothSeconds",
+  "LongSmoothSeconds",
  "DisableOpenpilotLongitudinal",
  "ClusterOffset",
  "LateralTune",
@@ -3956,6 +3956,32 @@
        "is_parent_toggle": true,
        "settings_tier": "advanced"
      },
+      {
+        "key": "LatSmoothSeconds",
+        "label": "Model Lateral Smoothing (s)",
+        "description": "Developer-only model lateral action smoothing. Default is 0.10 seconds.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.005,
+        "max": 2.0,
+        "step": 0.01,
+        "precision": 3,
+        "parent_key": "DeveloperUI",
+        "settings_tier": "advanced"
+      },
+      {
+        "key": "LongSmoothSeconds",
+        "label": "Model Longitudinal Smoothing (s)",
+        "description": "Developer-only model longitudinal action smoothing. Default is 0.30 seconds.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.005,
+        "max": 2.0,
+        "step": 0.01,
+        "precision": 3,
+        "parent_key": "DeveloperUI",
+        "settings_tier": "advanced"
+      },
      {
        "key": "DeveloperMetrics",
        "label": "Developer Metrics",
@@ -96,6 +96,7 @@ GITLAB_SUBMISSIONS_PROJECT_ID = "71992109"
 GITLAB_TOKEN = os.environ.get("GITLAB_TOKEN", "")
 LEGACY_LATERAL_METHOD_API_PREFIX = "/api/" + "".join(("f", "t", "m"))
 VASM_CONFIGURATION_KEYS = {"VASMEnabled", "VASMConfidenceThreshold", "VASMSmoothSeconds", "VASMAnnotationConfig"}
+MODEL_SMOOTHING_KEYS = {"LatSmoothSeconds", "LongSmoothSeconds"}

 GALAXY_DEPS_PATH = "/data/galaxy_deps"
 LEGACY_GALAXY_DEPS_PATH = "/data/" + "".join(chr(code) for code in (112, 111, 110, 100)) + "_deps"
@@ -4395,6 +4396,16 @@ def setup(app):
        "drivingmodel": "DrivingModel",
        "drivingmodelversion": "DrivingModelVersion",
      }.get(key.lower(), key)
+      if key in MODEL_SMOOTHING_KEYS:
+        if not params.get_bool("DeveloperUI"):
+          return jsonify({"error": "Model smoothing is available only with Developer UI enabled."}), 403
+        try:
+          numeric = float(data["value"])
+        except (TypeError, ValueError):
+          return jsonify({"error": f"{key} must be numeric."}), 400
+        if not math.isfinite(numeric) or numeric < 0.005 or numeric > 2.0:
+          return jsonify({"error": f"{key} must be between 0.005 and 2.0 seconds."}), 400
+        data["value"] = round(numeric / 0.005) * 0.005
      val = data["value"]
      selected_label_input = str(data.get("label") or "").strip()

@@ -43,6 +43,10 @@ KNOWN_READ_ONLY = {
    "openpilotMinutes", "CompletedTrainingVersion"
 }

+GALAXY_ONLY_PARAMS = {
+    "LatSmoothSeconds", "LongSmoothSeconds",
+}
+
 def extract_registered_keys(params_path: str) -> set:
    """Extracts all legally registered parameter keys from common/params_keys.h"""
    registered_keys = set()
@@ -100,7 +104,7 @@ def main():
    feasible_keys = registered_keys.intersection(ui_strings)

    # 3. Filter Read-Only
-    editable_keys = feasible_keys - KNOWN_READ_ONLY
+    editable_keys = (feasible_keys | GALAXY_ONLY_PARAMS) - KNOWN_READ_ONLY

    # 4. Export
    output_path = os.path.join(os.path.dirname(__file__), 'feasibleparams.txt')
@@ -1,9 +1,9 @@
 Dynamically Derived Feasible Param Candidates (The Golden List)
 ===============================================================

-Total globally registered C++ keys:  507
-Total explicit UI string references: 415
-Total Editable/Toggleable targets:   378
+Total globally registered C++ keys:  545
+Total explicit UI string references: 426
+Total Editable/Toggleable targets:   390

 AccelerationPath
 AccelerationProfile
@@ -28,6 +28,7 @@ AlwaysOnDM
 AlwaysOnLateral
 AutomaticUpdates
 AutomaticallyDownloadModels
+AvailableModelArtifactFormats
 AvailableModelNames
 AvailableModelSeries
 AvailableModels
@@ -37,6 +38,7 @@ BlindSpotMetrics
 BlindSpotPath
 BootLogo
 BorderMetrics
+CCMLaunchAssist
 CCMLead
 CCMSetSpeedMargin
 CCMSpeed
@@ -57,6 +59,7 @@ CalibrationParams
 CalibrationProgress
 CameraView
 CancelButtonControl
+ClearNavOnOffroad
 ColorScheme
 CommunityFavorites
 ConditionalChill
@@ -112,14 +115,13 @@ ForceStops
 ForceTorqueController
 GMDashSpoofOffsets
 GMPedalLongitudinal
-HKGRemoteStartBootsComma
-IgnoreIgnitionLine
 GoatScream
 GoatScreamCriticalAlerts
 GreenLightAlert
 GsmApn
 GsmMetered
 GsmRoaming
+HKGRemoteStartBootsComma
 HasAcceptedTerms
 HideAlerts
 HideChangingLanesBanner
@@ -129,11 +131,13 @@ HideLeadMarker
 HideMaxSpeed
 HideSpeed
 HideSpeedLimit
+HideSteeringWheel
 HideTurningBanner
 HigherBitrate
 HolidayThemes
 HumanLaneChanges
 IconPack
+IgnoreIgnitionLine
 IncreaseFollowingLowVisibility
 IncreaseFollowingRain
 IncreaseFollowingRainStorm
@@ -148,6 +152,7 @@ IsLdwEnabled
 IsMetric
 IsRHD
 IsRHDOverride
+JeepBrakeHold
 KonikDongleId
 LKASButtonControl
 LaneChangeSmoothing
@@ -177,6 +182,7 @@ LongitudinalActuatorDelayStock
 LongitudinalPersonality
 LongitudinalTune
 LoudBlindspotAlert
+LoudBlindspotAlertWhenDisengaged
 LowVoltageShutdown
 MainCruiseButtonControl
 MapAcceleration
@@ -189,6 +195,9 @@ MaxDesiredAcceleration
 MinimumLaneChangeSpeed
 ModeButtonControl
 Model
+LatSmoothSeconds
+LongSmoothSeconds
+ModelManifestVersion
 ModelRandomizer
 ModelSortMode
 ModelUI
@@ -202,7 +211,6 @@ NoLogging
 NoUploads
 NostalgiaMode
 NudgelessLaneChange
-NudgelessLaneChangeOnlyWhenEngaged
 NumericalTemp
 Offroad_ExcessiveActuation
 Offset1
@@ -322,13 +330,13 @@ StandardJerkSpeedDecrease
 StandardPersonalityProfile
 StandbyMode
 StarButtonControl
+StarPilotFavoriteSlots
 StartAccel
 StartAccelStock
 StartupMessageBottom
 StartupMessageTop
 StaticPedalsOnUI
 SteerDelay
-SteerDelayModeMigrated
 SteerDelayStock
 SteerFriction
 SteerFrictionStock
@@ -358,6 +366,7 @@ TrafficJerkDeceleration
 TrafficJerkSpeed
 TrafficJerkSpeedDecrease
 TrafficPersonalityProfile
+TrailerLoad
 TruckTuning
 TuningLevel
 TuningLevelConfirmed