From 94aff6faed65ee0043cd7ab539f1709b2d5f8e8b Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Wed, 24 Jun 2026 13:24:28 -0500 Subject: [PATCH] i6 --- selfdrive/controls/lib/latcontrol_torque.py | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index b899724c1..ebc4f0730 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -532,6 +532,11 @@ IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14 IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.12 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT = 0.12 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH = 0.035 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK = 0.18 +IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH = 0.09 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 @@ -1888,6 +1893,16 @@ def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: return 1.0 - reduction +def get_ioniq_6_highway_transition_output_taper_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) / + IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_LAT_WIDTH) + jerk_weight = _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK) / + IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_JERK_WIDTH) + reduction = IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX * speed_weight * center_weight * jerk_weight + return 1.0 - reduction + + def get_ioniq_6_low_speed_angle_assist_torque(desired_angle_deg: float, actual_angle_deg: float, current_output_torque: float, v_ego: float) -> float: angle_error = desired_angle_deg - actual_angle_deg @@ -2339,6 +2354,7 @@ class LatControlTorque(LatControl): output_torque, CS.vEgo) if ioniq_6_active: output_torque *= get_ioniq_6_highway_output_taper_scale(setpoint, CS.vEgo) + output_torque *= get_ioniq_6_highway_transition_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif volt_standard_test_active: output_torque *= volt_standard_center_taper elif volt_plexy_test_active: