mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-25 19:22:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

tune

Browse Source
This commit is contained in:
firestar5683
2026-07-24 14:17:48 -05:00
parent 0d2f0b6825
commit 8ef7e55e74
3 changed files with 80 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+7
View File
@@ -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
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+29
View File
@@ -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)
selfdrive/controls/tests/test_latcontrol.py
+44
View File
@@ -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)