From 48c4abb89830d6776162459f270ca5e435f7b2e5 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 28 Sep 2026 19:16:39 -0500 Subject: [PATCH] highway smoothing 2 --- selfdrive/controls/lib/highway_correction_gain.py | 13 +++++++++---- .../controls/tests/test_highway_correction_gain.py | 14 ++++++++++++-- selfdrive/ui/layouts/settings/starpilot/lateral.py | 4 ++-- starpilot/common/starpilot_variables.py | 2 +- 4 files changed, 24 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/highway_correction_gain.py b/selfdrive/controls/lib/highway_correction_gain.py index bd4d123d93..38911c23d8 100644 --- a/selfdrive/controls/lib/highway_correction_gain.py +++ b/selfdrive/controls/lib/highway_correction_gain.py @@ -20,18 +20,23 @@ from openpilot.common.realtime import DT_CTRL # ~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 +# Gating copies that smoother: only above ~35 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. +# +# Road test 00000b55: gain 0.4 cut straight-road weave to 0.67x at matched speed/roughness (OEM level above +# 60 mph); 0.7 did nothing. The fade-in moved from 40-50 to 30-40 mph because 45-50 mph was only partly +# covered. MIN_GAIN stops at 0.3: lower, the output is mostly the 1.5 s baseline, i.e. a low-pass again +# (35 deg lag at 0.5 Hz for 0.2, 53 deg for 0.1), the failure mode of the old smoother. BASELINE_TAU = 1.5 # s -SPEED_OFF = 40.0 * CV.MPH_TO_MS -SPEED_ON = 50.0 * CV.MPH_TO_MS +SPEED_OFF = 30.0 * CV.MPH_TO_MS +SPEED_ON = 40.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 +MIN_GAIN = 0.3 def _smoothstep(x: float, lo: float, hi: float) -> float: diff --git a/selfdrive/controls/tests/test_highway_correction_gain.py b/selfdrive/controls/tests/test_highway_correction_gain.py index bdb4bd60a6..90666720a7 100644 --- a/selfdrive/controls/tests/test_highway_correction_gain.py +++ b/selfdrive/controls/tests/test_highway_correction_gain.py @@ -72,7 +72,7 @@ def test_curve_entry_is_not_delayed_much(): def test_below_speed_gate_passes_through(): - v = 15.0 # ~34 mph + v = 13.0 # ~29 mph, below the 30-40 mph fade-in raw = _weave(v_ego=v) np.testing.assert_allclose(_run(HighwayCorrectionGain(), raw, gain=0.5, v_ego=v), raw, rtol=0, atol=1e-12) @@ -91,6 +91,16 @@ def test_bypass_fades_without_steps(): 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) @@ -104,5 +114,5 @@ def test_inactive_resets_and_passes_through(): def test_gain_is_clamped(): raw = _weave() low = _run(HighwayCorrectionGain(), raw, gain=0.0) - floor = _run(HighwayCorrectionGain(), raw, gain=0.4) + floor = _run(HighwayCorrectionGain(), raw, gain=0.3) np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 6b5dcd8171..265c406e07 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -320,9 +320,9 @@ class StarPilotLateralLayout(_SettingsPage): ), 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."), + subtitle=tr_noop("Straight roads only (35+ 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", + on_click=lambda: self._show_slider("HighwayCorrectionGain", 0.3, 1.0, step=0.05, value_type="float", title="Highway Correction Gain"), visible=lambda: alt_on() and hcg_available(), ), diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index be8237bd32..b3e3591beb 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -793,7 +793,7 @@ class StarPilotVariables: 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) + 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: # 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