mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-11 18:53:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

kona

Browse Source
This commit is contained in:
firestar5683
2026-09-10 21:28:03 -05:00
parent fbfadc65da
commit ecbd6362f7
3 changed files with 77 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+5
View File
@@ -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:
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+36
View File
@@ -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")
selfdrive/controls/tests/test_latcontrol.py
+36
View File
@@ -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