build

common/params_keys.h

@@ -431,6 +431,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

panda/board/obj/gitversion.h

@@ -1,2 +1,2 @@
 extern const uint8_t gitversion[19];
-const uint8_t gitversion[19] = "DEV-96ef704d-DEBUG";
+const uint8_t gitversion[19] = "DEV-73164cc8-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-96ef704d-DEBUG
+DEV-73164cc8-DEBUG

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
@@ -400,6 +401,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
@@ -760,6 +762,11 @@ class Controls:
       new_desired_curvature = self.gv70_highway_stabilizer.update(
         new_desired_curvature, CS.vEgo, stabilize_gv70, DT_CTRL)
 
+    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

selfdrive/controls/lib/highway_correction_gain.py

@@ -0,0 +1,66 @@
+from collections import deque
+
+from openpilot.common.constants import CV
+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.
+BASELINE_TAU = 1.5                        # s
+SPEED_OFF = 30.0 * CV.MPH_TO_MS
+SPEED_ON = 40.0 * CV.MPH_TO_MS
+LAT_ACCEL_ON = 0.25                       # m/s^2
+LAT_ACCEL_OFF = 0.6                       # 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
+
+
+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()
+    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
+
+    # tracked even when bypassed so fading in 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)

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -858,7 +858,7 @@ IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 3.55
 IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 9.10
 IONIQ_6_CENTER_TAPER_MAX = 0.082
 IONIQ_6_CENTER_TAPER_LAT = 0.24
-IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.025
+IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.08
 IONIQ_6_CENTER_TAPER_SPEED = 18.0
 IONIQ_6_CENTER_TAPER_SPEED_WIDTH = 2.5
 IONIQ_6_HIGHWAY_CENTER_TAPER_MAX = 0.046
@@ -868,7 +868,7 @@ IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED = 24.5
 IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH = 1.8
 IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX = 0.10
 IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT = 0.14
-IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04
+IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.12
 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED = 23.5
 IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH = 2.0
 IONIQ_6_HIGHWAY_TRANSITION_OUTPUT_TAPER_MAX = 0.18
@@ -885,8 +885,8 @@ IONIQ_6_LOW_MID_CENTER_TAPER_SPEED_WIDTH = 1.5
 IONIQ_6_DIRECTIONAL_TAPER_LAT_START = 0.19
 IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90
 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.06
-IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
-IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
+IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.17
+IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.28
 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.10
 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10
 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48
@@ -978,9 +978,9 @@ IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS
 # extra P/D gain right where there is no breakaway torque to overcome. Deadzone the jerk
 # feed below straight-line noise levels and fade friction near center at highway speed.
 IONIQ_6_FRICTION_JERK_DEADZONE = 0.30
-IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.50
+IONIQ_6_FRICTION_CENTER_FADE_MAX = 0.20
 IONIQ_6_FRICTION_CENTER_FADE_LAT = 0.15
-IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06
+IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH = 0.14
 IONIQ_6_FRICTION_CENTER_FADE_SPEED = 18.0
 IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH = 2.5
 # Newer Ioniq 6 highway center-chatter correction; activation is firmware-gated.