mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-28 01:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

i6 highway

Browse Source
This commit is contained in:
firestar5683
2026-05-13 11:29:40 -05:00
parent 06c00b0895
commit 3ecb73dc8a
2 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+11 -1
View File
@@ -334,6 +334,11 @@ IONIQ_6_CENTER_TAPER_LAT = 0.24
IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
IONIQ_6_CENTER_TAPER_SPEED = 18.0
IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.022
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT = 0.14
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH = 0.03
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 26.0
IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8
IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.088
IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.28
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.06
@@ -1128,6 +1133,11 @@ def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -
center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH)
high_speed_reduction = 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)
highway_center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
IONIQ_6_HIGHWAY_CENTER_TAPER_LAT_WIDTH)
highway_center_reduction = IONIQ_6_HIGHWAY_CENTER_TAPER_MAX * highway_speed_weight * highway_center_weight
low_mid_onset = _ioniq_6_sigmoid((v_ego - IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH)
low_mid_cutoff = _ioniq_6_sigmoid((IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX - v_ego) / IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH)
low_mid_speed_weight = low_mid_onset * low_mid_cutoff
@@ -1135,7 +1145,7 @@ def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -
IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH)
low_mid_reduction = IONIQ_6_LOW_MID_CENTER_TAPER_MAX * low_mid_speed_weight * low_mid_center_weight
return 1.0 - min(high_speed_reduction + low_mid_reduction, 0.12)
return 1.0 - min(high_speed_reduction + highway_center_reduction + low_mid_reduction, 0.12)
def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
selfdrive/controls/tests/test_latcontrol.py
+4 -1
View File
@@ -344,7 +344,10 @@ class TestLatControl:
assert get_ioniq_6_center_taper_scale(0.0, 10.0) > get_ioniq_6_center_taper_scale(0.0, 30.0)
assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.2, 30.0)
assert get_ioniq_6_center_taper_scale(0.0, 12.0) < get_ioniq_6_center_taper_scale(0.25, 12.0)
assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 7.0e-2
assert get_ioniq_6_center_taper_scale(0.0, 27.0) < get_ioniq_6_center_taper_scale(0.0, 22.0)
assert get_ioniq_6_center_taper_scale(0.24, 27.0) > 0.95
assert get_ioniq_6_center_taper_scale(0.24, 22.0) - get_ioniq_6_center_taper_scale(0.24, 27.0) < 1.0e-2
assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 7.2e-2
def test_kia_ev6_ff_scale_curve(self):
assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0