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

sonata

Browse Source
This commit is contained in:
firestar5683
2026-05-12 21:59:24 -05:00
parent 2ffb286569
commit 2516d13bc5
2 changed files with 51 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+46 -3
View File
@@ -229,16 +229,28 @@ VOLT_STANDARD_CENTER_TAPER_SPEED = 20.0
VOLT_STANDARD_CENTER_TAPER_SPEED_WIDTH = 2.5
SONATA_HYBRID_BASE_LAT_ACCEL_FACTOR_MULT = 1.04
SONATA_HYBRID_FF_REDUCTION = 0.08
SONATA_HYBRID_FF_REDUCTION_LEFT = 0.06
SONATA_HYBRID_FF_REDUCTION_RIGHT = 0.14
SONATA_HYBRID_FF_ONSET = 0.22
SONATA_HYBRID_FF_ONSET_WIDTH = 0.08
SONATA_HYBRID_FF_CUTOFF = 1.15
SONATA_HYBRID_FF_CUTOFF_WIDTH = 0.35
SONATA_HYBRID_TRANSITION_SPEED = 8.0
SONATA_HYBRID_PHASE_SCALE = 0.12
SONATA_HYBRID_TURN_IN_BOOST_LEFT = 0.04
SONATA_HYBRID_TURN_IN_BOOST_RIGHT = 0.00
SONATA_HYBRID_UNWIND_TAPER_LEFT = 0.08
SONATA_HYBRID_UNWIND_TAPER_RIGHT = 0.18
SONATA_HYBRID_CENTER_TAPER_MAX = 0.05
SONATA_HYBRID_CENTER_TAPER_LAT = 0.14
SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH = 0.025
SONATA_HYBRID_CENTER_TAPER_SPEED = 22.0
SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH = 2.5
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX = 0.10
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT = 0.09
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH = 0.02
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 6.0
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.0
GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4
GENESIS_G90_FF_GAIN_LEFT = 0.20
@@ -836,21 +848,52 @@ def _sonata_hybrid_sigmoid(x: float) -> float:
return _sigmoid(x)
def _sonata_hybrid_low_speed_factor(v_ego: float) -> float:
return 1.0 / (1.0 + (max(v_ego, 0.0) / SONATA_HYBRID_TRANSITION_SPEED) ** 2)
def _sonata_hybrid_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float:
return math.tanh((desired_lateral_accel * desired_lateral_jerk) / SONATA_HYBRID_PHASE_SCALE)
def _sonata_hybrid_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float:
return left_value if desired_lateral_accel >= 0.0 else right_value
def get_sonata_hybrid_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
del desired_lateral_jerk, v_ego
if desired_lateral_accel == 0.0:
return 1.0
abs_lateral_accel = abs(desired_lateral_accel)
onset = _sonata_hybrid_sigmoid((abs_lateral_accel - SONATA_HYBRID_FF_ONSET) / SONATA_HYBRID_FF_ONSET_WIDTH)
cutoff = _sonata_hybrid_sigmoid((SONATA_HYBRID_FF_CUTOFF - abs_lateral_accel) / SONATA_HYBRID_FF_CUTOFF_WIDTH)
return 1.0 - (SONATA_HYBRID_FF_REDUCTION * onset * cutoff)
base_reduction = _sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_FF_REDUCTION_LEFT,
SONATA_HYBRID_FF_REDUCTION_RIGHT) * onset * cutoff
phase = _sonata_hybrid_transition_phase(desired_lateral_accel, desired_lateral_jerk)
turn_in_weight = max(phase, 0.0)
unwind_weight = max(-phase, 0.0)
low_speed_factor = _sonata_hybrid_low_speed_factor(v_ego)
turn_in_boost = 1.0 + (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_TURN_IN_BOOST_LEFT,
SONATA_HYBRID_TURN_IN_BOOST_RIGHT) *
turn_in_weight * low_speed_factor)
unwind_taper = 1.0 - (_sonata_hybrid_side_value(desired_lateral_accel,
SONATA_HYBRID_UNWIND_TAPER_LEFT,
SONATA_HYBRID_UNWIND_TAPER_RIGHT) *
unwind_weight * (0.35 + 0.65 * low_speed_factor))
return (1.0 - base_reduction) * turn_in_boost * max(unwind_taper, 0.0)
def get_sonata_hybrid_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
speed_weight = _sonata_hybrid_sigmoid((v_ego - SONATA_HYBRID_CENTER_TAPER_SPEED) / SONATA_HYBRID_CENTER_TAPER_SPEED_WIDTH)
center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / SONATA_HYBRID_CENTER_TAPER_LAT_WIDTH)
reduction = SONATA_HYBRID_CENTER_TAPER_MAX * speed_weight * center_weight
low_speed_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_MAX - v_ego) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH)
low_speed_center_weight = _sonata_hybrid_sigmoid((SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT - abs(desired_lateral_accel)) /
SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_LAT_WIDTH)
reduction += SONATA_HYBRID_LOW_SPEED_CENTER_TAPER_MAX * low_speed_weight * low_speed_center_weight
return 1.0 - reduction
selfdrive/controls/tests/test_latcontrol.py
+5 -2
View File
@@ -202,7 +202,7 @@ class TestLatControl:
right_turn_in = get_volt_standard_friction_threshold(6.0, -0.7, -0.8)
left_unwind = get_volt_standard_friction_threshold(6.0, 0.7, -0.8)
right_unwind = get_volt_standard_friction_threshold(6.0, -0.7, 0.8)
assert right_turn_in < left_turn_in < base
assert right_turn_in <= left_turn_in < base
assert base < left_unwind <= right_unwind
def test_volt_standard_friction_scale_curve(self):
@@ -221,12 +221,15 @@ class TestLatControl:
def test_sonata_hybrid_ff_scale_curve(self):
assert get_sonata_hybrid_ff_scale(0.0, 0.0, 20.0) == 1.0
assert get_sonata_hybrid_ff_scale(0.6, 0.0, 20.0) == get_sonata_hybrid_ff_scale(-0.6, 0.0, 20.0)
assert get_sonata_hybrid_ff_scale(0.6, 0.0, 20.0) > get_sonata_hybrid_ff_scale(-0.6, 0.0, 20.0)
assert get_sonata_hybrid_ff_scale(0.30, 0.0, 20.0) < 1.0
assert get_sonata_hybrid_ff_scale(0.30, 0.0, 20.0) < get_sonata_hybrid_ff_scale(0.10, 0.0, 20.0)
assert get_sonata_hybrid_ff_scale(0.4, 0.7, 6.0) > get_sonata_hybrid_ff_scale(0.4, 0.0, 6.0)
assert get_sonata_hybrid_ff_scale(-0.4, 0.7, 6.0) < get_sonata_hybrid_ff_scale(-0.4, 0.0, 6.0)
def test_sonata_hybrid_center_taper_curve(self):
assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.0, 15.0)
assert get_sonata_hybrid_center_taper_scale(0.0, 3.0) < get_sonata_hybrid_center_taper_scale(0.0, 10.0)
assert get_sonata_hybrid_center_taper_scale(0.0, 30.0) < get_sonata_hybrid_center_taper_scale(0.20, 30.0) <= 1.0
def test_genesis_g90_ff_scale_curve(self):