diff --git a/common/libcommon.a b/common/libcommon.a index b7be2b0ed..e11d09e1c 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 3bdbf23a2..bf341db8c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -447,6 +447,8 @@ inline static std::unordered_map 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}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 723b05171..2e2958d32 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 599b2a326..225b0679e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -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, diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 079a46234..0bc23f7da 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -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, diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 00bf37ee6..119c2768b 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -19,6 +19,8 @@ SAFE_MODE_MANAGED_KEYS = ( "ModelVersion", "DrivingModelVersion", "ModelRandomizer", + "LatSmoothSeconds", + "LongSmoothSeconds", "DisableOpenpilotLongitudinal", "ClusterOffset", "LateralTune", diff --git a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json index 4e351cab5..b4be6d982 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json @@ -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", diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 2ab19d6b5..a36884a2c 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -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() diff --git a/tools/StarPilot/derive_feasible_params.py b/tools/StarPilot/derive_feasible_params.py index 0237bd935..aa29ded4a 100755 --- a/tools/StarPilot/derive_feasible_params.py +++ b/tools/StarPilot/derive_feasible_params.py @@ -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') diff --git a/tools/StarPilot/feasibleparams.txt b/tools/StarPilot/feasibleparams.txt index 3cca18831..d462f47c0 100644 --- a/tools/StarPilot/feasibleparams.txt +++ b/tools/StarPilot/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