numb

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_curvature_smoother import HighwayCurvatureSmoother
 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_curvature_smoother = HighwayCurvatureSmoother(self.CP)
     self.lc_entry_sign = 0.0
     self.lc_arrest_jerk_factor = 1.0
     self.turn_hold_curvature = 0.0
@@ -562,6 +564,7 @@ class Controls:
     if not CC.latActive:
       self.LaC.reset()
       self.lane_centering.reset()
+      self.highway_curvature_smoother.reset()
     tesla_pedal_override = self.CP.brand == "tesla" and bool(CS.gasPressed)
     if not CC.longActive and not tesla_pedal_override:
       self.LoC.reset()
@@ -736,6 +739,11 @@ class Controls:
             held_mag = min(lead_curvature * blinker_dir, abs(self.turn_hold_curvature) + CURVATURE_HOLD_RATCHET_RATE * DT_CTRL)
             self.turn_hold_curvature = math.copysign(held_mag, lead_curvature)
 
+    new_desired_curvature = self.highway_curvature_smoother.update(
+      new_desired_curvature, CS.vEgo, CC.latActive,
+      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']))
+
     new_desired_curvature = self.lane_centering.update(
       new_desired_curvature, model_v2, CS.vEgo,
       self.starpilot_toggles.lane_centering,

selfdrive/controls/lib/highway_curvature_smoother.py

@@ -0,0 +1,79 @@
+from collections import deque
+
+from openpilot.common.constants import CV
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import IONIQ_6_CARS
+
+# Near-straight highway smoothing of the model's desired curvature.
+#
+# Once the Ioniq 6 torque tune followed the model ~1:1 (SteerLatAccel 4.0), the remaining
+# highway weave was the model's own action.desiredCurvature wobbling at 0.35-1.4 Hz and
+# controlsd passing it straight through (corr +0.84..+1.00, drive 00000b20). OEM LFA on the
+# same car showed ~half that weave on straights (drive 00000b23), largely by not chasing it.
+#
+# A first-order low-pass (tau 0.3 s) removes ~1/3 of that band on straights while keeping
+# ~94% of the slow 0.1-0.35 Hz lane-keeping content (open-loop replay of af3/b20/b0a/aee).
+#
+# Gating is on an envelope of the raw commanded lateral accel -- the max over the last
+# ENVELOPE_HOLD seconds, with a rate-limited release -- not on the raw value: the smoother
+# releases the instant a curve begins (0 ms added onset delay across 51 highway curve onsets,
+# zero deviation once |lat accel| > 0.6), but the weave itself cannot modulate the weight at
+# its own frequency -- the gain-scheduled-on-the-oscillating-signal mistake behind the 0.67 Hz
+# taper limit cycle (drive 00000ade). The window must outlast the spacing between |weave|
+# peaks (0.35 Hz -> 1.4 s); a plain linear release sagged between peaks and still swung the
+# weight ~5% at the weave frequency.
+ENABLED = True                            # A/B switch; restart-only, no rebuild
+SMOOTH_TAU = 0.3                          # 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 smoothing
+LAT_ACCEL_OFF = 0.6                       # m/s^2, envelope above this: no smoothing
+ENVELOPE_HOLD = 2.0                       # s, sliding-max window
+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
+
+
+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 HighwayCurvatureSmoother:
+  def __init__(self, CP):
+    self.enabled = ENABLED and CP.carFingerprint in IONIQ_6_CARS
+    self.alpha = DT_CTRL / (SMOOTH_TAU + DT_CTRL)
+    self.reset()
+
+  def reset(self, curvature: float = 0.0) -> None:
+    self.filtered = 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, bypass: bool) -> float:
+    if not self.enabled or not lat_active:
+      self.reset(curvature)
+      return curvature
+
+    # The filter always tracks the raw command, so fading back in never steps the output.
+    self.filtered += self.alpha * (curvature - self.filtered)
+
+    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)
+
+    # Only the bypass part is rate limited: the envelope part is already continuous, and
+    # limiting it would keep smoothing into the start of a curve.
+    step = BYPASS_FADE_RATE * DT_CTRL
+    self.bypass_weight += min(max((0.0 if bypass else 1.0) - self.bypass_weight, -step), step)
+
+    self.weight = (self.bypass_weight * _smoothstep(v_ego, SPEED_OFF, SPEED_ON) *
+                   (1.0 - _smoothstep(self.envelope, LAT_ACCEL_ON, LAT_ACCEL_OFF)))
+    return curvature + self.weight * (self.filtered - curvature)

selfdrive/controls/tests/test_highway_curvature_smoother.py

@@ -0,0 +1,85 @@
+import math
+from types import SimpleNamespace
+
+import numpy as np
+
+from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.lib.highway_curvature_smoother import HighwayCurvatureSmoother
+
+
+_IONIQ_6 = SimpleNamespace(carFingerprint=HYUNDAI_CAR.HYUNDAI_IONIQ_6)
+_OTHER = SimpleNamespace(carFingerprint=HYUNDAI_CAR.HYUNDAI_IONIQ_5)
+_V_HWY = 31.0  # ~70 mph
+
+
+def _run(smoother, curvature, v_ego=_V_HWY, lat_active=True, bypass=False):
+  return np.array([smoother.update(float(c), v_ego, lat_active, bypass) for c in curvature])
+
+
+def _weave(seconds=20.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 test_other_cars_pass_through():
+  raw = _weave()
+  out = _run(HighwayCurvatureSmoother(_OTHER), raw)
+  np.testing.assert_array_equal(out, raw)
+
+
+def test_straight_weave_is_attenuated():
+  raw = _weave()
+  out = _run(HighwayCurvatureSmoother(_IONIQ_6), raw)
+  tail = slice(len(raw) // 2, None)  # after the weight has faded in
+  assert np.std(out[tail]) < 0.8 * np.std(raw[tail])
+
+
+def test_below_speed_gate_passes_through():
+  v = 15.0  # ~34 mph
+  raw = _weave(v_ego=v)
+  out = _run(HighwayCurvatureSmoother(_IONIQ_6), raw, v_ego=v)
+  np.testing.assert_allclose(out, raw, atol=1e-12)
+
+
+def test_real_curve_is_not_delayed():
+  # straight, then a highway curve ramping to 1.5 m/s^2 over 2 s
+  t = np.arange(0.0, 20.0, DT_CTRL)
+  lat = np.clip((t - 10.0) / 2.0, 0.0, 1.0) * 1.5
+  raw = lat / _V_HWY ** 2
+  out = _run(HighwayCurvatureSmoother(_IONIQ_6), raw)
+  in_curve = lat > 0.6
+  assert np.max(np.abs(out[in_curve] - raw[in_curve])) * _V_HWY ** 2 < 1e-3
+  # reaches 0.6 m/s^2 no later than the raw command
+  assert np.argmax(out * _V_HWY ** 2 >= 0.6) == np.argmax(in_curve)
+
+
+def test_weight_does_not_modulate_at_weave_frequency():
+  # weave peaks crossing the ON threshold must not make the weight oscillate
+  smoother = HighwayCurvatureSmoother(_IONIQ_6)
+  weights = []
+  for c in _weave(lat_accel_amp=0.3):
+    smoother.update(float(c), _V_HWY, True, False)
+    weights.append(smoother.weight)
+  tail = np.array(weights[len(weights) // 2:])
+  assert np.ptp(tail) < 0.02
+
+
+def test_bypass_and_reengage_have_no_step():
+  raw = _weave(seconds=30.0)
+  smoother = HighwayCurvatureSmoother(_IONIQ_6)
+  out = []
+  for i, c in enumerate(raw):
+    bypass = 1000 <= i < 1500  # e.g. blinker held for 5 s
+    out.append(smoother.update(float(c), _V_HWY, True, bypass))
+  out = np.array(out)
+  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_lat_inactive_resets_to_input():
+  smoother = HighwayCurvatureSmoother(_IONIQ_6)
+  _run(smoother, _weave(seconds=5.0))
+  assert smoother.update(0.002, _V_HWY, False, False) == 0.002
+  assert smoother.weight == 0.0
+  assert smoother.filtered == 0.002