From 3bc108c0535012bf5269b76c240ad41eac7082a2 Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Fri, 9 Oct 2026 09:26:06 -0500 Subject: [PATCH] Add Highway Smoothing for Ioniq 6 highway weave Co-Authored-By: Claude Opus 5.5 --- openpilot/common/params_keys.h | 2 + openpilot/selfdrive/controls/controlsd.py | 15 +- .../controls/lib/highway_correction_gain.py | 139 ++++++++++++ .../tests/test_highway_correction_gain.py | 202 ++++++++++++++++++ openpilot/starpilot/galaxy/settings.py | 4 +- openpilot/starpilot/galaxy/web/js/settings.js | 2 +- .../starpilot/ui/feature_settings_owner.py | 69 +++++- .../starpilot/ui/feature_settings_state.py | 1 + 8 files changed, 429 insertions(+), 5 deletions(-) create mode 100644 openpilot/selfdrive/controls/lib/highway_correction_gain.py create mode 100644 openpilot/selfdrive/controls/tests/test_highway_correction_gain.py diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 76e8951722..9fe02331a3 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -151,6 +151,8 @@ inline static std::unordered_map keys = { {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0"}}, {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0"}}, {"TurnAssist", {PERSISTENT, BOOL, "1"}}, + {"HighwaySmoothing", {PERSISTENT, BOOL, "0"}}, + {"HighwayCorrectionGain", {PERSISTENT, FLOAT, "0.3"}}, {"GMPedalLongitudinal", {PERSISTENT, BOOL, "0"}}, {"ApiCache_Device", {PERSISTENT, STRING}}, {"ApiCache_FirehoseStats", {PERSISTENT, JSON}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index d751e12301..358cdf58a6 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -15,6 +15,7 @@ from openpilot.common.swaglog import cloudlog from opendbc.car.car_helpers import interfaces from opendbc.car.vehicle_model import VehicleModel from openpilot.selfdrive.controls.lib.drive_helpers import clip_curvature +from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels, read_gain from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD @@ -84,7 +85,9 @@ class Controls: self.desired_curvature = 0.0 self.model_turn_assist = ModelTurnAssist() self.lane_centering_controller = LaneCenteringController() - self.lateral_pause = LateralPause(read_lateral_pause(self.params), self.params) + self.highway_correction_gain = HighwayCorrectionGain() + self.highway_gain = read_gain(self.params) + self.lateral_pause =LateralPause(read_lateral_pause(self.params), self.params) self.lane_change_policy = lane_change_policy(read_lane_change(self.params)) self.lane_change_smoother = LaneChangeSmoother() self.lane_centering_host = LaneCenteringHost(self.params) if lane_runtime_supported(self.CP) else None @@ -287,6 +290,16 @@ class Controls: self.last_lane_centering_result = result if result.candidate_curvature is not None: new_desired_curvature = result.candidate_curvature + if self.sm.frame % 100 == 0: + self.highway_gain = read_gain(self.params) + predicted = None + if self.sm.all_checks(['modelV2']): + predicted = predicted_lateral_accels(model_v2.position.t, model_v2.velocity.x, model_v2.orientationRate.z) + new_desired_curvature = self.highway_correction_gain.update( + new_desired_curvature, CS.vEgo, CC.latActive, self.highway_gain, + bypass=bool(CS.leftBlinker or CS.rightBlinker or CS.steeringPressed or + model_v2.meta.laneChangeState != LaneChangeState.off or self.sm.valid['lateralManeuverPlan']), + predicted=predicted) previous_curvature = self.desired_curvature factor = self.lane_change_smoother.factor( active=bool(CC.latActive and CS.canValid and not CS.canTimeout and self.sm.all_checks(['modelV2']) and diff --git a/openpilot/selfdrive/controls/lib/highway_correction_gain.py b/openpilot/selfdrive/controls/lib/highway_correction_gain.py new file mode 100644 index 0000000000..5ecdd52da9 --- /dev/null +++ b/openpilot/selfdrive/controls/lib/highway_correction_gain.py @@ -0,0 +1,139 @@ +import math +from collections import deque + +import numpy as np + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL +from openpilot.starpilot.saved_source import read_saved + +# 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). 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. +CURVE_LEVEL_TAU = 3.0 # s +LAT_ACCEL_ON = 0.25 # m/s^2 +LAT_ACCEL_OFF = 0.6 # m/s^2 +CURVE_LEVEL_BLEND = (0.15, 0.35) # m/s^2 of curve level: straight thresholds -> curve thresholds +# The model's predicted path crosses into or out of a curve ~1.2-1.7 s before its request does and wobbles +# less, so it switches smoothing off ahead of entries, exits and tightening. With it, the request gate no +# longer has to tighten in curves (where it tripped on the wobble it was smoothing) and stays as a backup +# for the transitions the prediction misses. +PREDICTION_HORIZONS = (1.5, 2.0) # s +PREDICTED_CURVE_ON = 0.20 # m/s^2 +PREDICTED_CURVE_OFF = 0.45 # m/s^2 +# without a prediction the request gate tightens in curves so exits switch smoothing off in time +CURVE_DEVIATION_ON = 0.12 # m/s^2 +CURVE_DEVIATION_OFF = 0.30 # m/s^2 +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.1 +DEFAULT_GAIN = 0.3 +ENABLE_KEY = "HighwaySmoothing" +GAIN_KEY = "HighwayCorrectionGain" + + +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) + + +def read_gain(params) -> float: + enabled, readable = read_saved(params, ENABLE_KEY, 8) + if not readable or enabled != b"1": + return 1.0 + raw, readable = read_saved(params, GAIN_KEY, 16) + try: + gain = DEFAULT_GAIN if raw is None else float(raw) if readable else 1.0 + except ValueError: + return 1.0 + return min(max(gain, MIN_GAIN), 1.0) if math.isfinite(gain) else 1.0 + + +def predicted_lateral_accels(t, velocity_x, yaw_rate) -> tuple[float, ...] | None: + t, velocity_x, yaw_rate = (np.asarray(list(x), dtype=float) for x in (t, velocity_x, yaw_rate)) + if len(t) < 2 or len(t) != len(velocity_x) or len(t) != len(yaw_rate) or t[-1] < PREDICTION_HORIZONS[-1]: + return None + return tuple(float(np.interp(h, t, velocity_x) * np.interp(h, t, yaw_rate)) for h in PREDICTION_HORIZONS) + + +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.curve_level = curvature + self.envelope = _Envelope() + self.prediction_envelope = _Envelope() + self.bypass_weight = 0.0 + self.weight = 0.0 + + def update(self, curvature: float, v_ego: float, lat_active: bool, gain: float, bypass: bool, + predicted: tuple[float, ...] | None = None) -> 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) + self.curve_level += self.alpha_curve * (curvature - self.curve_level) + + v2 = v_ego ** 2 + level = abs(self.curve_level) * v2 + in_curve = _smoothstep(level, *CURVE_LEVEL_BLEND) + reference = in_curve * self.curve_level + envelope = self.envelope.update(abs(curvature - reference) * v2) + + if predicted: + gate = 1.0 - _smoothstep(envelope, LAT_ACCEL_ON, LAT_ACCEL_OFF) + deviation = max(abs(p - reference * v2) for p in predicted) + on = LAT_ACCEL_ON + in_curve * (PREDICTED_CURVE_ON - LAT_ACCEL_ON) + off = LAT_ACCEL_OFF + in_curve * (PREDICTED_CURVE_OFF - LAT_ACCEL_OFF) + gate *= 1.0 - _smoothstep(self.prediction_envelope.update(deviation), on, off) + else: + if self.prediction_envelope.frame: + self.prediction_envelope = _Envelope() + on = LAT_ACCEL_ON + in_curve * (CURVE_DEVIATION_ON - LAT_ACCEL_ON) + off = LAT_ACCEL_OFF + in_curve * (CURVE_DEVIATION_OFF - LAT_ACCEL_OFF) + gate = 1.0 - _smoothstep(envelope, on, off) + gate *= 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) * gate + k = 1.0 - self.weight * (1.0 - gain) + if k >= 1.0: + return curvature + return self.baseline + k * (curvature - self.baseline) diff --git a/openpilot/selfdrive/controls/tests/test_highway_correction_gain.py b/openpilot/selfdrive/controls/tests/test_highway_correction_gain.py new file mode 100644 index 0000000000..b5f32e3b1c --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_highway_correction_gain.py @@ -0,0 +1,202 @@ +import math + +import numpy as np + +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.controls.lib.highway_correction_gain import HighwayCorrectionGain, predicted_lateral_accels + + +_V_HWY = 31.0 # ~70 mph + + +def _run(hcg, curvature, gain=0.7, v_ego=_V_HWY, lat_active=True, bypass=None, predicted=None): + bypass = np.zeros(len(curvature), bool) if bypass is None else bypass + predicted = [None] * len(curvature) if predicted is None else [tuple(p) for p in predicted] + return np.array([hcg.update(float(c), v_ego, lat_active, gain, bool(b), predicted=p) + for c, b, p in zip(curvature, bypass, predicted, strict=True)]) + + +def _lead(lat, seconds): + # prediction of the lateral accel profile `seconds` ahead, at both horizons + n = int(seconds / DT_CTRL) + ahead = np.concatenate([lat[n:], np.full(n, lat[-1])]) + return np.c_[ahead, ahead] + + +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_steady_curve_wobble_is_smoothed(): + raw = _weave(seconds=40.0, bias=1.0 / _V_HWY ** 2) + out = _run(HighwayCorrectionGain(), raw, gain=0.5) + 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(): + # 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) + 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(): + 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.1) + np.testing.assert_allclose(low, floor, rtol=0, atol=1e-15) + + +def test_predicted_lateral_accels(): + t = 10.0 * (np.arange(33) / 32) ** 2 + assert predicted_lateral_accels(t, np.full(33, 30.0), np.full(33, 0.02)) == (0.6, 0.6) + assert predicted_lateral_accels(t[:10], np.full(10, 30.0), np.full(10, 0.02)) is None # horizon too short + assert predicted_lateral_accels(t, np.full(32, 30.0), np.full(33, 0.02)) is None + + +def test_prediction_smooths_wobbly_curve_more(): + # wobble big enough that the request-only gate backs off; the predicted path holds the curve level + raw = _weave(seconds=40.0, lat_accel_amp=0.2, bias=1.0 / _V_HWY ** 2) + pred = np.full((len(raw), 2), 1.0) + tail = slice(len(raw) // 2, None) + amp_in, _ = _fit_sine(raw[tail], 0.6) + without = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5)[tail], 0.6)[0] / amp_in + with_pred = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.5, predicted=pred)[tail], 0.6)[0] / amp_in + assert 0.45 < with_pred < 0.62 + assert with_pred < without - 0.1 + + +def test_prediction_releases_before_curve_exit(): + t = np.arange(0.0, 30.0, DT_CTRL) + lat = np.interp(t, [0, 20, 22, 30], [1.2, 1.2, 0, 0]) + raw = lat / _V_HWY ** 2 + def excess(predicted): + hcg = HighwayCorrectionGain(); hcg.reset(raw[0]) + out = _run(hcg, raw, gain=0.5, predicted=predicted) + return np.max(np.abs((out - raw)[t > 19.0])) * _V_HWY ** 2 + with_pred = excess(_lead(lat, 1.5)) + assert with_pred < 0.02 + assert with_pred < excess(None) + + +def test_prediction_releases_before_curve_entry(): + 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 + def shortfall(predicted): + out = _run(HighwayCorrectionGain(), raw, gain=0.5, predicted=predicted) + return np.max(np.abs(raw - out)) * _V_HWY ** 2 + with_pred = shortfall(_lead(lat, 1.5)) + assert with_pred < 0.02 + assert with_pred < shortfall(None) + + +def test_straights_smooth_the_same_with_prediction(): + raw = _weave() + pred = np.zeros((len(raw), 2)) + tail = slice(len(raw) // 2, None) + a = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7)[tail], 0.6)[0] + b = _fit_sine(_run(HighwayCorrectionGain(), raw, gain=0.7, predicted=pred)[tail], 0.6)[0] + assert math.isclose(a, b, rel_tol=0.02) diff --git a/openpilot/starpilot/galaxy/settings.py b/openpilot/starpilot/galaxy/settings.py index 9d748666dd..38f477f2fc 100644 --- a/openpilot/starpilot/galaxy/settings.py +++ b/openpilot/starpilot/galaxy/settings.py @@ -34,7 +34,7 @@ from openpilot.starpilot.ui.vasm_owner import ANNOTATION as VASM_ANNOTATION, RES PAGES = frozenset(("hub", "vehicle", "slc", "lane", "lane_change", "profiles", "traffic", "aggressive", "standard", "relaxed", - "curve", "torque", "aol", "wheel", "appearance", "display", "data", "sounds", "pip", "sentry", "vasm", + "curve", "torque", "highway_smoothing", "aol", "wheel", "appearance", "display", "data", "sounds", "pip", "sentry", "vasm", "conditional", "conditional/cem", "conditional/ccm", *(f"{name}/{category}" for name in ("traffic", "aggressive", "standard", "relaxed") for category in ("acceleration", "braking", "following")))) @@ -438,7 +438,7 @@ class SettingsGateway: with self.lock: ctx = self.context.sample() sections = [] - for page in ("torque", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"): + for page in ("torque", "highway_smoothing", "lane", "lane_change", "aol", "conditional", "profiles", "slc", "curve"): state = self._state(page, ctx) sections.append({"title": state.title, "rows": [{"label": row.label, "value": row.value} for row in state.rows[:64]]}) diff --git a/openpilot/starpilot/galaxy/web/js/settings.js b/openpilot/starpilot/galaxy/web/js/settings.js index 23fea5be10..c7b7d631ba 100644 --- a/openpilot/starpilot/galaxy/web/js/settings.js +++ b/openpilot/starpilot/galaxy/web/js/settings.js @@ -15,7 +15,7 @@ const post = (fetcher, path, body, signal) => fetcher(path, { }) export const SETTINGS_SECTIONS = Object.freeze([ - { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "lane"] }, + { id: "lateral", label: "Lateral (Steering)", icon: "bi-arrows-move", pages: ["aol", "lane_change", "torque", "highway_smoothing", "lane"] }, { id: "longitudinal", label: "Longitudinal (Speed & Following)", icon: "bi-speedometer2", pages: ["conditional", "curve", "profiles", "slc", "traffic", "aggressive", "standard", "relaxed"] }, { id: "wheel", label: "Wheel Controls", icon: "bi-controller", pages: ["wheel"] }, { id: "visual", label: "Visual (Display & UI)", icon: "bi-eye", pages: ["appearance", "ui_layout", "favorites", "pip"] }, diff --git a/openpilot/starpilot/ui/feature_settings_owner.py b/openpilot/starpilot/ui/feature_settings_owner.py index a789bec35d..5a42fb27e1 100644 --- a/openpilot/starpilot/ui/feature_settings_owner.py +++ b/openpilot/starpilot/ui/feature_settings_owner.py @@ -70,7 +70,7 @@ from openpilot.starpilot.controllers.toyota_cruise import capability as toyota_c BOOL_DEFAULTS = { - "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True, + "GMPedalLongitudinal": False, "ForceStops": False, "AlwaysAllowUploads": False, "TurnAssist": True, "HighwaySmoothing": False, "ReverseCruise": False, "ToyotaAutoHold": False, "VoltSNG": False, "GMAutoHold": False, "VoltOnePedalMode": False, "LongPitch": True, "DisableOpenpilotLongitudinal": False, "SpeedLimitController": False, "ShowSpeedLimits": True, @@ -642,6 +642,64 @@ class FeatureSettingsOwner: authorized=authorized, temp_prefix=".turn-assist-") return result.committed and result.verified + def _highway_smoothing_capability(self) -> tuple | None: + from openpilot.starpilot.lateral.controller_selection import policy_for + cp = self.vehicle_params() + if cp is None or policy_for(cp) != 'ioniq6': + return None + return (cp.carFingerprint, cp.lateralTuning.which(), str(cp.steerControlType), cp.passive, cp.dashcamOnly, cp.notCar, + tuple((str(config.safetyModel), int(config.safetyParam)) for config in cp.safetyConfigs)) + + def _highway_smoothing_rows(self, configurable: bool) -> list[FeatureRow]: + from openpilot.selfdrive.controls.lib.highway_correction_gain import DEFAULT_GAIN, ENABLE_KEY, GAIN_KEY, MIN_GAIN + capability = self._highway_smoothing_capability() + allowed = configurable and self.authority("torque") and capability is not None + toggle = self._bool_row(ENABLE_KEY, "Highway Smoothing", allowed) + rows = [replace(toggle, capability=capability, vehicle_fingerprint=self.vehicle_fingerprint(), dependencies=(), + reason=toggle.reason if toggle.value not in ("On", "Off") else + ("Calms highway weave on straights and steady curves above 35 mph. Applies within one second." + if capability is not None else "Highway Smoothing is available on the Ioniq 6"))] + if toggle.value != "On": + return rows + value, raw, valid = self._value(GAIN_KEY) + try: + number = DEFAULT_GAIN if raw is None else float(value) + valid = valid and self._readable(GAIN_KEY) and math.isfinite(number) and MIN_GAIN <= number <= 1.0 + except ValueError: + valid, number = False, DEFAULT_GAIN + rows.append(FeatureRow(GAIN_KEY, "Strength", f"{number:.2f}" if valid else "Invalid saved value", raw, + step=0.05 if valid else 0., minimum=MIN_GAIN, maximum=1.0, + available=allowed and (valid or self.authority("parked_preferences") and self._readable(GAIN_KEY)), + repair_value=str(DEFAULT_GAIN) if not valid else "", default_value=str(DEFAULT_GAIN), + vehicle_fingerprint=self.vehicle_fingerprint(), capability=capability, + reason="Lower values follow the model's quick back-and-forth corrections less. 1.00 matches off.")) + return rows + + def _apply_highway_smoothing(self, request: FeatureSettingsRequest) -> bool: + from openpilot.selfdrive.controls.lib.highway_correction_gain import ENABLE_KEY, MIN_GAIN + def authorized(): + row = next((row for row in self._highway_smoothing_rows(self.authority("preferences")) if row.key == request.key), None) + return (row is not None and row.available and (not row.repair_value or request.value == row.repair_value) and + row.capability == request.capability and row.vehicle_fingerprint == request.vehicle_fingerprint and + row.source == request.expected and request.related_source is None and not request.direction) + if not authorized(): + return False + if request.key == ENABLE_KEY: + if request.value not in ("Off", "On"): + return False + raw = b"1" if request.value == "On" else b"0" + else: + try: + number = float(request.value) + except ValueError: + return False + if not math.isfinite(number) or not MIN_GAIN <= number <= 1.0: + return False + raw = str(number).encode() + result = commit_exact(self.params, key=request.key, max_bytes=128, raw=raw, expected=request.expected, + authorized=authorized, temp_prefix=".highway-smoothing-") + return result.committed and result.verified + def snapshot(self, page: str, *, parked: bool, system_long: bool, lateral_context: bool, metric: bool, configure_while_driving: bool = False) -> FeatureSettingsState: selected_vehicle = self._vehicle_params_source() @@ -674,6 +732,10 @@ class FeatureSettingsOwner: FeatureRow("", "Steering and Torque", "Select the steering controller and tune its response", page=FeaturePage.TORQUE, available=True), FeatureRow("", "Always On Lateral", "Keep steering assistance active independently of cruise control", page=FeaturePage.AOL, available=True), FeatureRow("", "Wheel Controls", "Assign steering-wheel buttons and cruise behavior", page=FeaturePage.WHEEL, available=True)] + if self._highway_smoothing_capability() is not None: + rows.insert(rows.index(next(row for row in rows if row.page == FeaturePage.TORQUE)) + 1, + FeatureRow("", "Highway Smoothing", "Calm highway weave on straights and steady curves", + page=FeaturePage.HIGHWAY_SMOOTHING, available=True)) if (self._long_pitch_capability() is not None or self._bolt_disable_capability() is not None or self._auto_hold_capability() is not None or self._pedal_setup_capability() is not None or self._gm_stop_capability("VoltSNG") is not None or @@ -821,6 +883,9 @@ class FeatureSettingsOwner: capability=capability, dependencies=self._dependents(DOCUMENT_KEY), reason="Use the supplied vehicle tune and any values you customize below")) rows.extend(numeric_rows) + elif page == FeaturePage.HIGHWAY_SMOOTHING: + title = "Highway Smoothing" + rows.extend(self._highway_smoothing_rows(configurable)) elif page == FeaturePage.AOL: title = "Always On Lateral" rows.extend(self._lateral_pause_rows(configurable)) @@ -1122,6 +1187,8 @@ class FeatureSettingsOwner: return self._apply_lateral_pause(request) if key == "TurnAssist": return self._apply_turn_assist(request) + if key in ("HighwaySmoothing", "HighwayCorrectionGain"): + return self._apply_highway_smoothing(request) if key == 'LateralControllerSelection': return self.controller.apply(request) if key == SETUP_ACTION: diff --git a/openpilot/starpilot/ui/feature_settings_state.py b/openpilot/starpilot/ui/feature_settings_state.py index a18456824f..8dad28fae9 100644 --- a/openpilot/starpilot/ui/feature_settings_state.py +++ b/openpilot/starpilot/ui/feature_settings_state.py @@ -47,6 +47,7 @@ class FeaturePage(StrEnum): CURVE = "curve" TORQUE = "torque" AOL = "aol" + HIGHWAY_SMOOTHING = "highway_smoothing" WHEEL = "wheel" CONDITIONAL = "conditional" CONDITIONAL_CEM = "conditional/cem"