Add Highway Smoothing for Ioniq 6 highway weave

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

openpilot/common/params_keys.h

@@ -151,6 +151,8 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0"}},
     {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0"}},
     {"TurnAssist", {PERSISTENT, BOOL, "1"}},
+    {"HighwaySmoothing", {PERSISTENT, BOOL, "0"}},
+    {"HighwayCorrectionGain", {PERSISTENT, FLOAT, "0.3"}},
     {"GMPedalLongitudinal", {PERSISTENT, BOOL, "0"}},
     {"ApiCache_Device", {PERSISTENT, STRING}},
     {"ApiCache_FirehoseStats", {PERSISTENT, JSON}},

openpilot/selfdrive/controls/controlsd.py

@@ -15,6 +15,7 @@ from openpilot.common.swaglog import cloudlog
 from opendbc.car.car_helpers import interfaces
 from opendbc.car.vehicle_model import VehicleModel
 from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels, read_gain
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
@@ -84,7 +85,9 @@ class Controls:
     self.desired_curvature = 0.0
     self.model_turn_assist = ModelTurnAssist()
     self.lane_centering_controller = LaneCenteringController()
-    self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params)
+    self.highway_correction_gain = HighwayCorrectionGain()
+    self.highway_gain = read_gain(self.params)
+    self.lateral_pause =LateralPause(read_lateral_pause(self.params), self.params)
     self.lane_change_policy = lane_change_policy(read_lane_change(self.params))
     self.lane_change_smoother = LaneChangeSmoother()
     self.lane_centering_host = LaneCenteringHost(self.params) if lane_runtime_supported(self.CP) else None
@@ -287,6 +290,16 @@ class Controls:
       self.last_lane_centering_result = result
       if result.candidate_curvature is not None:
         new_desired_curvature = result.candidate_curvature
+    if self.sm.frame % 100 == 0:
+      self.highway_gain = read_gain(self.params)
+    predicted = None
+    if self.sm.all_checks(['modelV2']):
+      predicted = predicted_lateral_accels(model_v2.position.t, model_v2.velocity.x, model_v2.orientationRate.z)
+    new_desired_curvature = self.highway_correction_gain.update(
+      new_desired_curvature, CS.vEgo, CC.latActive, self.highway_gain,
+      bypass=bool(CS.leftBlinker or CS.rightBlinker or CS.steeringPressed or
+                  model_v2.meta.laneChangeState != LaneChangeState.off or self.sm.valid['lateralManeuverPlan']),
+      predicted=predicted)
     previous_curvature = self.desired_curvature
     factor = self.lane_change_smoother.factor(
       active=bool(CC.latActive and CS.canValid and not CS.canTimeout and self.sm.all_checks(['modelV2']) and

openpilot/selfdrive/controls/lib/highway_correction_gain.py

@@ -0,0 +1,139 @@
+import math
+from collections import deque
+
+import numpy as np
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.starpilot.saved_source import read_saved
+
+# Highway weave is a loop through the model: it reacts to the car's own sway and openpilot delivers that
+# request ~1:1 with ~0.4 s lag. Scaling only the fast part of the request lowers that loop's gain without
+# adding lag (a low-pass adds lag and did not help). Near MIN_GAIN the output is mostly the slow baseline,
+# i.e. a low-pass again (~35 deg lag at 0.5 Hz for 0.2, ~53 deg for 0.1).
+BASELINE_TAU = 1.5                        # s
+SPEED_OFF = 30.0 * CV.MPH_TO_MS
+SPEED_ON = 40.0 * CV.MPH_TO_MS
+# Inside a curve the gate measures distance from the curve's own level (a slow average of the request)
+# instead of from zero, so a steady curve is smoothed like a straight. Until the level builds up it is the
+# original zero-referenced gate, which keeps curve entries as before.
+CURVE_LEVEL_TAU = 3.0                     # s
+LAT_ACCEL_ON = 0.25                       # m/s^2
+LAT_ACCEL_OFF = 0.6                       # m/s^2
+CURVE_LEVEL_BLEND = (0.15, 0.35)          # m/s^2 of curve level: straight thresholds -> curve thresholds
+# The model's predicted path crosses into or out of a curve ~1.2-1.7 s before its request does and wobbles
+# less, so it switches smoothing off ahead of entries, exits and tightening. With it, the request gate no
+# longer has to tighten in curves (where it tripped on the wobble it was smoothing) and stays as a backup
+# for the transitions the prediction misses.
+PREDICTION_HORIZONS = (1.5, 2.0)          # s
+PREDICTED_CURVE_ON = 0.20                 # m/s^2
+PREDICTED_CURVE_OFF = 0.45                # m/s^2
+# without a prediction the request gate tightens in curves so exits switch smoothing off in time
+CURVE_DEVIATION_ON = 0.12                 # m/s^2
+CURVE_DEVIATION_OFF = 0.30                # m/s^2
+TIGHT_CURVE_ON = 1.5                      # m/s^2
+TIGHT_CURVE_OFF = 2.0                     # m/s^2
+# held longer than the weave's peak spacing so the weave can't modulate its own gain
+ENVELOPE_HOLD = 2.0                       # s
+ENVELOPE_RELEASE_RATE = 0.3               # m/s^2 per second
+BYPASS_FADE_RATE = 2.0                    # per second
+MIN_GAIN = 0.1
+DEFAULT_GAIN = 0.3
+ENABLE_KEY = "HighwaySmoothing"
+GAIN_KEY = "HighwayCorrectionGain"
+
+
+def _smoothstep(x: float, lo: float, hi: float) -> float:
+  t = min(max((x - lo) / (hi - lo), 0.0), 1.0)
+  return t * t * (3.0 - 2.0 * t)
+
+
+def read_gain(params) -> float:
+  enabled, readable = read_saved(params, ENABLE_KEY, 8)
+  if not readable or enabled != b"1":
+    return 1.0
+  raw, readable = read_saved(params, GAIN_KEY, 16)
+  try:
+    gain = DEFAULT_GAIN if raw is None else float(raw) if readable else 1.0
+  except ValueError:
+    return 1.0
+  return min(max(gain, MIN_GAIN), 1.0) if math.isfinite(gain) else 1.0
+
+
+def predicted_lateral_accels(t, velocity_x, yaw_rate) -> tuple[float, ...] | None:
+  t, velocity_x, yaw_rate = (np.asarray(list(x), dtype=float) for x in (t, velocity_x, yaw_rate))
+  if len(t) < 2 or len(t) != len(velocity_x) or len(t) != len(yaw_rate) or t[-1] < PREDICTION_HORIZONS[-1]:
+    return None
+  return tuple(float(np.interp(h, t, velocity_x) * np.interp(h, t, yaw_rate)) for h in PREDICTION_HORIZONS)
+
+
+class _Envelope:
+  def __init__(self):
+    self.frame = 0
+    self.peaks: deque[tuple[int, float]] = deque()
+    self.value = 0.0
+
+  def update(self, x: float) -> float:
+    self.frame += 1
+    while self.peaks and self.peaks[-1][1] <= x:
+      self.peaks.pop()
+    self.peaks.append((self.frame, x))
+    while self.peaks[0][0] <= self.frame - ENVELOPE_HOLD / DT_CTRL:
+      self.peaks.popleft()
+    self.value = max(self.peaks[0][1], self.value - ENVELOPE_RELEASE_RATE * DT_CTRL)
+    return self.value
+
+
+class HighwayCorrectionGain:
+  def __init__(self):
+    self.alpha = DT_CTRL / (BASELINE_TAU + DT_CTRL)
+    self.alpha_curve = DT_CTRL / (CURVE_LEVEL_TAU + DT_CTRL)
+    self.reset()
+
+  def reset(self, curvature: float = 0.0) -> None:
+    self.baseline = curvature
+    self.curve_level = curvature
+    self.envelope = _Envelope()
+    self.prediction_envelope = _Envelope()
+    self.bypass_weight = 0.0
+    self.weight = 0.0
+
+  def update(self, curvature: float, v_ego: float, lat_active: bool, gain: float, bypass: bool,
+             predicted: tuple[float, ...] | None = None) -> float:
+    if not lat_active:
+      self.reset(curvature)
+      return curvature
+
+    # tracked even when bypassed so fading in never steps the output
+    self.baseline += self.alpha * (curvature - self.baseline)
+    self.curve_level += self.alpha_curve * (curvature - self.curve_level)
+
+    v2 = v_ego ** 2
+    level = abs(self.curve_level) * v2
+    in_curve = _smoothstep(level, *CURVE_LEVEL_BLEND)
+    reference = in_curve * self.curve_level
+    envelope = self.envelope.update(abs(curvature - reference) * v2)
+
+    if predicted:
+      gate = 1.0 - _smoothstep(envelope, LAT_ACCEL_ON, LAT_ACCEL_OFF)
+      deviation = max(abs(p - reference * v2) for p in predicted)
+      on = LAT_ACCEL_ON + in_curve * (PREDICTED_CURVE_ON - LAT_ACCEL_ON)
+      off = LAT_ACCEL_OFF + in_curve * (PREDICTED_CURVE_OFF - LAT_ACCEL_OFF)
+      gate *= 1.0 - _smoothstep(self.prediction_envelope.update(deviation), on, off)
+    else:
+      if self.prediction_envelope.frame:
+        self.prediction_envelope = _Envelope()
+      on = LAT_ACCEL_ON + in_curve * (CURVE_DEVIATION_ON - LAT_ACCEL_ON)
+      off = LAT_ACCEL_OFF + in_curve * (CURVE_DEVIATION_OFF - LAT_ACCEL_OFF)
+      gate = 1.0 - _smoothstep(envelope, on, off)
+    gate *= 1.0 - _smoothstep(level, TIGHT_CURVE_ON, TIGHT_CURVE_OFF)
+
+    step = BYPASS_FADE_RATE * DT_CTRL
+    self.bypass_weight += min(max((0.0 if bypass else 1.0) - self.bypass_weight, -step), step)
+
+    gain = min(max(gain, MIN_GAIN), 1.0)
+    self.weight = self.bypass_weight * _smoothstep(v_ego, SPEED_OFF, SPEED_ON) * gate
+    k = 1.0 - self.weight * (1.0 - gain)
+    if k >= 1.0:
+      return curvature
+    return self.baseline + k * (curvature - self.baseline)

openpilot/selfdrive/controls/tests/test_highway_correction_gain.py

@@ -0,0 +1,202 @@
+import math
+
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels
+
+
+_V_HWY = 31.0  # ~70 mph
+
+
+def _run(hcg, curvature, gain=0.7, v_ego=_V_HWY, lat_active=True, bypass=None, predicted=None):
+  bypass = np.zeros(len(curvature), bool) if bypass is None else bypass
+  predicted = [None] * len(curvature) if predicted is None else [tuple(p) for p in predicted]
+  return np.array([hcg.update(float(c), v_ego, lat_active, gain, bool(b), predicted=p)
+                   for c, b, p in zip(curvature, bypass, predicted, strict=True)])
+
+
+def _lead(lat, seconds):
+  # prediction of the lateral accel profile `seconds` ahead, at both horizons
+  n = int(seconds / DT_CTRL)
+  ahead = np.concatenate([lat[n:], np.full(n, lat[-1])])
+  return np.c_[ahead, ahead]
+
+
+def _weave(seconds=30.0, freq=0.6, lat_accel_amp=0.08, v_ego=_V_HWY, bias=0.0):
+  t = np.arange(0.0, seconds, DT_CTRL)
+  return bias + (lat_accel_amp / v_ego ** 2) * np.sin(2 * math.pi * freq * t)
+
+
+def _fit_sine(x, freq):
+  t = np.arange(len(x)) * DT_CTRL
+  basis = np.c_[np.sin(2 * math.pi * freq * t), np.cos(2 * math.pi * freq * t), np.ones(len(x))]
+  (s, c, _), *_ = np.linalg.lstsq(basis, x, rcond=None)
+  return math.hypot(s, c), math.atan2(c, s)
+
+
+def test_gain_one_is_exact_pass_through():
+  raw = _weave(bias=0.1 / _V_HWY ** 2)
+  np.testing.assert_array_equal(_run(HighwayCorrectionGain(), raw, gain=1.0), raw)
+
+
+def test_weave_band_scaled_by_gain_with_little_lag():
+  for freq in (0.4, 0.6, 0.8):
+    raw = _weave(freq=freq)
+    out = _run(HighwayCorrectionGain(), raw, gain=0.7)
+    tail = slice(len(raw) // 2, None)  # after the bypass fade-in and baseline settle
+    amp_in, ph_in = _fit_sine(raw[tail], freq)
+    amp_out, ph_out = _fit_sine(out[tail], freq)
+    assert 0.68 < amp_out / amp_in < 0.76, freq
+    lag_deg = math.degrees((ph_in - ph_out + math.pi) % (2 * math.pi) - math.pi)
+    assert 0.0 <= lag_deg < 8.0, (freq, lag_deg)
+
+
+def test_steady_offset_passes_unchanged():
+  bias = 0.15 / _V_HWY ** 2
+  raw = np.full(int(30 / DT_CTRL), bias)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  np.testing.assert_allclose(out[-100:], bias, rtol=1e-6)
+
+
+def test_steady_curve_wobble_is_smoothed():
+  raw = _weave(seconds=40.0, bias=1.0 / _V_HWY ** 2)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  amp_out, _ = _fit_sine(out[tail], 0.6)
+  assert 0.45 < amp_out / amp_in < 0.62
+  assert abs(out[tail].mean() - raw[tail].mean()) * _V_HWY ** 2 < 0.01  # the curve itself is untouched
+
+
+def test_tight_curves_pass_through():
+  raw = _weave(seconds=40.0, bias=2.5 / _V_HWY ** 2)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  tail = slice(len(raw) // 2, None)
+  np.testing.assert_allclose(out[tail], raw[tail], rtol=0, atol=1e-12)
+
+
+def test_curve_entry_is_not_delayed_much():
+  # 5 s straight, then ramp to 1.2 m/s^2 over 2 s and hold
+  t = np.arange(0.0, 20.0, DT_CTRL)
+  lat = np.interp(t, [0, 5, 7, 20], [0, 0, 1.2, 1.2])
+  raw = lat / _V_HWY ** 2
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  shortfall = (raw - out) * _V_HWY ** 2
+  # the gate only fully releases at LAT_ACCEL_OFF, so curve entry is briefly softened
+  assert np.max(np.abs(shortfall)) < 0.15
+  assert np.argmax(out * _V_HWY ** 2 >= 0.6) == np.argmax(lat >= 0.6)
+  assert np.max(np.abs(shortfall[t > 12.0])) < 0.01
+
+
+def test_curve_exit_is_not_held():
+  # 20 s steady curve (smoothing active), then unwind to straight over 2 s
+  t = np.arange(0.0, 30.0, DT_CTRL)
+  lat = np.interp(t, [0, 2, 20, 22, 30], [1.2, 1.2, 1.2, 0, 0])
+  raw = lat / _V_HWY ** 2
+  hcg = HighwayCorrectionGain()
+  hcg.reset(raw[0])
+  out = _run(hcg, raw, gain=0.5)
+  excess = (out - raw)[t > 19.0] * _V_HWY ** 2
+  assert np.max(np.abs(excess)) < 0.15
+  i_raw = np.argmax(lat[t > 19.0] <= 0.6)
+  i_out = np.argmax(out[t > 19.0] * _V_HWY ** 2 <= 0.6)
+  assert (i_out - i_raw) * DT_CTRL < 0.15
+
+
+def test_below_speed_gate_passes_through():
+  v = 13.0  # below SPEED_OFF
+  raw = _weave(v_ego=v)
+  np.testing.assert_allclose(_run(HighwayCorrectionGain(), raw, gain=0.5, v_ego=v), raw, rtol=0, atol=1e-12)
+
+
+def test_bypass_fades_without_steps():
+  raw = _weave(seconds=30.0)
+  bypass = np.zeros(len(raw), bool)
+  bypass[int(15 / DT_CTRL):int(20 / DT_CTRL)] = True
+  hcg = HighwayCorrectionGain()
+  out = _run(hcg, raw, gain=0.5, bypass=bypass)
+  mid = slice(int(17 / DT_CTRL), int(20 / DT_CTRL))
+  np.testing.assert_allclose(out[mid], raw[mid], rtol=0, atol=1e-12)
+  max_raw_step = np.max(np.abs(np.diff(raw)))
+  assert np.max(np.abs(np.diff(out))) < 1.5 * max_raw_step
+
+
+def test_active_from_40_mph():
+  v = 40.0 * 0.44704
+  raw = _weave(v_ego=v)
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5, v_ego=v)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  amp_out, _ = _fit_sine(out[tail], 0.6)
+  assert 0.48 < amp_out / amp_in < 0.56
+
+
+def test_inactive_resets_and_passes_through():
+  hcg = HighwayCorrectionGain()
+  raw = _weave(seconds=10.0)
+  _run(hcg, raw, gain=0.5)
+  assert hcg.weight > 0.9
+  out = _run(hcg, raw[:50], gain=0.5, lat_active=False)
+  np.testing.assert_array_equal(out, raw[:50])
+  assert hcg.weight == 0.0 and hcg.bypass_weight == 0.0
+
+
+def test_gain_is_clamped():
+  raw = _weave()
+  low = _run(HighwayCorrectionGain(), raw, gain=0.0)
+  floor = _run(HighwayCorrectionGain(), raw, gain=0.1)
+  np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15)
+
+
+def test_predicted_lateral_accels():
+  t = 10.0 * (np.arange(33) / 32) ** 2
+  assert predicted_lateral_accels(t, np.full(33, 30.0), np.full(33, 0.02)) == (0.6, 0.6)
+  assert predicted_lateral_accels(t[:10], np.full(10, 30.0), np.full(10, 0.02)) is None  # horizon too short
+  assert predicted_lateral_accels(t, np.full(32, 30.0), np.full(33, 0.02)) is None
+
+
+def test_prediction_smooths_wobbly_curve_more():
+  # wobble big enough that the request-only gate backs off; the predicted path holds the curve level
+  raw = _weave(seconds=40.0, lat_accel_amp=0.2, bias=1.0 / _V_HWY ** 2)
+  pred = np.full((len(raw), 2), 1.0)
+  tail = slice(len(raw) // 2, None)
+  amp_in, _ = _fit_sine(raw[tail], 0.6)
+  without = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5)[tail], 0.6)[0] / amp_in
+  with_pred = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5, predicted=pred)[tail], 0.6)[0] / amp_in
+  assert 0.45 < with_pred < 0.62
+  assert with_pred < without - 0.1
+
+
+def test_prediction_releases_before_curve_exit():
+  t = np.arange(0.0, 30.0, DT_CTRL)
+  lat = np.interp(t, [0, 20, 22, 30], [1.2, 1.2, 0, 0])
+  raw = lat / _V_HWY ** 2
+  def excess(predicted):
+    hcg = HighwayCorrectionGain(); hcg.reset(raw[0])
+    out = _run(hcg, raw, gain=0.5, predicted=predicted)
+    return np.max(np.abs((out - raw)[t > 19.0])) * _V_HWY ** 2
+  with_pred = excess(_lead(lat, 1.5))
+  assert with_pred < 0.02
+  assert with_pred < excess(None)
+
+
+def test_prediction_releases_before_curve_entry():
+  t = np.arange(0.0, 20.0, DT_CTRL)
+  lat = np.interp(t, [0, 5, 7, 20], [0, 0, 1.2, 1.2])
+  raw = lat / _V_HWY ** 2
+  def shortfall(predicted):
+    out = _run(HighwayCorrectionGain(), raw, gain=0.5, predicted=predicted)
+    return np.max(np.abs(raw - out)) * _V_HWY ** 2
+  with_pred = shortfall(_lead(lat, 1.5))
+  assert with_pred < 0.02
+  assert with_pred < shortfall(None)
+
+
+def test_straights_smooth_the_same_with_prediction():
+  raw = _weave()
+  pred = np.zeros((len(raw), 2))
+  tail = slice(len(raw) // 2, None)
+  a = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7)[tail], 0.6)[0]
+  b = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7, predicted=pred)[tail], 0.6)[0]
+  assert math.isclose(a, b, rel_tol=0.02)

openpilot/selfdrive/locationd/test/test_torqued.py

@@ -1,6 +1,27 @@
+import numpy as np
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.hyundai.values import CAR as HYUNDAI
 from opendbc.car.structs import car
 from openpilot.common.test import OpenpilotTestCase
-from openpilot.selfdrive.locationd.torqued import TorqueEstimator
+from openpilot.selfdrive.locationd.torqued import (TorqueEstimator, LAT_ACC_THRESHOLD, FACTOR_SANITY,
+                                                   IONIQ_6_LAT_ACC_THRESHOLD, IONIQ_6_FACTOR_SANITY)
+
+
+def _fill_line(est, slope, seed=0):
+  rng = np.random.default_rng(seed)
+  for (low, high), min_pts in zip(est.filtered_points.buckets.keys(),
+                                  est.filtered_points.buckets_min_points.values(), strict=True):
+    for _ in range(int(min_pts)):
+      x = rng.uniform(low, high)
+      est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+  # a single bucket caps at POINTS_PER_BUCKET, below min_points_total
+  keys = list(est.filtered_points.buckets)
+  i = 0
+  while len(est.filtered_points) < est.min_points_total:
+    x = rng.uniform(*keys[i % len(keys)])
+    est.filtered_points.add_point(x, slope * x + rng.normal(0.0, 0.02))
+    i += 1
 
 
 class TestTorqued(OpenpilotTestCase):
@@ -25,3 +46,25 @@ class TestTorqued(OpenpilotTestCase):
 
     msg = est.get_msg()
     assert msg.lateralTorqueParameters.calPerc == 100
+
+  def test_ioniq_6_learner_limits(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    factor = cp.lateralTuning.torque.latAccelFactor
+    assert est.lat_acc_threshold == IONIQ_6_LAT_ACC_THRESHOLD
+    assert np.isclose(est.max_lataccel_factor, factor * (1 + IONIQ_6_FACTOR_SANITY))
+
+    other = TorqueEstimator(interfaces[HYUNDAI.HYUNDAI_IONIQ_5].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_5))
+    assert other.lat_acc_threshold == LAT_ACC_THRESHOLD
+    assert other.factor_sanity == FACTOR_SANITY
+
+  def test_ioniq_6_learned_slope_is_not_clamped(self):
+    cp = interfaces[HYUNDAI.HYUNDAI_IONIQ_6].get_non_essential_params(HYUNDAI.HYUNDAI_IONIQ_6)
+    est = TorqueEstimator(cp)
+    slope = cp.lateralTuning.torque.latAccelFactor * 1.45
+    _fill_line(est, slope)
+    captured = {}
+    est.update_params = lambda params: captured.update(params)
+    msg = est.get_msg()
+    assert msg.lateralTorqueParameters.valid
+    assert abs(captured["latAccelFactor"] - slope) < 0.1

openpilot/selfdrive/locationd/torqued.py

@@ -32,6 +32,11 @@ STEER_MIN_THRESHOLD = 0.02
 MIN_FILTER_DECAY = 50
 MAX_FILTER_DECAY = 250
 LAT_ACC_THRESHOLD = 1
+# Ioniq 6 makes ~4.5 m/s^2 per unit torque, so the 1 m/s^2 cap discards nearly every point in the
+# outer torque buckets (calPerc stuck at 50%) and truncating on lateral accel biases the slope low.
+IONIQ_6_LAT_ACC_THRESHOLD = 2.5
+# Its learned factor (~4.3) sits above the default +/-30% window around the 3.0 base.
+IONIQ_6_FACTOR_SANITY = 0.5
 STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
 MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
 MIN_ENGAGE_BUFFER = 2  # secs
@@ -73,6 +78,11 @@ class TorqueEstimator(ParameterEstimator):
       self.factor_sanity = FACTOR_SANITY
       self.friction_sanity = FRICTION_SANITY
 
+    self.lat_acc_threshold = LAT_ACC_THRESHOLD
+    if str(CP.carFingerprint) == 'HYUNDAI_IONIQ_6':
+      self.lat_acc_threshold = IONIQ_6_LAT_ACC_THRESHOLD
+      self.factor_sanity = max(self.factor_sanity, IONIQ_6_FACTOR_SANITY)
+
     self.offline_friction = 0.0
     self.offline_latAccelFactor = 0.0
     self.resets = 0.0
@@ -211,7 +221,7 @@ class TorqueEstimator(ParameterEstimator):
         steer = np.interp(t, self.raw_points['carOutput_t'], self.raw_points['steer_torque']).item()
         lateral_acc = (vego * yaw_rate) - (np.sin(roll) * ACCELERATION_DUE_TO_GRAVITY).item()
         if all(lat_active) and not any(steer_override) and (vego > MIN_VEL) and (abs(steer) > STEER_MIN_THRESHOLD):
-          if abs(lateral_acc) <= LAT_ACC_THRESHOLD:
+          if abs(lateral_acc) <= self.lat_acc_threshold:
             self.filtered_points.add_point(steer, lateral_acc)
 
           if self.track_all_points:

openpilot/starpilot/galaxy/settings.py

@@ -34,7 +34,7 @@ from openpilot.starpilot.ui.vasm_owner import ANNOTATION as VASM_ANNOTATION, RES
 
 
 PAGES = frozenset(("hub", "vehicle", "slc", "lane", "lane_change", "profiles", "traffic", "aggressive", "standard", "relaxed",
-                   "curve", "torque", "aol", "wheel", "appearance", "display", "data", "sounds", "pip", "sentry", "vasm",
+                   "curve", "torque", "highway_smoothing", "aol", "wheel", "appearance", "display", "data", "sounds", "pip", "sentry", "vasm",
                    "conditional", "conditional/cem", "conditional/ccm",
                    *(f"{name}/{category}" for name in ("traffic", "aggressive", "standard", "relaxed")
                      for category in ("acceleration", "braking", "following"))))
@@ -438,7 +438,7 @@ class SettingsGateway:
     with self.lock:
       ctx = self.context.sample()
       sections = []
-      for page in ("torque", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"):
+      for page in ("torque", "highway_smoothing", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"):
         state = self._state(page, ctx)
         sections.append({"title": state.title,
                          "rows": [{"label": row.label, "value": row.value} for row in state.rows[:64]]})

openpilot/starpilot/galaxy/web/js/settings.js

@@ -15,7 +15,7 @@ const post = (fetcher, path, body, signal) => fetcher(path, {
 })
 
 export const SETTINGS_SECTIONS = Object.freeze([
-  { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "lane"] },
+  { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "highway_smoothing", "lane"] },
   { id: "longitudinal", label: "Longitudinal (Speed & Following)", icon: "bi-speedometer2", pages: ["conditional", "curve", "profiles", "slc", "traffic", "aggressive", "standard", "relaxed"] },
   { id: "wheel", label: "Wheel Controls", icon: "bi-controller", pages: ["wheel"] },
   { id: "visual", label: "Visual (Display & UI)", icon: "bi-eye", pages: ["appearance", "ui_layout", "favorites", "pip"] },

openpilot/starpilot/ui/feature_settings_owner.py

@@ -70,7 +70,7 @@ from openpilot.starpilot.controllers.toyota_cruise import capability as toyota_c
 
 
 BOOL_DEFAULTS = {
-  "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True,
+  "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True, "HighwaySmoothing": False,
   "ReverseCruise": False, "ToyotaAutoHold": False, "VoltSNG": False, "GMAutoHold": False, "VoltOnePedalMode": False,
   "LongPitch": True, "DisableOpenpilotLongitudinal": False,
   "SpeedLimitController": False, "ShowSpeedLimits": True,
@@ -642,6 +642,64 @@ class FeatureSettingsOwner:
                           authorized=authorized, temp_prefix=".turn-assist-")
     return result.committed and result.verified
 
+  def _highway_smoothing_capability(self) -> tuple | None:
+    from openpilot.starpilot.lateral.controller_selection import policy_for
+    cp = self.vehicle_params()
+    if cp is None or policy_for(cp) != 'ioniq6':
+      return None
+    return (cp.carFingerprint, cp.lateralTuning.which(), str(cp.steerControlType), cp.passive, cp.dashcamOnly, cp.notCar,
+            tuple((str(config.safetyModel), int(config.safetyParam)) for config in cp.safetyConfigs))
+
+  def _highway_smoothing_rows(self, configurable: bool) -> list[FeatureRow]:
+    from openpilot.selfdrive.controls.lib.highway_correction_gain import DEFAULT_GAIN, ENABLE_KEY, GAIN_KEY, MIN_GAIN
+    capability = self._highway_smoothing_capability()
+    allowed = configurable and self.authority("torque") and capability is not None
+    toggle = self._bool_row(ENABLE_KEY, "Highway Smoothing", allowed)
+    rows = [replace(toggle, capability=capability, vehicle_fingerprint=self.vehicle_fingerprint(), dependencies=(),
+                    reason=toggle.reason if toggle.value not in ("On", "Off") else
+                           ("Calms highway weave on straights and steady curves above 35 mph. Applies within one second."
+                            if capability is not None else "Highway Smoothing is available on the Ioniq 6"))]
+    if toggle.value != "On":
+      return rows
+    value, raw, valid = self._value(GAIN_KEY)
+    try:
+      number = DEFAULT_GAIN if raw is None else float(value)
+      valid = valid and self._readable(GAIN_KEY) and math.isfinite(number) and MIN_GAIN <= number <= 1.0
+    except ValueError:
+      valid, number = False, DEFAULT_GAIN
+    rows.append(FeatureRow(GAIN_KEY, "Strength", f"{number:.2f}" if valid else "Invalid saved value", raw,
+                           step=0.05 if valid else 0., minimum=MIN_GAIN, maximum=1.0,
+                           available=allowed and (valid or self.authority("parked_preferences") and self._readable(GAIN_KEY)),
+                           repair_value=str(DEFAULT_GAIN) if not valid else "", default_value=str(DEFAULT_GAIN),
+                           vehicle_fingerprint=self.vehicle_fingerprint(), capability=capability,
+                           reason="Lower values follow the model's quick back-and-forth corrections less. 1.00 matches off."))
+    return rows
+
+  def _apply_highway_smoothing(self, request: FeatureSettingsRequest) -> bool:
+    from openpilot.selfdrive.controls.lib.highway_correction_gain import ENABLE_KEY, MIN_GAIN
+    def authorized():
+      row = next((row for row in self._highway_smoothing_rows(self.authority("preferences")) if row.key == request.key), None)
+      return (row is not None and row.available and (not row.repair_value or request.value == row.repair_value) and
+              row.capability == request.capability and row.vehicle_fingerprint == request.vehicle_fingerprint and
+              row.source == request.expected and request.related_source is None and not request.direction)
+    if not authorized():
+      return False
+    if request.key == ENABLE_KEY:
+      if request.value not in ("Off", "On"):
+        return False
+      raw = b"1" if request.value == "On" else b"0"
+    else:
+      try:
+        number = float(request.value)
+      except ValueError:
+        return False
+      if not math.isfinite(number) or not MIN_GAIN <= number <= 1.0:
+        return False
+      raw = str(number).encode()
+    result = commit_exact(self.params, key=request.key, max_bytes=128, raw=raw, expected=request.expected,
+                          authorized=authorized, temp_prefix=".highway-smoothing-")
+    return result.committed and result.verified
+
   def snapshot(self, page: str, *, parked: bool, system_long: bool, lateral_context: bool, metric: bool,
                configure_while_driving: bool = False) -> FeatureSettingsState:
     selected_vehicle = self._vehicle_params_source()
@@ -674,6 +732,10 @@ class FeatureSettingsOwner:
               FeatureRow("", "Steering and Torque", "Select the steering controller and tune its response", page=FeaturePage.TORQUE, available=True),
               FeatureRow("", "Always On Lateral", "Keep steering assistance active independently of cruise control", page=FeaturePage.AOL, available=True),
               FeatureRow("", "Wheel Controls", "Assign steering-wheel buttons and cruise behavior", page=FeaturePage.WHEEL, available=True)]
+      if self._highway_smoothing_capability() is not None:
+        rows.insert(rows.index(next(row for row in rows if row.page == FeaturePage.TORQUE)) + 1,
+                    FeatureRow("", "Highway Smoothing", "Calm highway weave on straights and steady curves",
+                               page=FeaturePage.HIGHWAY_SMOOTHING, available=True))
       if (self._long_pitch_capability() is not None or self._bolt_disable_capability() is not None or
           self._auto_hold_capability() is not None or self._pedal_setup_capability() is not None or
           self._gm_stop_capability("VoltSNG") is not None or
@@ -821,6 +883,9 @@ class FeatureSettingsOwner:
                             capability=capability, dependencies=self._dependents(DOCUMENT_KEY),
                             reason="Use the supplied vehicle tune and any values you customize below"))
       rows.extend(numeric_rows)
+    elif page == FeaturePage.HIGHWAY_SMOOTHING:
+      title = "Highway Smoothing"
+      rows.extend(self._highway_smoothing_rows(configurable))
     elif page == FeaturePage.AOL:
       title = "Always On Lateral"
       rows.extend(self._lateral_pause_rows(configurable))
@@ -1122,6 +1187,8 @@ class FeatureSettingsOwner:
       return self._apply_lateral_pause(request)
     if key == "TurnAssist":
       return self._apply_turn_assist(request)
+    if key in ("HighwaySmoothing", "HighwayCorrectionGain"):
+      return self._apply_highway_smoothing(request)
     if key == 'LateralControllerSelection':
       return self.controller.apply(request)
     if key == SETUP_ACTION:

openpilot/starpilot/ui/feature_settings_state.py

@@ -47,6 +47,7 @@ class FeaturePage(StrEnum):
   CURVE = "curve"
   TORQUE = "torque"
   AOL = "aol"
+  HIGHWAY_SMOOTHING = "highway_smoothing"
   WHEEL = "wheel"
   CONDITIONAL = "conditional"
   CONDITIONAL_CEM = "conditional/cem"