From cdb266cba25860aa40db4a467fc453b184265ba5 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 6 Jul 2026 15:12:46 -0500 Subject: [PATCH] Weaving Straights --- selfdrive/controls/lib/latcontrol_torque.py | 7 ++++++- .../controls/lib/latcontrol_vehicle_tunes.py | 16 ++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 10 ++++++++++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index be460ceb0..7f4625ed7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -293,6 +293,7 @@ class LatControlTorque(LatControl): friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3) friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper) + friction_scale *= get_ioniq_6_friction_center_fade_scale(setpoint, CS.vEgo) elif sonata_active: ff *= get_sonata_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * sonata_center_taper elif sonata_hybrid_active: @@ -326,7 +327,11 @@ class LatControlTorque(LatControl): if trailer_load_kg > 0.0: ff *= get_trailer_lateral_ff_scale(trailer_load_kg, CS.vEgo, setpoint) friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint) - ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) + friction_jerk = desired_lateral_jerk + if ioniq_6_active: + # planner jerk noise on straights (< ~0.3 m/s^3) chatters the friction compensation + friction_jerk = math.copysign(max(abs(desired_lateral_jerk) - IONIQ_6_FRICTION_JERK_DEADZONE, 0.0), desired_lateral_jerk) + ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * friction_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: boost_scale = np.interp(abs(gravity_adjusted_future_lateral_accel), [0.0, DEADZONE_BOOST_LAT_ACCEL], [1.0, 0.0]) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index bd7b893a5..a386ac8ee 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -571,6 +571,16 @@ IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS +# Friction compensation near zero lateral accel amplifies planner jerk noise into a slow +# (~0.5 Hz) weave on straights: the 0.09/0.39 small-signal slope plus the jerk feed acts as +# extra P/D gain right where there is no breakaway torque to overcome. Deadzone the jerk +# feed below straight-line noise levels and fade friction near center at highway speed. +IONIQ_6_FRICTION_JERK_DEADZONE = 0.30 +IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50 +IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15 +IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06 +IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0 +IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.03 @@ -1839,6 +1849,12 @@ def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desir return min(max(friction_scale, 0.82), 1.08) +def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float: + speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH) + center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH) + return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight + + def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float: speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH) center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index e03f7a98f..8fb48421d 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -60,6 +60,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_ioniq_6_directional_taper_scale, get_ioniq_6_output_taper_scale, get_ioniq_6_ff_scale, + get_ioniq_6_friction_center_fade_scale, get_ioniq_6_friction_scale, get_ioniq_6_friction_threshold, get_ioniq_6_low_speed_angle_assist_torque, @@ -546,6 +547,15 @@ class TestLatControl: assert right_turn_in >= left_turn_in > base assert base > left_unwind >= right_unwind + def test_ioniq_6_friction_center_fade_curve(self): + # fades friction near zero lateral accel at highway speed, inactive at city speed and in turns + assert get_ioniq_6_friction_center_fade_scale(0.0, 30.0) < get_ioniq_6_friction_center_fade_scale(0.0, 8.0) + assert get_ioniq_6_friction_center_fade_scale(0.0, 30.0) < get_ioniq_6_friction_center_fade_scale(0.5, 30.0) + assert get_ioniq_6_friction_center_fade_scale(0.0, 30.0) >= 0.5 + assert get_ioniq_6_friction_center_fade_scale(0.5, 30.0) > 0.95 + assert get_ioniq_6_friction_center_fade_scale(-0.5, 30.0) > 0.95 + assert get_ioniq_6_friction_center_fade_scale(0.0, 8.0) > 0.95 + def test_ioniq_6_center_taper_curve(self): assert get_ioniq_6_center_taper_scale(0.0, 10.0) > get_ioniq_6_center_taper_scale(0.0, 30.0) assert get_ioniq_6_center_taper_scale(0.0, 30.0) < get_ioniq_6_center_taper_scale(0.2, 30.0)