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

i6

Browse Source
This commit is contained in:
firestar5683
2026-05-04 12:49:37 -05:00
parent 4abda00404
commit 3f3a63a14b
2 changed files with 23 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+17 -7
View File
@@ -230,19 +230,19 @@ IONIQ_6_FF_CUTOFF = 0.48
IONIQ_6_FF_CUTOFF_WIDTH = 0.12
IONIQ_6_TRANSITION_SPEED = 10.0
IONIQ_6_PHASE_SCALE = 0.10
IONIQ_6_TURN_IN_BOOST_LEFT = 1.08
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.24
IONIQ_6_TURN_IN_BOOST_LEFT = 1.14
IONIQ_6_TURN_IN_BOOST_RIGHT = 1.18
IONIQ_6_UNWIND_TAPER_LEFT = 1.84
IONIQ_6_UNWIND_TAPER_RIGHT = 4.10
IONIQ_6_FRICTION_MULT = 0.995
IONIQ_6_FRICTION_MULT = 0.970
IONIQ_6_FRICTION_LAT_RISE = 0.20
IONIQ_6_FRICTION_JERK_RISE = 0.24
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.34
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.54
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.38
IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.50
IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.15
IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 5.00
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.18
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.34
IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.20
IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.30
IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.86
IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 4.55
IONIQ_6_CENTER_TAPER_MAX = 0.056
@@ -267,6 +267,12 @@ IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.10
IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.04
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.95
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.10
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.10
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.18
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.35
IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55
IONIQ_6_OUTPUT_TAPER_SPEED = 8.5
IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5
IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90
@@ -861,11 +867,15 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH)
band_weight = onset * cutoff
heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH)
phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk)
unwind_weight = max(-phase, 0.0)
base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT)
unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT)
heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT)
heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT)
reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight)
reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight)
floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT)
floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight
return max(1.0 - reduction, floor)
selfdrive/controls/tests/test_latcontrol.py
+6 -3
View File
@@ -245,19 +245,22 @@ class TestLatControl:
assert get_ioniq_6_ff_scale(0.4, 0.0, 20.0) > get_ioniq_6_ff_scale(-0.4, 0.0, 20.0)
assert get_ioniq_6_ff_scale(0.4, 0.7, 8.0) > get_ioniq_6_ff_scale(0.4, 0.0, 8.0) > get_ioniq_6_ff_scale(0.4, -0.7, 8.0)
assert get_ioniq_6_ff_scale(-0.4, -0.7, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.0, 8.0) >= get_ioniq_6_ff_scale(-0.4, 0.7, 8.0)
assert abs(get_ioniq_6_ff_scale(1.2, 0.0, 20.0) - 1.0) < 0.02
assert get_ioniq_6_ff_scale(-1.2, 0.0, 20.0) < get_ioniq_6_ff_scale(1.2, 0.0, 20.0) < 1.0
assert get_ioniq_6_ff_scale(-1.2, 0.7, 20.0) <= get_ioniq_6_ff_scale(-1.2, 0.0, 20.0)
def test_ioniq_6_directional_taper_curve(self):
assert get_ioniq_6_directional_taper_scale(0.0, 0.0) == 1.0
assert get_ioniq_6_directional_taper_scale(-0.5, 0.0) < get_ioniq_6_directional_taper_scale(0.5, 0.0) < 1.0
assert get_ioniq_6_directional_taper_scale(-0.5, 0.7) <= get_ioniq_6_directional_taper_scale(-0.5, 0.0)
assert get_ioniq_6_directional_taper_scale(1.2, 0.0) > 0.96
assert get_ioniq_6_directional_taper_scale(-1.2, 0.0) < get_ioniq_6_directional_taper_scale(1.2, 0.0) < 1.0
assert get_ioniq_6_directional_taper_scale(-1.2, 0.7) <= get_ioniq_6_directional_taper_scale(-1.2, 0.0)
def test_ioniq_6_output_taper_curve(self):
assert get_ioniq_6_output_taper_scale(0.0, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.0, 0.0, 8.0) <= 1.0
assert get_ioniq_6_output_taper_scale(-0.5, 0.0, 25.0) < get_ioniq_6_output_taper_scale(0.5, 0.0, 25.0) < 1.0
assert get_ioniq_6_output_taper_scale(-0.5, 0.7, 25.0) <= get_ioniq_6_output_taper_scale(-0.5, 0.0, 25.0)
assert get_ioniq_6_output_taper_scale(1.2, 0.0, 25.0) > 0.94
assert get_ioniq_6_output_taper_scale(-1.2, 0.0, 25.0) < get_ioniq_6_output_taper_scale(1.2, 0.0, 25.0) < 1.0
assert get_ioniq_6_output_taper_scale(-1.2, 0.7, 25.0) <= get_ioniq_6_output_taper_scale(-1.2, 0.0, 25.0)
def test_ioniq_6_friction_threshold_curve(self):
base = max(get_friction_threshold(6.0), 0.35)