add smooth curves

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

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)

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

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,

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