From 40183ef52e1bf8c272a6a528ab094af88d5930b4 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Sat, 3 Oct 2026 08:46:20 -0500 Subject: [PATCH] add smooth curves --- .../controls/lib/highway_correction_gain.py | 59 ++++++++++++++----- .../tests/test_highway_correction_gain.py | 41 ++++++++++--- .../ui/layouts/settings/starpilot/lateral.py | 4 +- .../common/assets/device_settings_layout.json | 4 +- starpilot/common/starpilot_variables.py | 2 +- 5 files changed, 82 insertions(+), 28 deletions(-) diff --git a/selfdrive/controls/lib/highway_correction_gain.py b/selfdrive/controls/lib/highway_correction_gain.py index eb9aa4fd33..650b4d607a 100644 --- a/selfdrive/controls/lib/highway_correction_gain.py +++ b/selfdrive/controls/lib/highway_correction_gain.py @@ -5,18 +5,29 @@ 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. +# adding lag (a low-pass adds lag and did not help). Near MIN_GAIN the output is mostly the slow baseline, +# i.e. a low-pass again (~35 deg lag at 0.5 Hz for 0.2, ~53 deg for 0.1). BASELINE_TAU = 1.5 # s SPEED_OFF = 30.0 * CV.MPH_TO_MS SPEED_ON = 40.0 * CV.MPH_TO_MS +# Inside a curve the gate measures distance from the curve's own level (a slow average of the request) +# instead of from zero, so a steady curve is smoothed like a straight. Until the level builds up it is the +# original zero-referenced gate, which keeps curve entries as before; in the curve the thresholds tighten so +# the request leaving the level (exit, tightening) switches smoothing off before the slow baseline can hold +# the car in the curve. +CURVE_LEVEL_TAU = 3.0 # s LAT_ACCEL_ON = 0.25 # m/s^2 LAT_ACCEL_OFF = 0.6 # m/s^2 +CURVE_DEVIATION_ON = 0.12 # m/s^2 +CURVE_DEVIATION_OFF = 0.30 # m/s^2 +CURVE_LEVEL_BLEND = (0.15, 0.35) # m/s^2 of curve level: straight thresholds -> curve thresholds +TIGHT_CURVE_ON = 1.5 # m/s^2 +TIGHT_CURVE_OFF = 2.0 # 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 +MIN_GAIN = 0.1 def _smoothstep(x: float, lo: float, hi: float) -> float: @@ -24,16 +35,33 @@ def _smoothstep(x: float, lo: float, hi: float) -> float: return t * t * (3.0 - 2.0 * t) +class _Envelope: + def __init__(self): + self.frame = 0 + self.peaks: deque[tuple[int, float]] = deque() + self.value = 0.0 + + def update(self, x: float) -> float: + self.frame += 1 + while self.peaks and self.peaks[-1][1] <= x: + self.peaks.pop() + self.peaks.append((self.frame, x)) + while self.peaks[0][0] <= self.frame - ENVELOPE_HOLD / DT_CTRL: + self.peaks.popleft() + self.value = max(self.peaks[0][1], self.value - ENVELOPE_RELEASE_RATE * DT_CTRL) + return self.value + + class HighwayCorrectionGain: def __init__(self): self.alpha = DT_CTRL / (BASELINE_TAU + DT_CTRL) + self.alpha_curve = DT_CTRL / (CURVE_LEVEL_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.curve_level = curvature + self.envelope = _Envelope() self.bypass_weight = 0.0 self.weight = 0.0 @@ -44,22 +72,21 @@ class HighwayCorrectionGain: # tracked even when bypassed so fading in never steps the output self.baseline += self.alpha * (curvature - self.baseline) + self.curve_level += self.alpha_curve * (curvature - self.curve_level) - 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) + v2 = v_ego ** 2 + level = abs(self.curve_level) * v2 + in_curve = _smoothstep(level, *CURVE_LEVEL_BLEND) + on = LAT_ACCEL_ON + in_curve * (CURVE_DEVIATION_ON - LAT_ACCEL_ON) + off = LAT_ACCEL_OFF + in_curve * (CURVE_DEVIATION_OFF - LAT_ACCEL_OFF) + envelope = self.envelope.update(abs(curvature - in_curve * self.curve_level) * v2) + gate = (1.0 - _smoothstep(envelope, on, off)) * (1.0 - _smoothstep(level, TIGHT_CURVE_ON, TIGHT_CURVE_OFF)) 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))) + self.weight = self.bypass_weight * _smoothstep(v_ego, SPEED_OFF, SPEED_ON) * gate k = 1.0 - self.weight * (1.0 - gain) if k >= 1.0: return curvature diff --git a/selfdrive/controls/tests/test_highway_correction_gain.py b/selfdrive/controls/tests/test_highway_correction_gain.py index 0d42057c06..2cdddcf2f2 100644 --- a/selfdrive/controls/tests/test_highway_correction_gain.py +++ b/selfdrive/controls/tests/test_highway_correction_gain.py @@ -50,22 +50,49 @@ def test_steady_offset_passes_unchanged(): np.testing.assert_allclose(out[-100:], bias, rtol=1e-6) -def test_curves_pass_through(): - raw = _weave(bias=1.0 / _V_HWY ** 2) +def test_steady_curve_wobble_is_smoothed(): + raw = _weave(seconds=40.0, bias=1.0 / _V_HWY ** 2) out = _run(HighwayCorrectionGain(), raw, gain=0.5) - np.testing.assert_allclose(out, raw, rtol=0, atol=1e-12) + tail = slice(len(raw) // 2, None) + amp_in, _ = _fit_sine(raw[tail], 0.6) + amp_out, _ = _fit_sine(out[tail], 0.6) + assert 0.45 < amp_out / amp_in < 0.62 + assert abs(out[tail].mean() - raw[tail].mean()) * _V_HWY ** 2 < 0.01 # the curve itself is untouched + + +def test_tight_curves_pass_through(): + raw = _weave(seconds=40.0, bias=2.5 / _V_HWY ** 2) + out = _run(HighwayCorrectionGain(), raw, gain=0.5) + tail = slice(len(raw) // 2, None) + np.testing.assert_allclose(out[tail], raw[tail], 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]) + # 5 s straight, then ramp to 1.2 m/s^2 over 2 s and hold + t = np.arange(0.0, 20.0, DT_CTRL) + lat = np.interp(t, [0, 5, 7, 20], [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) + assert np.max(np.abs(shortfall[t > 12.0])) < 0.01 + + +def test_curve_exit_is_not_held(): + # 20 s steady curve (smoothing active), then unwind to straight over 2 s + t = np.arange(0.0, 30.0, DT_CTRL) + lat = np.interp(t, [0, 2, 20, 22, 30], [1.2, 1.2, 1.2, 0, 0]) + raw = lat / _V_HWY ** 2 + hcg = HighwayCorrectionGain() + hcg.reset(raw[0]) + out = _run(hcg, raw, gain=0.5) + excess = (out - raw)[t > 19.0] * _V_HWY ** 2 + assert np.max(np.abs(excess)) < 0.15 + i_raw = np.argmax(lat[t > 19.0] <= 0.6) + i_out = np.argmax(out[t > 19.0] * _V_HWY ** 2 <= 0.6) + assert (i_out - i_raw) * DT_CTRL < 0.15 def test_below_speed_gate_passes_through(): @@ -109,5 +136,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.3) + floor = _run(HighwayCorrectionGain(), raw, gain=0.1) 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 dd3a81bbf8..81d94a615f 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -326,9 +326,9 @@ class StarPilotLateralLayout(_SettingsPage): ), 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."), + subtitle=tr_noop("Straights and steady curves (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", + on_click=lambda: self._show_slider("HighwayCorrectionGain", 0.1, 1.0, step=0.05, value_type="float", title="Highway Smoothing"), visible=lambda: alt_on() and hcg_available(), ), diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 9ce647d172..59f7ac8bcd 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -79,10 +79,10 @@ { "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.", + "description": "Straights and steady curves (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, + "min": 0.1, "max": 1.0, "step": 0.05, "precision": 2, diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 6185930855..5c7690bb0c 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.3, max=1.0) + toggle.highway_correction_gain = self.get_value("HighwayCorrectionGain", cast=float, condition=advanced_lateral_tuning, default=1.0, min=0.1, 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