blueee

common/params_keys.h

@@ -420,6 +420,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"GoatScream", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"GoatScreamCriticalAlerts", {PERSISTENT, BOOL, "0", "0", 1, SETTINGS_SIMPLE}},
     {"GreenLightAlert", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}},
+    {"HKGHighwayFrictionThreshold", {PERSISTENT, BOOL, "0", "0", 3}},
     {"HideAlerts", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"HideChangingLanesBanner", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"HideDistanceProfileBanner", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
@@ -431,6 +432,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}},
@@ -447,6 +449,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
     {"IncreaseFollowingSnow", {PERSISTENT, FLOAT, "0.0", "0.0", 2, SETTINGS_SIMPLE}},
     {"AggressiveCoolingEnabled", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
     {"IncreaseThermalLimits", {PERSISTENT, BOOL, "0", "0", 2, SETTINGS_SIMPLE}},
+    {"Ioniq6WeaveTune", {PERSISTENT, BOOL, "1", "1", 3}},
     {"IssueReported", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}},
     {"KonikDongleId", {PERSISTENT, STRING, "", "", 0}},
     {"KonikMinutes", {PERSISTENT, INT, "0", "0", 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-dc492787-DEBUG";

panda/board/obj/version

@@ -1 +1 @@
-DEV-96ef704d-DEBUG
+DEV-dc492787-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_torque.py

@@ -229,6 +229,8 @@ class LatControlTorque(LatControl):
     flm_profile_active = bool(getattr(starpilot_toggles, "flm_trial_applied", False) and
                               getattr(starpilot_toggles, "flm_active_profile_id", ""))
     set_flm_runtime_overrides(getattr(starpilot_toggles, "flm_active_overrides", None) if flm_profile_active else None)
+    set_lateral_test_toggles(getattr(starpilot_toggles, "ioniq_6_weave_tune", True),
+                             getattr(starpilot_toggles, "hkg_highway_friction_threshold", False))
     flm_surface_active = flm_profile_active and flm_runtime_overrides_active()
     measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
     measurement = measured_curvature * CS.vEgo ** 2

selfdrive/controls/lib/latcontrol_vehicle_tunes.py

@@ -15,6 +15,8 @@ from openpilot.starpilot.common.testing_grounds import testing_ground
 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30
 STANDARD_FRICTION_THRESHOLD = 0.30
 HKG_CANFD_BASE_FRICTION_THRESHOLD = 0.39
+HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD = 0.78
+HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD_BP = [15.0, 25.0]  # m/s
 FLM_SCHEMA_VERSION = 1
 FLM_FRICTION_SPEED_KNOTS = [0.0, 5.0, 10.0, 15.0, 25.0]
 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20
@@ -858,7 +860,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 +870,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 +887,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,11 +980,18 @@ 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
+# values before the weave tune, used when the Ioniq6WeaveTune toggle is off
+IONIQ_6_LEGACY_CENTER_TAPER_LAT_WIDTH = 0.025
+IONIQ_6_LEGACY_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH = 0.04
+IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_LEFT = 0.11
+IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.45
+IONIQ_6_LEGACY_FRICTION_CENTER_FADE_MAX = 0.50
+IONIQ_6_LEGACY_FRICTION_CENTER_FADE_LAT_WIDTH = 0.06
 # Newer Ioniq 6 highway center-chatter correction; activation is firmware-gated.
 IONIQ_6_2025_FRICTION_SCALE_MULT = 0.80
 IONIQ_6_2025_FRICTION_JERK_DEADZONE = 0.45
@@ -1339,6 +1348,19 @@ TRAILER_LATERAL_FRICTION_GAIN = 0.03
 _FLM_ACTIVE_OVERRIDES_TEXT = ""
 _FLM_ACTIVE_OVERRIDES = {}
 
+_IONIQ_6_WEAVE_TUNE = True
+_HKG_HIGHWAY_FRICTION_THRESHOLD = False
+
+
+def set_lateral_test_toggles(ioniq_6_weave_tune: bool, hkg_highway_friction_threshold: bool) -> None:
+  global _IONIQ_6_WEAVE_TUNE, _HKG_HIGHWAY_FRICTION_THRESHOLD
+  _IONIQ_6_WEAVE_TUNE = bool(ioniq_6_weave_tune)
+  _HKG_HIGHWAY_FRICTION_THRESHOLD = bool(hkg_highway_friction_threshold)
+
+
+def _ioniq_6_weave(tuned: float, legacy: float) -> float:
+  return tuned if _IONIQ_6_WEAVE_TUNE else legacy
+
 
 def _sigmoid(x: float) -> float:
   if x >= 0.0:
@@ -1358,7 +1380,11 @@ def _standard_friction_threshold_default(v_ego: float) -> float:
 
 
 def _hkg_canfd_base_friction_threshold_default(v_ego: float) -> float:
-  return max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD)
+  threshold = max(_gm_base_friction_threshold_default(v_ego), HKG_CANFD_BASE_FRICTION_THRESHOLD)
+  if _HKG_HIGHWAY_FRICTION_THRESHOLD:
+    threshold = max(threshold, float(np.interp(v_ego, HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD_BP,
+                                               [HKG_CANFD_BASE_FRICTION_THRESHOLD, HKG_CANFD_HIGHWAY_FRICTION_THRESHOLD])))
+  return threshold
 
 
 def _flm_copy_json(value):
@@ -3861,8 +3887,10 @@ def get_ioniq_6_friction_scale(v_ego: float, desired_lateral_accel: float, desir
 
 def get_ioniq_6_friction_center_fade_scale(desired_lateral_accel: float, v_ego: float) -> float:
   speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_FRICTION_CENTER_FADE_SPEED) / IONIQ_6_FRICTION_CENTER_FADE_SPEED_WIDTH)
-  center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH)
-  return 1.0 - IONIQ_6_FRICTION_CENTER_FADE_MAX * speed_weight * center_weight
+  lat_width = _ioniq_6_weave(IONIQ_6_FRICTION_CENTER_FADE_LAT_WIDTH, IONIQ_6_LEGACY_FRICTION_CENTER_FADE_LAT_WIDTH)
+  center_weight = _ioniq_6_sigmoid((IONIQ_6_FRICTION_CENTER_FADE_LAT - abs(desired_lateral_accel)) / lat_width)
+  fade_max = _ioniq_6_weave(IONIQ_6_FRICTION_CENTER_FADE_MAX, IONIQ_6_LEGACY_FRICTION_CENTER_FADE_MAX)
+  return 1.0 - fade_max * speed_weight * center_weight
 
 
 def get_ioniq_6_2025_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
@@ -3914,7 +3942,8 @@ def get_ioniq_6_2025_low_speed_center_friction_scale(desired_lateral_accel: floa
 
 def get_ioniq_6_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
   speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_CENTER_TAPER_SPEED) / IONIQ_6_CENTER_TAPER_SPEED_WIDTH)
-  center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / IONIQ_6_CENTER_TAPER_LAT_WIDTH)
+  lat_width = _ioniq_6_weave(IONIQ_6_CENTER_TAPER_LAT_WIDTH, IONIQ_6_LEGACY_CENTER_TAPER_LAT_WIDTH)
+  center_weight = _ioniq_6_sigmoid((IONIQ_6_CENTER_TAPER_LAT - abs(desired_lateral_accel)) / lat_width)
   high_speed_reduction = _flm_vehicle_knob("hyundai_ioniq_6.center_taper_max", IONIQ_6_CENTER_TAPER_MAX) * speed_weight * center_weight
 
   highway_speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED) / IONIQ_6_HIGHWAY_CENTER_TAPER_SPEED_WIDTH)
@@ -3959,7 +3988,9 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la
     curvy_turn_in_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_TURN_IN_TRIM_LAT_END - abs_lateral_accel) /
                                                 IONIQ_6_CURVY_TURN_IN_TRIM_LAT_CUTOFF_WIDTH)
     curvy_turn_in_trim_weight = curvy_turn_in_speed_weight * curvy_turn_in_lat_onset * curvy_turn_in_lat_cutoff * turn_in_weight
-  base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT)
+  base_reduction = _ioniq_6_side_value(desired_lateral_accel,
+                                       _ioniq_6_weave(IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_LEFT),
+                                       _ioniq_6_weave(IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT, IONIQ_6_LEGACY_DIRECTIONAL_TAPER_BASE_RIGHT))
   unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT)
   heavy_base_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT)
   heavy_unwind_reduction = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT, IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT)
@@ -4010,7 +4041,7 @@ def get_ioniq_6_output_taper_scale(desired_lateral_accel: float, desired_lateral
 def get_ioniq_6_highway_output_taper_scale(desired_lateral_accel: float, v_ego: float) -> float:
   speed_weight = _ioniq_6_sigmoid((v_ego - IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED) / IONIQ_6_HIGHWAY_OUTPUT_TAPER_SPEED_WIDTH)
   center_weight = _ioniq_6_sigmoid((IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
-                                   IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH)
+                                   _ioniq_6_weave(IONIQ_6_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH, IONIQ_6_LEGACY_HIGHWAY_OUTPUT_TAPER_LAT_WIDTH))
   reduction = IONIQ_6_HIGHWAY_OUTPUT_TAPER_MAX * speed_weight * center_weight
   return 1.0 - reduction