highway smoothing

common/params_keys.h

@@ -432,6 +432,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"HideSpeedLimit", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"HideSteeringWheel", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"HigherBitrate", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
+    {"HighwayCorrectionGain", {PERSISTENT, FLOAT, "1.0", "1.0", 3}},
     {"HolidayThemes", {PERSISTENT, BOOL, "1", "0", 0, SETTINGS_SIMPLE}},
     {"HumanLaneChanges", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"IconPack", {PERSISTENT, STRING, "stock", "stock", 0}},
@@ -577,7 +578,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"PauseAOLOnBrake", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"PauseLateralSpeed", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
-    {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
+    {"LateralResumeDelay",{PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}},
     {"PedalsOnUI", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"PIPPreviewEnabled", {PERSISTENT, BOOL, "0", "0", 1}},
     {"PIPPreviewMask", {PERSISTENT, JSON, "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", 2}},

selfdrive/controls/controlsd.py

@@ -25,6 +25,7 @@ from openpilot.selfdrive.controls.lib.drive_helpers import (
   get_lateral_active,
   update_lateral_fault_latch,
 )
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain
 from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringController
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
@@ -399,6 +400,7 @@ class Controls:
     self.desired_curvature = 0.0
     self.lc_smooth_release = 0.0
     self.lane_centering = LaneCenteringController()
+    self.highway_correction_gain = HighwayCorrectionGain()
     self.lc_entry_sign = 0.0
     self.lc_arrest_jerk_factor = 1.0
     self.turn_hold_curvature = 0.0
@@ -747,6 +749,11 @@ class Controls:
       bool(CS.leftBlinker or CS.rightBlinker),
       bool(CS.steeringPressed))
 
+    new_desired_curvature = self.highway_correction_gain.update(
+      new_desired_curvature, CS.vEgo, CC.latActive, getattr(self.starpilot_toggles, "highway_correction_gain", 1.0),
+      bypass=bool(CS.leftBlinker or CS.rightBlinker or CS.steeringPressed or self.turn_hold_curvature != 0.0 or
+                  model_v2.meta.laneChangeState != LaneChangeState.off or self.sm.valid['lateralManeuverPlan']))
+
     jerk_factor = 1.0
     if self.starpilot_toggles.lane_change_pace < 10:
       set_jerk = self.starpilot_toggles.lane_change_jerk_factor

selfdrive/controls/lib/highway_correction_gain.py

@@ -0,0 +1,81 @@
+from collections import deque
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+
+# Straight-highway gain on the FAST part of the desired curvature (user setting "HighwayCorrectionGain").
+#
+# The Ioniq 6 highway weave is a lightly damped loop through the driving model, not a controller or EPS
+# problem (drives 00000b2c..00000b52):
+#   - openpilot delivers 0.97x of the model's requested lateral accel, ~0.4 s late; Kp 0.4-1.4 and the
+#     EPS damping byte changed nothing.
+#   - with Hyundai LFA steering (model open loop) the model's request wobbles half as much and follows the
+#     car's own motion (gain ~0.94 at 0.4-0.8 Hz): road kicks -> car sways -> model asks to follow the
+#     sway -> openpilot delivers it. LFA delivers only ~1/1.36 of what the model asks and its weave is
+#     flat vs road roughness (0.09) while openpilot's grows with it (0.18 -> 0.25).
+#
+# So this lowers the loop gain at the weave frequency instead of filtering: the output is
+#   baseline + k * (curvature - baseline),  k = 1 - weight * (1 - gain)
+# with baseline a slow (1.5 s) low-pass. At 0.35-0.8 Hz the baseline has little content, so the result is
+# ~gain with only ~5 deg of phase lag, and DC / slow lane keeping passes unchanged. The earlier tau 0.3 s
+# low-pass smoother (drive 00000b27) did nothing because it added lag to the same loop it meant to damp.
+#
+# Gating copies that smoother: only above ~45 mph, only on near-straight road (sliding-max envelope of the
+# raw commanded lateral accel so the weave cannot modulate its own gain), faded out for blinkers,
+# overrides, turn holds, lane changes and maneuver plans. gain = 1.0 is an exact pass-through.
+BASELINE_TAU = 1.5                        # s
+SPEED_OFF = 40.0 * CV.MPH_TO_MS
+SPEED_ON = 50.0 * CV.MPH_TO_MS
+LAT_ACCEL_ON = 0.25                       # m/s^2, envelope below this: full effect
+LAT_ACCEL_OFF = 0.6                       # m/s^2, envelope above this: no effect
+ENVELOPE_HOLD = 2.0                       # s, sliding-max window (outlasts 0.35 Hz weave peak spacing)
+ENVELOPE_RELEASE_RATE = 0.3               # m/s^2 per second, after the hold
+BYPASS_FADE_RATE = 2.0                    # per second: blinker/override fade in/out over 0.5 s
+MIN_GAIN = 0.4
+
+
+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)
+
+
+class HighwayCorrectionGain:
+  def __init__(self):
+    self.alpha = DT_CTRL / (BASELINE_TAU + DT_CTRL)
+    self.reset()
+
+  def reset(self, curvature: float = 0.0) -> None:
+    self.baseline = curvature
+    self.envelope = 0.0
+    self.frame = 0
+    self.peaks: deque[tuple[int, float]] = deque()  # monotonic (frame, lat accel) for the sliding max
+    self.bypass_weight = 0.0
+    self.weight = 0.0
+
+  def update(self, curvature: float, v_ego: float, lat_active: bool, gain: float, bypass: bool) -> float:
+    if not lat_active:
+      self.reset(curvature)
+      return curvature
+
+    # The baseline always tracks the raw command, so fading in or changing the gain never steps the output.
+    self.baseline += self.alpha * (curvature - self.baseline)
+
+    lat_accel = abs(curvature) * v_ego ** 2
+    self.frame += 1
+    while self.peaks and self.peaks[-1][1] <= lat_accel:
+      self.peaks.pop()
+    self.peaks.append((self.frame, lat_accel))
+    while self.peaks[0][0] <= self.frame - ENVELOPE_HOLD / DT_CTRL:
+      self.peaks.popleft()
+    self.envelope = max(self.peaks[0][1], self.envelope - ENVELOPE_RELEASE_RATE * DT_CTRL)
+
+    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) *
+                   (1.0 - _smoothstep(self.envelope, LAT_ACCEL_ON, LAT_ACCEL_OFF)))
+    k = 1.0 - self.weight * (1.0 - gain)
+    if k >= 1.0:
+      return curvature
+    return self.baseline + k * (curvature - self.baseline)

selfdrive/controls/tests/test_highway_correction_gain.py

@@ -0,0 +1,108 @@
+import math
+
+import numpy as np
+
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain
+
+
+_V_HWY = 31.0  # ~70 mph
+
+
+def _run(hcg, curvature, gain=0.7, v_ego=_V_HWY, lat_active=True, bypass=None):
+  bypass = np.zeros(len(curvature), bool) if bypass is None else bypass
+  return np.array([hcg.update(float(c), v_ego, lat_active, gain, bool(b)) for c, b in zip(curvature, bypass, strict=True)])
+
+
+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)  # a gain, not a low-pass: the 0.3 s smoother lagged ~50 deg
+
+
+def test_steady_offset_passes_unchanged():
+  bias = 0.15 / _V_HWY ** 2  # slight lane-keeping bias / crown, below the curve gate
+  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)  # baseline settled (e^-20 after 30 s)
+
+
+def test_curves_pass_through():
+  raw = _weave(bias=1.0 / _V_HWY ** 2)  # 1 m/s^2 highway curve with the same wobble on top
+  out = _run(HighwayCorrectionGain(), raw, gain=0.5)
+  np.testing.assert_allclose(out, raw, 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, 12.0, DT_CTRL)
+  lat = np.interp(t, [0, 5, 7, 12], [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 releases between 0.25 and 0.6 m/s^2, so the first ~0.3 m/s^2 of a curve is briefly softened
+  # (~0.13 m/s^2 at gain 0.5, <2 cm of path at 70 mph) ...
+  assert np.max(np.abs(shortfall)) < 0.15  # m/s^2
+  # ... but 0.6 m/s^2 is reached with no delay, and the curve itself is untouched
+  assert np.argmax(out * _V_HWY ** 2 >= 0.6) == np.argmax(lat >= 0.6)
+  np.testing.assert_allclose(out[t > 8.0], raw[t > 8.0], rtol=0, atol=1e-12)
+
+
+def test_below_speed_gate_passes_through():
+  v = 15.0  # ~34 mph
+  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  # blinker / override for 5 s
+  hcg = HighwayCorrectionGain()
+  out = _run(hcg, raw, gain=0.5, bypass=bypass)
+  # fully bypassed well inside the window
+  mid = slice(int(17 / DT_CTRL), int(20 / DT_CTRL))
+  np.testing.assert_allclose(out[mid], raw[mid], rtol=0, atol=1e-12)
+  # no output jumps bigger than the raw signal's own per-frame change allows
+  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_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.4)
+  np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15)

selfdrive/ui/layouts/settings/starpilot/lateral.py

@@ -103,6 +103,19 @@ class StarPilotLateralLayout(_SettingsPage):
     def aol_on():
       return p.get_bool("AlwaysOnLateral")
 
+    hcg_known = []
+
+    def hcg_available():
+      # HighwayCorrectionGain is a new params_keys.h entry: until scons rebuilds params_pyx, touching it raises
+      # UnknownKeyName, so keep the row hidden instead of crashing the UI on a stale build.
+      if not hcg_known:
+        try:
+          p.get_float("HighwayCorrectionGain")
+          hcg_known.append(True)
+        except Exception:
+          hcg_known.append(False)
+      return hcg_known[0]
+
     def lc_on():
       return p.get_bool("LaneChanges")
 
@@ -305,6 +318,14 @@ class StarPilotLateralLayout(_SettingsPage):
         on_click=lambda: self._show_slider("SteerKP", max(0.01, cs.steerKp) * 0.5, max(0.01, cs.steerKp) * 3.0, step=0.01, value_type="float"),
         visible=lambda: alt_on() and cs.steerKp != 0 and cs.isTorqueCar and not cs.isAngleCar,
       ),
+      SettingRow(
+        "HighwayCorrectionGain", "value", tr_noop("Highway Correction Gain"),
+        subtitle=tr_noop("Straight highway only (45+ mph). 1.00 = stock. Lower follows the model's quick back-and-forth corrections less, to calm weave."),
+        get_value=lambda: f"{p.get_float('HighwayCorrectionGain'):.2f}",
+        on_click=lambda: self._show_slider("HighwayCorrectionGain", 0.4, 1.0, step=0.05, value_type="float",
+                                           title="Highway Correction Gain"),
+        visible=lambda: alt_on() and hcg_available(),
+      ),
       SettingRow(
         "SteerLatAccel", "value", tr_noop("Lateral Acceleration"),
         subtitle=tr_noop("Maps steering torque to turning response."),

starpilot/common/starpilot_variables.py

@@ -792,6 +792,10 @@ class StarPilotVariables:
     toggle.use_custom_latAccelFactor = bool(round(toggle.latAccelFactor, 2) != round(latAccelFactor, 2)) and is_torque_car and not toggle.force_auto_tune or toggle.force_auto_tune_off
     toggle.steerRatio = self.get_value("SteerRatio", cast=float, condition=advanced_lateral_tuning, default=steerRatio, min=steerRatio * 0.5, max=steerRatio * 1.5)
     toggle.use_custom_steerRatio = bool(round(toggle.steerRatio, 2) != round(steerRatio, 2)) and not toggle.force_auto_tune or toggle.force_auto_tune_off
+    try:
+      toggle.highway_correction_gain = self.get_value("HighwayCorrectionGain", cast=float, condition=advanced_lateral_tuning, default=1.0, min=0.4, max=1.0)
+    except Exception:  # new params_keys.h entry not compiled yet (scons pending): behave as stock
+      toggle.highway_correction_gain = 1.0
     honda_pid_lateral = toggle.car_make == "honda" and CP.lateralTuning.which() == "pid" and not is_angle_car
     toggle.honda_lateral_pid_kp_scale = self.get_value("HondaLateralPidKpScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0)
     toggle.honda_lateral_pid_ki_scale = self.get_value("HondaLateralPidKiScale", cast=float, condition=honda_pid_lateral, default=1.0, min=0.1, max=4.0)