From 987de36f546c04709dc8702a67fd8e76a9d6731f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Thu, 9 Jul 2026 17:34:56 -0500 Subject: [PATCH] Float The Buffalo --- .../controls/lib/latcontrol_vehicle_tunes.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index a386ac8ee..014b78a6a 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -570,6 +570,18 @@ IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_SPEED_WIDTH = 2.5 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_START = 0.06 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END = 0.22 IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH = 0.035 +IONIQ_6_CURVY_SPEED_MIN = 10.5 +IONIQ_6_CURVY_SPEED_MAX = 19.0 +IONIQ_6_CURVY_SPEED_MIN_WIDTH = 1.5 +IONIQ_6_CURVY_SPEED_MAX_WIDTH = 1.8 +IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT = 0.12 +IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT = 0.16 +IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT = 0.10 +IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT = 0.14 +IONIQ_6_CURVY_UNWIND_LAT_START = 0.55 +IONIQ_6_CURVY_UNWIND_LAT_END = 3.4 +IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH = 0.16 +IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH = 0.50 IONIQ_6_LOW_SPEED_PID_RESET_SPEED = 0.1 * CV.MPH_TO_MS # Friction compensation near zero lateral accel amplifies planner jerk noise into a slow # (~0.5 Hz) weave on straights: the 0.09/0.39 small-signal slope plus the jerk feed acts as @@ -1780,6 +1792,12 @@ def _ioniq_6_transition_envelope(v_ego: float, desired_lateral_accel: float, des return _ioniq_6_low_speed_factor(v_ego) * lat_factor * jerk_factor +def _ioniq_6_curvy_speed_weight(v_ego: float) -> float: + onset = _ioniq_6_sigmoid((max(v_ego, 0.0) - IONIQ_6_CURVY_SPEED_MIN) / IONIQ_6_CURVY_SPEED_MIN_WIDTH) + cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_SPEED_MAX - max(v_ego, 0.0)) / IONIQ_6_CURVY_SPEED_MAX_WIDTH) + return onset * cutoff + + def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float, directional_taper_scale: float | None = None) -> float: if desired_lateral_accel == 0.0: @@ -1902,8 +1920,26 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la heavy_base_reduction *= 1.0 - low_speed_relief_weight reduction = band_weight * (base_reduction + unwind_reduction * unwind_weight) reduction += heavy_band_weight * (heavy_base_reduction + heavy_unwind_reduction * unwind_weight) + curvy_unwind_weight = 0.0 + curvy_unwind_floor_relief = 0.0 + if v_ego is not None: + curvy_unwind_speed_weight = _ioniq_6_curvy_speed_weight(v_ego) + curvy_unwind_lat_onset = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_CURVY_UNWIND_LAT_START) / + IONIQ_6_CURVY_UNWIND_LAT_ONSET_WIDTH) + curvy_unwind_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_CURVY_UNWIND_LAT_END - abs_lateral_accel) / + IONIQ_6_CURVY_UNWIND_LAT_CUTOFF_WIDTH) + curvy_unwind_weight = curvy_unwind_speed_weight * curvy_unwind_lat_onset * curvy_unwind_lat_cutoff * unwind_weight + curvy_unwind_floor_relief = (_ioniq_6_side_value(desired_lateral_accel, + IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_LEFT, + IONIQ_6_CURVY_UNWIND_FLOOR_RELIEF_RIGHT) * + curvy_unwind_weight) + reduction += (_ioniq_6_side_value(desired_lateral_accel, + IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_LEFT, + IONIQ_6_CURVY_UNWIND_EXTRA_REDUCTION_RIGHT) * + curvy_unwind_weight) floor = _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT) floor -= _ioniq_6_side_value(desired_lateral_accel, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT, IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT) * unwind_weight + floor -= curvy_unwind_floor_relief return max(1.0 - reduction, floor)