highway smoothing 2

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:

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)

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(),
       ),

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