diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0fdf7c97cb..97f4005d37 100644 --- a/selfdrive/controls/controlsd.py +++ b/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, diff --git a/selfdrive/controls/lib/highway_curvature_smoother.py b/selfdrive/controls/lib/highway_curvature_smoother.py new file mode 100644 index 0000000000..7e7c2a3820 --- /dev/null +++ b/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) diff --git a/selfdrive/controls/tests/test_highway_curvature_smoother.py b/selfdrive/controls/tests/test_highway_curvature_smoother.py new file mode 100644 index 0000000000..e66ec7cfea --- /dev/null +++ b/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