From 148929d2f12227dfa255fdedb39fbcc22fc03a1f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 26 May 2026 11:09:12 -0500 Subject: [PATCH] i5 --- selfdrive/controls/lib/latcontrol_torque.py | 32 +++++++++++++++------ selfdrive/controls/tests/test_latcontrol.py | 7 ++++- 2 files changed, 29 insertions(+), 10 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 1cad8c919..f9b9573bb 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -325,27 +325,32 @@ GENESIS_G90_TURN_IN_FRICTION_BOOST_RIGHT = 0.14 GENESIS_G90_UNWIND_FRICTION_REDUCTION_LEFT = 0.04 GENESIS_G90_UNWIND_FRICTION_REDUCTION_RIGHT = 0.30 -IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT = 1.18 +IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT = 1.22 IONIQ_5_FF_ONSET = 0.10 IONIQ_5_FF_ONSET_WIDTH = 0.05 IONIQ_5_FF_CUTOFF = 1.20 IONIQ_5_FF_CUTOFF_WIDTH = 0.30 IONIQ_5_TRANSITION_SPEED = 11.0 IONIQ_5_PHASE_SCALE = 0.10 -IONIQ_5_FF_REDUCTION_LEFT = 0.10 +IONIQ_5_FF_REDUCTION_LEFT = 0.14 IONIQ_5_FF_REDUCTION_RIGHT = 0.18 IONIQ_5_TURN_IN_BOOST_LEFT = 0.04 IONIQ_5_TURN_IN_BOOST_RIGHT = 0.00 -IONIQ_5_UNWIND_TAPER_LEFT = 0.40 -IONIQ_5_UNWIND_TAPER_RIGHT = 0.70 +IONIQ_5_UNWIND_TAPER_LEFT = 0.52 +IONIQ_5_UNWIND_TAPER_RIGHT = 0.82 IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.05 IONIQ_5_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.00 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.18 -IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.32 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_LEFT = 0.24 +IONIQ_5_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.38 IONIQ_5_TURN_IN_FRICTION_BOOST_LEFT = 0.02 IONIQ_5_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.15 -IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.26 +IONIQ_5_UNWIND_FRICTION_REDUCTION_LEFT = 0.22 +IONIQ_5_UNWIND_FRICTION_REDUCTION_RIGHT = 0.32 +IONIQ_5_CENTER_TAPER_MAX = 0.08 +IONIQ_5_CENTER_TAPER_LAT = 0.16 +IONIQ_5_CENTER_TAPER_LAT_WIDTH = 0.03 +IONIQ_5_CENTER_TAPER_SPEED = 20.0 +IONIQ_5_CENTER_TAPER_SPEED_WIDTH = 2.5 IONIQ_EV_OLD_BASE_LAT_ACCEL_FACTOR_MULT = 1.16 IONIQ_EV_OLD_FF_REDUCTION_LEFT = 0.16 @@ -1216,6 +1221,13 @@ def get_ioniq_5_friction_scale(v_ego: float, desired_lateral_accel: float, desir return min(max(friction_scale, 0.86), 1.04) +def get_ioniq_5_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_5_sigmoid((v_ego - IONIQ_5_CENTER_TAPER_SPEED) / IONIQ_5_CENTER_TAPER_SPEED_WIDTH) + center_weight = _ioniq_5_sigmoid((IONIQ_5_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_5_CENTER_TAPER_LAT_WIDTH) + reduction = IONIQ_5_CENTER_TAPER_MAX * speed_weight * center_weight + return 1.0 - reduction + + def _ioniq_ev_old_sigmoid(x: float) -> float: return _sigmoid(x) @@ -1699,6 +1711,7 @@ class LatControlTorque(LatControl): kia_forte_active = self.is_kia_forte kia_ev6_test_active = self.is_kia_ev6 and kia_ev6_lateral_testing_ground_active() volt_plexy_test_active = self.is_volt_cc and volt_plexy_lateral_testing_ground_active() + ioniq_5_center_taper = get_ioniq_5_center_taper_scale(setpoint, CS.vEgo) if ioniq_5_active else 1.0 volt_standard_center_taper = get_volt_standard_center_taper_scale(setpoint, CS.vEgo) if volt_standard_test_active else 1.0 ioniq_ev_old_center_taper = get_ioniq_ev_old_center_taper_scale(setpoint, CS.vEgo) if ioniq_ev_old_active else 1.0 ioniq_6_center_taper = get_ioniq_6_center_taper_scale(setpoint, CS.vEgo) if ioniq_6_active else 1.0 @@ -1727,9 +1740,10 @@ class LatControlTorque(LatControl): friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif ioniq_5_active: - ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_ioniq_5_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_5_center_taper) elif ioniq_ev_old_active: ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 570dce77a..9da9f86af 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -43,6 +43,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, + get_ioniq_5_center_taper_scale, get_ioniq_ev_old_center_taper_scale, get_ioniq_ev_old_ff_scale, get_ioniq_6_center_taper_scale, @@ -339,6 +340,10 @@ class TestLatControl: assert unwind_left_scale < 1.0 assert unwind_right_scale < unwind_left_scale + 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 + def test_ioniq_ev_old_ff_scale_curve(self): assert get_ioniq_ev_old_ff_scale(0.0, 0.0, 20.0) == 1.0 assert get_ioniq_ev_old_ff_scale(0.35, 0.0, 20.0) > get_ioniq_ev_old_ff_scale(-0.35, 0.0, 20.0) @@ -494,7 +499,7 @@ class TestLatControl: _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) assert lac_log.active - assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.18) + assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 1.22) def test_ioniq_6_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6)