diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 304e4d894..dead8a691 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -211,39 +211,39 @@ 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 = 0.58 -IONIQ_6_TURN_IN_BOOST_RIGHT = 0.42 -IONIQ_6_UNWIND_TAPER_LEFT = 0.94 -IONIQ_6_UNWIND_TAPER_RIGHT = 1.74 +IONIQ_6_TURN_IN_BOOST_LEFT = 0.66 +IONIQ_6_TURN_IN_BOOST_RIGHT = 0.50 +IONIQ_6_UNWIND_TAPER_LEFT = 1.08 +IONIQ_6_UNWIND_TAPER_RIGHT = 1.92 IONIQ_6_FRICTION_MULT = 0.995 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.14 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.72 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 1.56 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.06 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.05 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 0.60 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.28 -IONIQ_6_CENTER_TAPER_MAX = 0.032 -IONIQ_6_CENTER_TAPER_LAT = 0.13 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 0.84 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 1.74 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.07 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 0.72 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 1.44 +IONIQ_6_CENTER_TAPER_MAX = 0.036 +IONIQ_6_CENTER_TAPER_LAT = 0.15 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 IONIQ_6_CENTER_TAPER_SPEED = 18.0 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5 -IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.024 -IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.16 +IONIQ_6_LOW_MID_CENTER_TAPER_MAX = 0.032 +IONIQ_6_LOW_MID_CENTER_TAPER_LAT = 0.18 IONIQ_6_LOW_MID_CENTER_TAPER_LAT_WIDTH = 0.03 -IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 9.5 -IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 14.0 +IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MIN = 9.0 +IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_MAX = 15.0 IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.0 IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.15 IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08 -IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.02 -IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.30 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.28 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.86 +IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.04 +IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.34 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.34 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.96 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.70 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b0530f7a9..0d1c5af4b 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -153,7 +153,7 @@ class TestLatControl: def test_volt_standard_ff_scale_curve(self): assert get_volt_standard_ff_scale(0.0, 0.0, 20.0) == 1.0 - assert get_volt_standard_ff_scale(-0.5, 0.0, 20.0) > get_volt_standard_ff_scale(0.5, 0.0, 20.0) + assert get_volt_standard_ff_scale(-0.5, 0.0, 20.0) >= get_volt_standard_ff_scale(0.5, 0.0, 20.0) assert get_volt_standard_ff_scale(0.6, 0.7, 8.0) > get_volt_standard_ff_scale(0.6, 0.0, 8.0) > get_volt_standard_ff_scale(0.6, -0.7, 8.0) assert get_volt_standard_ff_scale(-0.6, -0.7, 8.0) > get_volt_standard_ff_scale(-0.6, 0.0, 8.0) > get_volt_standard_ff_scale(-0.6, 0.7, 8.0) assert get_volt_standard_ff_scale(2.0, 0.0, 20.0) < get_volt_standard_ff_scale(0.8, 0.0, 20.0) @@ -212,20 +212,19 @@ class TestLatControl: assert get_ioniq_6_ff_scale(0.0, 0.0, 20.0) == 1.0 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 get_ioniq_6_ff_scale(0.4, 0.0, 20.0) > get_ioniq_6_ff_scale(1.2, 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 abs(get_ioniq_6_ff_scale(1.2, 0.0, 20.0) - 1.0) < 0.02 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(-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 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(-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 def test_ioniq_6_friction_threshold_curve(self): @@ -251,7 +250,7 @@ class TestLatControl: assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.0, 10.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) < 1e-3 + assert abs(get_ioniq_6_center_taper_scale(0.2, 30.0) - 1.0) < 3e-3 def test_kia_ev6_ff_scale_curve(self): assert get_kia_ev6_ff_scale(0.0, 0.0, 20.0) == 1.0