Highway Smoothing

common/params_keys.h

@@ -431,6 +431,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}},

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
@@ -400,6 +401,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
@@ -760,6 +762,11 @@ class Controls:
       new_desired_curvature = self.gv70_highway_stabilizer.update(
         new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL)
 
+    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,66 @@
+from collections import deque
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+
+# 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). Below MIN_GAIN the output is mostly the slow baseline,
+# i.e. a low-pass again.
+BASELINE_TAU = 1.5                        # s
+SPEED_OFF = 30.0 * CV.MPH_TO_MS
+SPEED_ON = 40.0 * CV.MPH_TO_MS
+LAT_ACCEL_ON = 0.25                       # m/s^2
+LAT_ACCEL_OFF = 0.6                       # 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.3
+
+
+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()
+    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
+
+    # tracked even when bypassed so fading in 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,113 @@
+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)
+
+
+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_curves_pass_through():
+  raw = _weave(bias=1.0 / _V_HWY ** 2)
+  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():
+  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 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)
+  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 = 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.3)
+  np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15)

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

@@ -103,6 +103,18 @@ class StarPilotLateralLayout(_SettingsPage):
     def aol_on():
       return p.get_bool("AlwaysOnLateral")
 
+    hcg_known = []
+
+    def hcg_available():
+      # raises UnknownKeyName until scons rebuilds params_pyx with the new key
+      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")
 
@@ -312,6 +324,14 @@ class StarPilotLateralLayout(_SettingsPage):
         on_click=lambda: self._show_slider("SteerKP", max(0.01, cs.steerKp) * 0.5, max(0.01, cs.steerKp) * 1.5, 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 Smoothing"),
+        subtitle=tr_noop("Straight roads only (35+ mph). 1.00 = off. 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.3, 1.0, step=0.05, value_type="float",
+                                           title="Highway Smoothing"),
+        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/assets/device_settings_layout.json

@@ -76,6 +76,19 @@
         "parent_key": "AdvancedLateralTune",
         "settings_tier": "advanced"
       },
+      {
+        "key": "HighwayCorrectionGain",
+        "label": "Highway Smoothing",
+        "description": "Straight roads only (35+ mph). 1.00 = off. Lower follows the model's quick back-and-forth corrections less, to calm weave.",
+        "data_type": "float",
+        "ui_type": "numeric",
+        "min": 0.3,
+        "max": 1.0,
+        "step": 0.05,
+        "precision": 2,
+        "parent_key": "AdvancedLateralTune",
+        "settings_tier": "advanced"
+      },
       {
         "key": "SteerLatAccel",
         "label": "Lateral Acceleration",

starpilot/common/safe_mode.py

@@ -50,6 +50,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "SteerKP",
   "SteerLatAccel",
   "SteerRatio",
+  "HighwayCorrectionGain",
   "CameraOffset",
   "LaneCentering",
   "LaneCenteringPauseOnSignal",

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.3, max=1.0)
+    except Exception:  # UnknownKeyName until scons rebuilds params_pyx with the new key
+      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)