From dc492787eb63fc8e16d9726a1bb0db69aa70fce5 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 29 Sep 2026 15:46:14 -0500 Subject: [PATCH] Add Ioniq 6 weave tune and HKG highway friction threshold test toggles --- common/params_keys.h | 2 + selfdrive/controls/lib/latcontrol_torque.py | 2 + .../controls/lib/latcontrol_vehicle_tunes.py | 43 +++++++++++++++--- .../tests/test_lateral_test_toggles.py | 44 +++++++++++++++++++ .../ui/layouts/settings/starpilot/lateral.py | 30 +++++++++---- selfdrive/ui/lib/starpilot_state.py | 2 + starpilot/common/starpilot_variables.py | 6 +++ 7 files changed, 115 insertions(+), 14 deletions(-) create mode 100644 selfdrive/controls/tests/test_lateral_test_toggles.py diff --git a/common/params_keys.h b/common/params_keys.h index 7f6f5a558f..f6e4add645 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -420,6 +420,7 @@ inline static std::unordered_map keys = { {"GoatScream", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"GoatScreamCriticalAlerts", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"GreenLightAlert", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, + {"HKGHighwayFrictionThreshold", {PERSISTENT, BOOL, "0", "0", 3}}, {"HideAlerts", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"HideChangingLanesBanner", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"HideDistanceProfileBanner", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, @@ -448,6 +449,7 @@ inline static std::unordered_map keys = { {"IncreaseFollowingSnow", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}}, {"AggressiveCoolingEnabled", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, {"IncreaseThermalLimits", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}}, + {"Ioniq6WeaveTune", {PERSISTENT, BOOL, "1", "1", 3}}, {"IssueReported", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}}, {"KonikMinutes", {PERSISTENT, INT, "0", "0", 0}}, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4243da6178..a7bad6e5ea 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -229,6 +229,8 @@ class LatControlTorque(LatControl): flm_profile_active = bool(getattr(starpilot_toggles, "flm_trial_applied", False) and getattr(starpilot_toggles, "flm_active_profile_id", "")) set_flm_runtime_overrides(getattr(starpilot_toggles, "flm_active_overrides", None) if flm_profile_active else None) + set_lateral_test_toggles(getattr(starpilot_toggles, "ioniq_6_weave_tune", True), + getattr(starpilot_toggles, "hkg_highway_friction_threshold", False)) flm_surface_active = flm_profile_active and flm_runtime_overrides_active() measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) measurement = measured_curvature * CS.vEgo ** 2 diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index d2b65771ce..fef9586c9f 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -15,6 +15,8 @@ from openpilot.starpilot.common.testing_grounds import testing_ground CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 STANDARD_FRICTION_THRESHOLD = 0.30 HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39 +HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD = 0.78 +HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD_BP = [15.0, 25.0] # m/s FLM_SCHEMA_VERSION = 1 FLM_FRICTION_SPEED_KNOTS = [0.0, 5.0, 10.0, 15.0, 25.0] CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 @@ -983,6 +985,13 @@ IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15 IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.14 IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0 IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5 +# values before the weave tune, used when the Ioniq6WeaveTune toggle is off +IONIQ_6_LEGACY_CENTER_TAPER_LAT_WIDTH = 0.025 +IONIQ_6_LEGACY_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 +IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_LEFT = 0.11 +IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45 +IONIQ_6_LEGACY_FRICTION_CENTER_FADE_MAX = 0.50 +IONIQ_6_LEGACY_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06 # Newer Ioniq 6 highway center-chatter correction; activation is firmware-gated. IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80 IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45 @@ -1339,6 +1348,19 @@ TRAILER_LATERAL_FRICTION_GAIN = 0.03 _FLM_ACTIVE_OVERRIDES_TEXT = "" _FLM_ACTIVE_OVERRIDES = {} +_IONIQ_6_WEAVE_TUNE = True +_HKG_HIGHWAY_FRICTION_THRESHOLD = False + + +def set_lateral_test_toggles(ioniq_6_weave_tune: bool, hkg_highway_friction_threshold: bool) -> None: + global _IONIQ_6_WEAVE_TUNE, _HKG_HIGHWAY_FRICTION_THRESHOLD + _IONIQ_6_WEAVE_TUNE = bool(ioniq_6_weave_tune) + _HKG_HIGHWAY_FRICTION_THRESHOLD = bool(hkg_highway_friction_threshold) + + +def _ioniq_6_weave(tuned: float, legacy: float) -> float: + return tuned if _IONIQ_6_WEAVE_TUNE else legacy + def _sigmoid(x: float) -> float: if x >= 0.0: @@ -1358,7 +1380,11 @@ def _standard_friction_threshold_default(v_ego: float) -> float: def _hkg_canfd_base_friction_threshold_default(v_ego: float) -> float: - return max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + threshold = max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD) + if _HKG_HIGHWAY_FRICTION_THRESHOLD: + threshold = max(threshold, float(np.interp(v_ego, HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD_BP, + [HKG_CANFD_BASE_FRICTION_THRESHOLD, HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD]))) + return threshold def _flm_copy_json(value): @@ -3861,8 +3887,10 @@ def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desir def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH) - center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH) - return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight + lat_width = _ioniq_6_weave(IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH, IONIQ_6_LEGACY_FRICTION_CENTER_FADE_LAT_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / lat_width) + fade_max = _ioniq_6_weave(IONIQ_6_FRICTION_CENTER_FADE_MAX, IONIQ_6_LEGACY_FRICTION_CENTER_FADE_MAX) + return 1.0 - fade_max * speed_weight * center_weight def get_ioniq_6_2025_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: @@ -3914,7 +3942,8 @@ def get_ioniq_6_2025_low_speed_center_friction_scale(desired_lateral_accel: floa def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) - center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) + lat_width = _ioniq_6_weave(IONIQ_6_CENTER_TAPER_LAT_WIDTH, IONIQ_6_LEGACY_CENTER_TAPER_LAT_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / lat_width) high_speed_reduction = _flm_vehicle_knob("hyundai_ioniq_6.center_taper_max", IONIQ_6_CENTER_TAPER_MAX) * speed_weight * center_weight highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH) @@ -3959,7 +3988,9 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la curvy_turn_in_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END - abs_lateral_accel) / IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH) curvy_turn_in_trim_weight = curvy_turn_in_speed_weight * curvy_turn_in_lat_onset * curvy_turn_in_lat_cutoff * turn_in_weight - base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT) + base_reduction = _ioniq_6_side_value(desired_lateral_accel, + _ioniq_6_weave(IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_LEFT), + _ioniq_6_weave(IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT, IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_RIGHT)) unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT) heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT) heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT) @@ -4010,7 +4041,7 @@ def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / - IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH) + _ioniq_6_weave(IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH, IONIQ_6_LEGACY_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH)) reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight return 1.0 - reduction diff --git a/selfdrive/controls/tests/test_lateral_test_toggles.py b/selfdrive/controls/tests/test_lateral_test_toggles.py new file mode 100644 index 0000000000..0b6d3999d1 --- /dev/null +++ b/selfdrive/controls/tests/test_lateral_test_toggles.py @@ -0,0 +1,44 @@ +import math + +import pytest + +from openpilot.common.constants import CV +from openpilot.selfdrive.controls.lib import latcontrol_vehicle_tunes as tunes + + +@pytest.fixture(autouse=True) +def _reset_toggles(): + yield + tunes.set_lateral_test_toggles(True, False) + + +def _fade(v_ego): + return tunes.get_ioniq_6_friction_center_fade_scale(0.0, v_ego) + + +def test_defaults_match_production(): + assert tunes._IONIQ_6_WEAVE_TUNE and not tunes._HKG_HIGHWAY_FRICTION_THRESHOLD + + +def test_weave_tune_off_restores_legacy_values(): + v = 30.0 + tunes.set_lateral_test_toggles(True, False) + tuned = (_fade(v), tunes.get_ioniq_6_center_taper_scale(0.2, v), tunes.get_ioniq_6_highway_output_taper_scale(0.12, v)) + tunes.set_lateral_test_toggles(False, False) + legacy = (_fade(v), tunes.get_ioniq_6_center_taper_scale(0.2, v), tunes.get_ioniq_6_highway_output_taper_scale(0.12, v)) + assert all(not math.isclose(a, b) for a, b in zip(tuned, legacy, strict=True)) + + speed_weight = tunes._ioniq_6_sigmoid((v - tunes.IONIQ_6_FRICTION_CENTER_FADE_SPEED) / tunes.IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH) + center_weight = tunes._ioniq_6_sigmoid(tunes.IONIQ_6_FRICTION_CENTER_FADE_LAT / tunes.IONIQ_6_LEGACY_FRICTION_CENTER_FADE_LAT_WIDTH) + assert math.isclose(legacy[0], 1.0 - tunes.IONIQ_6_LEGACY_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight) + + +def test_hkg_highway_friction_threshold(): + mph = CV.MPH_TO_MS + assert math.isclose(tunes.get_hkg_canfd_base_friction_threshold(70 * mph), tunes.HKG_CANFD_BASE_FRICTION_THRESHOLD) + + tunes.set_lateral_test_toggles(True, True) + assert math.isclose(tunes.get_hkg_canfd_base_friction_threshold(20 * mph), tunes.HKG_CANFD_BASE_FRICTION_THRESHOLD) + assert math.isclose(tunes.get_hkg_canfd_base_friction_threshold(20.0), 0.585) + assert math.isclose(tunes.get_hkg_canfd_base_friction_threshold(70 * mph), tunes.HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD) + assert math.isclose(tunes.get_ioniq_6_friction_threshold(70 * mph), tunes.HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index dd3a81bbf8..732e6cee91 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -103,17 +103,17 @@ class StarPilotLateralLayout(_SettingsPage): def aol_on(): return p.get_bool("AlwaysOnLateral") - hcg_known = [] + known_keys = {} - def hcg_available(): + def key_available(key): # raises UnknownKeyName until scons rebuilds params_pyx with the new key - if not hcg_known: + if key not in known_keys: try: - p.get_float("HighwayCorrectionGain") - hcg_known.append(True) + p.get(key) + known_keys[key] = True except Exception: - hcg_known.append(False) - return hcg_known[0] + known_keys[key] = False + return known_keys[key] def lc_on(): return p.get_bool("LaneChanges") @@ -330,7 +330,21 @@ class StarPilotLateralLayout(_SettingsPage): get_value=lambda: f"{p.get_float('HighwayCorrectionGain'):.2f}", on_click=lambda: self._show_slider("HighwayCorrectionGain", 0.3, 1.0, step=0.05, value_type="float", title="Highway Smoothing"), - visible=lambda: alt_on() and hcg_available(), + visible=lambda: alt_on() and key_available("HighwayCorrectionGain"), + ), + SettingRow( + "Ioniq6WeaveTune", "toggle", tr_noop("Ioniq 6 Weave Tune"), + subtitle=tr_noop("Smoother center tapers and friction fade for highway weave. Off = previous values."), + get_state=lambda: p.get_bool("Ioniq6WeaveTune"), + set_state=lambda s: p.put_bool("Ioniq6WeaveTune", s), + visible=lambda: alt_on() and cs.isIoniq6 and key_available("Ioniq6WeaveTune"), + ), + SettingRow( + "HKGHighwayFrictionThreshold", "toggle", tr_noop("Highway Friction Threshold"), + subtitle=tr_noop("Raises the friction threshold from 0.39 to 0.78 between 34 and 56 mph."), + get_state=lambda: p.get_bool("HKGHighwayFrictionThreshold"), + set_state=lambda s: p.put_bool("HKGHighwayFrictionThreshold", s), + visible=lambda: alt_on() and cs.isHKGCanFd and cs.isTorqueCar and not cs.isAngleCar and key_available("HKGHighwayFrictionThreshold"), ), SettingRow( "SteerLatAccel", "value", tr_noop("Lateral Acceleration"), diff --git a/selfdrive/ui/lib/starpilot_state.py b/selfdrive/ui/lib/starpilot_state.py index a736156658..543aae29c8 100644 --- a/selfdrive/ui/lib/starpilot_state.py +++ b/selfdrive/ui/lib/starpilot_state.py @@ -27,6 +27,7 @@ class StarPilotCarState: isTorqueCar: bool = False isTSK: bool = False isHKGCanFd: bool = False + isIoniq6: bool = False # ========== Car Capabilities ========== hasBSM: bool = False @@ -170,6 +171,7 @@ class StarPilotState: self.car_state.isFord = car_make == "ford" self.car_state.isHKG = car_make == "hyundai" self.car_state.isHKGCanFd = self.car_state.isHKG and safety_model == car.CarParams.SafetyModel.hyundaiCanfd + self.car_state.isIoniq6 = car_fingerprint.startswith("HYUNDAI_IONIQ_6") self.car_state.isJeep = car_make == "chrysler" and car_fingerprint.startswith("JEEP_") self.car_state.isSubaru = car_make == "subaru" self.car_state.isTesla = car_make == "tesla" diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 18889c9da3..02d6daccd4 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -796,6 +796,12 @@ class StarPilotVariables: toggle.highway_correction_gain = self.get_value("HighwayCorrectionGain", cast=float, condition=advanced_lateral_tuning, default=1.0, min=0.3, max=1.0) except Exception: # UnknownKeyName until scons rebuilds params_pyx with the new key toggle.highway_correction_gain = 1.0 + try: + toggle.ioniq_6_weave_tune = self.get_value("Ioniq6WeaveTune", condition=advanced_lateral_tuning, default=True) + toggle.hkg_highway_friction_threshold = self.get_value("HKGHighwayFrictionThreshold", condition=advanced_lateral_tuning) + except Exception: # UnknownKeyName until scons rebuilds params_pyx with the new keys + toggle.ioniq_6_weave_tune = True + toggle.hkg_highway_friction_threshold = False honda_pid_lateral = toggle.car_make == "honda" and CP.lateralTuning.which() == "pid" and not is_angle_car toggle.honda_lateral_pid_kp_scale = self.get_value("HondaLateralPidKpScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0) toggle.honda_lateral_pid_ki_scale = self.get_value("HondaLateralPidKiScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0)