Weaving Straights

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])

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)

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)