From 20ff7931fc8d10b969562ecf85e14aff75e5a3fe Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Tue, 29 Sep 2026 14:17:44 -0500 Subject: [PATCH] Highway Smoothing --- common/params_keys.h | 1 + selfdrive/controls/controlsd.py | 7 ++ .../controls/lib/highway_correction_gain.py | 66 ++++++++++ .../tests/test_highway_correction_gain.py | 113 ++++++++++++++++++ .../ui/layouts/settings/starpilot/lateral.py | 20 ++++ .../common/assets/device_settings_layout.json | 13 ++ starpilot/common/safe_mode.py | 1 + starpilot/common/starpilot_variables.py | 4 + 8 files changed, 225 insertions(+) create mode 100644 selfdrive/controls/lib/highway_correction_gain.py create mode 100644 selfdrive/controls/tests/test_highway_correction_gain.py diff --git a/common/params_keys.h b/common/params_keys.h index 2719e0436a..7f6f5a558f 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -431,6 +431,7 @@ inline static std::unordered_map 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}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 320eea60a9..c7d26bcbe8 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_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 diff --git a/selfdrive/controls/lib/highway_correction_gain.py b/selfdrive/controls/lib/highway_correction_gain.py new file mode 100644 index 0000000000..eb9aa4fd33 --- /dev/null +++ b/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) diff --git a/selfdrive/controls/tests/test_highway_correction_gain.py b/selfdrive/controls/tests/test_highway_correction_gain.py new file mode 100644 index 0000000000..0d42057c06 --- /dev/null +++ b/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) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index 03032e41e7..dd3a81bbf8 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/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."), diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 8d67a60e4b..0bb685f6b4 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/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", diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 18429831fb..cb41ff4620 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -50,6 +50,7 @@ SAFE_MODE_MANAGED_KEYS = ( "SteerKP", "SteerLatAccel", "SteerRatio", + "HighwayCorrectionGain", "CameraOffset", "LaneCentering", "LaneCenteringPauseOnSignal", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 23d3cd3334..18889c9da3 100644 --- a/starpilot/common/starpilot_variables.py +++ b/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)