From 8ef7e55e7461407eefcceeb239385b21f2f833d4 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Fri, 24 Jul 2026 14:17:48 -0500 Subject: [PATCH] tune --- selfdrive/controls/lib/latcontrol_torque.py | 7 +++ .../controls/lib/latcontrol_vehicle_tunes.py | 29 ++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 44 +++++++++++++++++++ 3 files changed, 80 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 6b44c8ff7..a84bcf9ba 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -97,6 +97,7 @@ class LatControlTorque(LatControl): self.is_elantra_non_scc = CP.carFingerprint in ELANTRA_NON_SCC_CARS self.is_kia_xceed = CP.carFingerprint in KIA_XCEED_CARS self.is_kia_niro_phev_2022 = CP.carFingerprint in KIA_NIRO_PHEV_2022_CARS + self.is_kia_stinger_2022 = CP.carFingerprint in KIA_STINGER_2022_CARS self.is_kia_forte = CP.carFingerprint in KIA_FORTE_CARS self.is_kia_ev6 = CP.carFingerprint in KIA_EV6_CARS self.is_kia_carnival = CP.carFingerprint in KIA_CARNIVAL_CARS @@ -254,6 +255,7 @@ class LatControlTorque(LatControl): elantra_non_scc_active = self.is_elantra_non_scc kia_xceed_active = self.is_kia_xceed kia_niro_phev_2022_active = self.is_kia_niro_phev_2022 + kia_stinger_2022_active = self.is_kia_stinger_2022 kia_forte_active = self.is_kia_forte kia_ev6_active = self.is_kia_ev6 kia_carnival_active = self.is_kia_carnival @@ -269,6 +271,7 @@ class LatControlTorque(LatControl): sonata_hybrid_center_taper = get_sonata_hybrid_center_taper_scale(setpoint, CS.vEgo) if sonata_hybrid_active else 1.0 kia_xceed_center_taper = get_kia_xceed_center_taper_scale(setpoint, CS.vEgo) if kia_xceed_active else 1.0 kia_niro_phev_2022_center_taper = get_kia_niro_phev_2022_center_taper_scale(setpoint, CS.vEgo) if kia_niro_phev_2022_active else 1.0 + kia_stinger_2022_center_taper = get_kia_stinger_2022_center_taper_scale(setpoint, CS.vEgo) if kia_stinger_2022_active else 1.0 kia_forte_center_taper = get_kia_forte_center_taper_scale(setpoint, CS.vEgo) if kia_forte_active else 1.0 kia_ev6_center_taper = get_kia_ev6_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_active else 1.0 kia_ev6_low_speed_center_taper = get_kia_ev6_low_speed_center_taper_scale(setpoint, CS.vEgo) if kia_ev6_active else 1.0 @@ -344,6 +347,8 @@ class LatControlTorque(LatControl): ff *= get_kia_xceed_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_xceed_center_taper elif kia_niro_phev_2022_active: friction_threshold = get_kia_niro_phev_2022_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + elif kia_stinger_2022_active: + friction_threshold = get_kia_stinger_2022_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) elif kia_forte_active: ff *= get_kia_forte_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * kia_forte_center_taper friction_threshold = get_kia_forte_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) @@ -435,6 +440,8 @@ class LatControlTorque(LatControl): output_torque *= silverado_center_taper elif kia_niro_phev_2022_active: output_torque *= kia_niro_phev_2022_center_taper + elif kia_stinger_2022_active: + output_torque *= kia_stinger_2022_center_taper elif self.is_civic_bosch_modified and civic_bosch_modified_a_lateral_testing_ground_active(): output_torque *= civic_bosch_modified_a_center_taper pid_log.active = True diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index b6e26b0cc..a2d584294 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -125,6 +125,9 @@ KIA_XCEED_CARS = ( KIA_NIRO_PHEV_2022_CARS = ( HYUNDAI_CAR.KIA_NIRO_PHEV_2022, ) +KIA_STINGER_2022_CARS = ( + HYUNDAI_CAR.KIA_STINGER_2022, +) KIA_FORTE_CARS = ( HYUNDAI_CAR.KIA_FORTE, HYUNDAI_CAR.KIA_FORTE_2019_NON_SCC, @@ -354,6 +357,13 @@ KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK = 0.22 KIA_NIRO_PHEV_2022_FRICTION_CALM_JERK_WIDTH = 0.06 KIA_NIRO_PHEV_2022_FRICTION_THRESHOLD_GAIN = 0.12 +KIA_STINGER_2022_CENTER_TAPER_MAX = 0.12 +KIA_STINGER_2022_CENTER_TAPER_LAT = 0.30 +KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH = 0.05 +KIA_STINGER_2022_CENTER_TAPER_SPEED = 10.0 +KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH = 2.5 +KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN = 0.10 + KIA_CARNIVAL_CENTER_TAPER_MAX = 0.20 KIA_CARNIVAL_CENTER_TAPER_LAT = 0.20 KIA_CARNIVAL_CENTER_TAPER_LAT_WIDTH = 0.055 @@ -1764,6 +1774,25 @@ def get_kia_niro_phev_2022_friction_threshold(v_ego: float, desired_lateral_acce return base_threshold * min(max(threshold_scale, 1.0), 1.18) +def _kia_stinger_2022_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: + speed_weight = _sigmoid((v_ego - KIA_STINGER_2022_CENTER_TAPER_SPEED) / KIA_STINGER_2022_CENTER_TAPER_SPEED_WIDTH) + center_weight = _sigmoid((KIA_STINGER_2022_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / + KIA_STINGER_2022_CENTER_TAPER_LAT_WIDTH) + return speed_weight, center_weight + + +def get_kia_stinger_2022_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego) + return 1.0 - (KIA_STINGER_2022_CENTER_TAPER_MAX * speed_weight * center_weight) + + +def get_kia_stinger_2022_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, + desired_lateral_jerk: float = 0.0) -> float: + del desired_lateral_jerk + speed_weight, center_weight = _kia_stinger_2022_center_weights(desired_lateral_accel, v_ego) + return get_standard_friction_threshold(v_ego) * (1.0 + KIA_STINGER_2022_FRICTION_THRESHOLD_GAIN * speed_weight * center_weight) + + def _kia_carnival_center_weights(desired_lateral_accel: float, v_ego: float) -> tuple[float, float]: speed_onset = _sigmoid((v_ego - KIA_CARNIVAL_CENTER_TAPER_SPEED) / KIA_CARNIVAL_CENTER_TAPER_SPEED_WIDTH) speed_cutoff = _sigmoid((KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX - v_ego) / KIA_CARNIVAL_CENTER_TAPER_SPEED_MAX_WIDTH) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2ffa248e5..710f3ae75 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -86,6 +86,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_kia_carnival_center_taper_scale, get_kia_carnival_friction_center_fade_scale, get_kia_carnival_friction_threshold, + get_kia_stinger_2022_center_taper_scale, + get_kia_stinger_2022_friction_threshold, get_tucson_4th_gen_center_taper_scale, get_tucson_4th_gen_friction_threshold, get_kia_ev6_center_taper_scale, @@ -930,6 +932,48 @@ class TestLatControl: assert controller.is_kia_carnival assert lac_log.active + def test_kia_stinger_2022_near_center_stabilization(self): + low_speed_center = get_kia_stinger_2022_center_taper_scale(0.0, 4.0) + highway_center = get_kia_stinger_2022_center_taper_scale(0.0, 20.0) + highway_moderate = get_kia_stinger_2022_center_taper_scale(0.30, 20.0) + highway_turn = get_kia_stinger_2022_center_taper_scale(0.60, 20.0) + + assert highway_center < 0.89 + assert highway_center < highway_moderate < highway_turn + assert low_speed_center > 0.98 + assert highway_turn > 0.99 + + base_threshold = get_standard_friction_threshold(20.0) + center_threshold = get_kia_stinger_2022_friction_threshold(20.0, 0.0) + turn_threshold = get_kia_stinger_2022_friction_threshold(20.0, 0.60) + assert center_threshold == pytest.approx(base_threshold * 1.10, rel=0.01) + assert turn_threshold == pytest.approx(base_threshold, rel=0.01) + + def test_kia_stinger_2022_default_update_path(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_STINGER_2022) + CS.vEgo = 20.0 + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert controller.is_kia_stinger_2022 + assert lac_log.active + + def test_kia_stinger_2022_tapers_near_center_output(self, monkeypatch): + tapered_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_STINGER_2022) + CS.vEgo = 20.0 + tapered_output, _, _ = tapered_controller.update( + True, CS, VM, params, False, 0.00025, False, 0.2, None, None, starpilot_toggles, + ) + + monkeypatch.setattr(latcontrol_torque, "get_kia_stinger_2022_center_taper_scale", lambda *_args: 1.0) + base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_STINGER_2022) + CS.vEgo = 20.0 + base_output, _, _ = base_controller.update( + True, CS, VM, params, False, 0.00025, False, 0.2, None, None, starpilot_toggles, + ) + + assert abs(tapered_output) < abs(base_output) + def test_tucson_4th_gen_low_speed_center_taper_curve(self): low_speed_center = get_tucson_4th_gen_center_taper_scale(0.0, 8.5) low_speed_moderate = get_tucson_4th_gen_center_taper_scale(0.30, 8.5)