From ecbd6362f79d1c52bf0356941495fdfe2b5c92d6 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 10 Sep 2026 21:28:03 -0500 Subject: [PATCH] kona --- selfdrive/controls/lib/latcontrol_torque.py | 5 +++ .../controls/lib/latcontrol_vehicle_tunes.py | 36 +++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 36 +++++++++++++++++++ 3 files changed, 77 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e8f58002b7..0ce7de9a97 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -129,6 +129,7 @@ class LatControlTorque(LatControl): self.is_kia_stinger_2022 = CP.carFingerprint in KIA_STINGER_2022_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kona_non_scc = CP.carFingerprint in KONA_NON_SCC_CARS + self.is_kona_ev_2022 = CP.carFingerprint in KONA_EV_2022_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_kia_carnival = CP.carFingerprint in KIA_CARNIVAL_CARS self.is_tucson_4th_gen = CP.carFingerprint in TUCSON_4TH_GEN_CARS @@ -462,6 +463,8 @@ class LatControlTorque(LatControl): friction_scale *= get_kia_carnival_friction_center_fade_scale(setpoint, CS.vEgo) elif self.is_kona_non_scc: friction_threshold = get_kona_non_scc_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + elif self.is_kona_ev_2022: + friction_threshold = get_kona_ev_2022_friction_threshold(CS.vEgo, setpoint) elif tucson_4th_gen_active: friction_threshold = get_tucson_4th_gen_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif self.is_silverado: @@ -607,6 +610,8 @@ class LatControlTorque(LatControl): rapid_reversal = setpoint * desired_lateral_jerk < 0.0 if output_torque * setpoint > 0.0 or rapid_reversal: output_torque *= get_kona_non_scc_highway_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo) + elif self.is_kona_ev_2022: + output_torque *= get_kona_ev_2022_center_output_scale(setpoint, CS.vEgo) elif rav4_tss2_active: output_torque *= get_rav4_tss2_center_output_scale(setpoint, CS.vEgo) elif rav4_prime_active: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 5c53209854..40d0c36582 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -171,6 +171,9 @@ KIA_FORTE_CARS = ( KONA_NON_SCC_CARS = ( HYUNDAI_CAR.HYUNDAI_KONA_NON_SCC, ) +KONA_EV_2022_CARS = ( + HYUNDAI_CAR.HYUNDAI_KONA_EV_2022, +) PRIUS_CARS = ( TOYOTA_CAR.TOYOTA_PRIUS, TOYOTA_CAR.TOYOTA_PRIUS_RETROFIT, @@ -1313,6 +1316,16 @@ KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT = 0.28 KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_ONSET = 11.0 KONA_NON_SCC_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_GAIN = 0.08 +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT = 0.20 +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.05 +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 +KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.5 +KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX = 0.045 +KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT = 0.20 +KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.05 +KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED = 18.0 +KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED_WIDTH = 2.5 TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS @@ -2017,6 +2030,29 @@ def get_kona_non_scc_center_taper_scale(desired_lateral_accel: float, v_ego: flo return 1.0 - (KONA_NON_SCC_CENTER_TAPER_MAX * speed_weight * center_weight) +def _kona_ev_2022_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((v_ego - KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED) / + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH) + center_weight = _sigmoid((KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_LAT_WIDTH) + return speed_weight, center_weight + + +def get_kona_ev_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0) -> float: + speed_weight, center_weight = _kona_ev_2022_center_weights(desired_lateral_accel, v_ego) + return get_standard_friction_threshold(v_ego) * ( + 1.0 + KONA_EV_2022_CENTER_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight + ) + + +def get_kona_ev_2022_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _sigmoid((v_ego - KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED) / + KONA_EV_2022_CENTER_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + KONA_EV_2022_CENTER_OUTPUT_TAPER_LAT_WIDTH) + return 1.0 - (KONA_EV_2022_CENTER_OUTPUT_TAPER_MAX * speed_weight * center_weight) + + def civic_bosch_modified_lateral_testing_ground_active() -> bool: return testing_ground.use("8", "B") diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 7edb0efc93..764cdc8aeb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -34,6 +34,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_hkg_canfd_base_friction_threshold, get_ioniq_6_2025_low_speed_center_error_scale, get_ioniq_6_2025_low_speed_center_friction_scale, + get_kona_ev_2022_center_output_scale, + get_kona_ev_2022_friction_threshold, get_kona_non_scc_center_taper_scale, get_kona_non_scc_friction_threshold, get_kona_non_scc_highway_transition_output_scale, @@ -1388,6 +1390,40 @@ class TestLatControl: assert base_output > 0.0 assert tapered_output == pytest.approx(base_output) + def test_kona_ev_2022_center_cleanup_is_high_speed_and_center_gated(self): + low_speed_threshold = get_kona_ev_2022_friction_threshold(8.0, 0.0) + highway_base = get_standard_friction_threshold(27.0) + highway_threshold = get_kona_ev_2022_friction_threshold(27.0, 0.0) + highway_curve_threshold = get_kona_ev_2022_friction_threshold(27.0, 0.6) + + assert low_speed_threshold == pytest.approx(get_standard_friction_threshold(8.0), abs=0.001) + assert highway_threshold > highway_base + assert highway_curve_threshold == pytest.approx(highway_base, abs=0.001) + assert get_kona_ev_2022_center_output_scale(0.0, 27.0) < 0.96 + assert get_kona_ev_2022_center_output_scale(0.6, 27.0) == pytest.approx(1.0, abs=0.001) + assert get_kona_ev_2022_center_output_scale(0.0, 8.0) == pytest.approx(1.0, abs=0.001) + + def test_kona_ev_2022_center_output_taper_update_path(self, monkeypatch): + monkeypatch.setattr(latcontrol_torque, "get_kona_ev_2022_center_output_scale", lambda *_args: 1.0) + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_KONA_EV_2022) + CS.vEgo = 27.0 + base_output, _, lac_log = controller.update( + True, CS, VM, params, False, 0.0005, False, 0.1, None, None, starpilot_toggles, + ) + + monkeypatch.setattr(latcontrol_torque, "get_kona_ev_2022_center_output_scale", lambda *_args: 0.5) + tapered_controller, tapered_VM, tapered_CS, tapered_params, tapered_toggles = self._build_torque_controller( + HYUNDAI.HYUNDAI_KONA_EV_2022, + ) + tapered_CS.vEgo = 27.0 + tapered_output, _, _ = tapered_controller.update( + True, tapered_CS, tapered_VM, tapered_params, False, 0.0005, False, 0.1, None, None, tapered_toggles, + ) + + assert controller.is_kona_ev_2022 + assert lac_log.active + assert tapered_output == pytest.approx(base_output * 0.5) + def test_ioniq_5_center_taper_curve(self): assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.0, 10.0) assert get_ioniq_5_center_taper_scale(0.0, 25.0) < get_ioniq_5_center_taper_scale(0.20, 25.0) <= 1.0