diff --git a/common/params_keys.h b/common/params_keys.h index 957058f5dc..8eb728ebdd 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -432,6 +432,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}}, @@ -577,7 +578,7 @@ inline static std::unordered_map keys = { {"PauseAOLOnBrake", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"PauseLateralOnSignal", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"PauseLateralSpeed", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}}, - {"LateralResumeDelay", {PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}}, + {"LateralResumeDelay",{PERSISTENT, FLOAT, "0.0", "0.0", 1, SETTINGS_SIMPLE}}, {"PedalsOnUI", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}}, {"PIPPreviewEnabled", {PERSISTENT, BOOL, "0", "0", 1}}, {"PIPPreviewMask", {PERSISTENT, JSON, "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", "{\"width\":1928,\"height\":1208,\"center_left\":[315,548],\"center_right\":[1571,539],\"crop_size\":580}", 2}}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0fdf7c97cb..4e3569108a 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 @@ -399,6 +400,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 @@ -747,6 +749,11 @@ class Controls: bool(CS.leftBlinker or CS.rightBlinker), bool(CS.steeringPressed)) + 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..bd4d123d93 --- /dev/null +++ b/selfdrive/controls/lib/highway_correction_gain.py @@ -0,0 +1,81 @@ +from collections import deque + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_CTRL + +# Straight-highway gain on the FAST part of the desired curvature (user setting "HighwayCorrectionGain"). +# +# The Ioniq 6 highway weave is a lightly damped loop through the driving model, not a controller or EPS +# problem (drives 00000b2c..00000b52): +# - openpilot delivers 0.97x of the model's requested lateral accel, ~0.4 s late; Kp 0.4-1.4 and the +# EPS damping byte changed nothing. +# - with Hyundai LFA steering (model open loop) the model's request wobbles half as much and follows the +# car's own motion (gain ~0.94 at 0.4-0.8 Hz): road kicks -> car sways -> model asks to follow the +# sway -> openpilot delivers it. LFA delivers only ~1/1.36 of what the model asks and its weave is +# flat vs road roughness (0.09) while openpilot's grows with it (0.18 -> 0.25). +# +# So this lowers the loop gain at the weave frequency instead of filtering: the output is +# baseline + k * (curvature - baseline), k = 1 - weight * (1 - gain) +# with baseline a slow (1.5 s) low-pass. At 0.35-0.8 Hz the baseline has little content, so the result is +# ~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 +# 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. +BASELINE_TAU = 1.5 # 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 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 + + +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() # 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, gain: float, bypass: bool) -> float: + if not lat_active: + self.reset(curvature) + return curvature + + # The baseline always tracks the raw command, so fading in or changing the gain 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..bdb4bd60a6 --- /dev/null +++ b/selfdrive/controls/tests/test_highway_correction_gain.py @@ -0,0 +1,108 @@ +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) # a gain, not a low-pass: the 0.3 s smoother lagged ~50 deg + + +def test_steady_offset_passes_unchanged(): + bias = 0.15 / _V_HWY ** 2 # slight lane-keeping bias / crown, below the curve gate + 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) # baseline settled (e^-20 after 30 s) + + +def test_curves_pass_through(): + raw = _weave(bias=1.0 / _V_HWY ** 2) # 1 m/s^2 highway curve with the same wobble on top + 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(): + # 5 s straight, then ramp to 1.2 m/s^2 over 2 s and hold + 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 releases between 0.25 and 0.6 m/s^2, so the first ~0.3 m/s^2 of a curve is briefly softened + # (~0.13 m/s^2 at gain 0.5, <2 cm of path at 70 mph) ... + assert np.max(np.abs(shortfall)) < 0.15 # m/s^2 + # ... but 0.6 m/s^2 is reached with no delay, and the curve itself is untouched + 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 = 15.0 # ~34 mph + 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 # blinker / override for 5 s + hcg = HighwayCorrectionGain() + out = _run(hcg, raw, gain=0.5, bypass=bypass) + # fully bypassed well inside the window + mid = slice(int(17 / DT_CTRL), int(20 / DT_CTRL)) + np.testing.assert_allclose(out[mid], raw[mid], rtol=0, atol=1e-12) + # no output jumps bigger than the raw signal's own per-frame change allows + 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_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.4) + 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 ad3d3981dc..6b5dcd8171 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -103,6 +103,19 @@ class StarPilotLateralLayout(_SettingsPage): def aol_on(): return p.get_bool("AlwaysOnLateral") + hcg_known = [] + + def hcg_available(): + # HighwayCorrectionGain is a new params_keys.h entry: until scons rebuilds params_pyx, touching it raises + # UnknownKeyName, so keep the row hidden instead of crashing the UI on a stale build. + 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") @@ -305,6 +318,14 @@ class StarPilotLateralLayout(_SettingsPage): on_click=lambda: self._show_slider("SteerKP", max(0.01, cs.steerKp) * 0.5, max(0.01, cs.steerKp) * 3.0, 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 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."), + 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", + title="Highway Correction Gain"), + 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/starpilot_variables.py b/starpilot/common/starpilot_variables.py index 53b9eead84..be8237bd32 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.4, 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 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)