From e4e416bfb35e05d91e6b3d45fe62214b39a3587f Mon Sep 17 00:00:00 2001 From: whoisdomi Date: Mon, 6 Jul 2026 14:56:20 -0500 Subject: [PATCH] Notchy Highways --- selfdrive/controls/lib/latcontrol_torque.py | 8 +++++- .../controls/lib/latcontrol_vehicle_tunes.py | 27 ++++++++++++------- selfdrive/controls/tests/test_latcontrol.py | 2 +- 3 files changed, 26 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index babfb860f..be460ceb0 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -59,6 +59,7 @@ class LatControlTorque(LatControl): self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt) self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) + self.ioniq_6_directional_taper_filter = FirstOrderFilter(1.0, IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC, self.dt) self.previous_measurement = 0.0 self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt) self.low_speed_reset_threshold = max(CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) @@ -164,6 +165,7 @@ class LatControlTorque(LatControl): self.measurement_rate_filter.x = 0.0 self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len, maxlen=self.lat_accel_request_buffer_len) self.prev_desired_lateral_accel = 0.0 + self.ioniq_6_directional_taper_filter.x = 1.0 else: if self.prev_steering_pressed and not CS.steeringPressed: self.pid.i *= self.steer_release_i_decay @@ -283,7 +285,11 @@ class LatControlTorque(LatControl): ff *= get_ioniq_ev_old_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_ev_old_center_taper friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_ev_old_center_taper) elif ioniq_6_active: - ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_6_center_taper + # smooth the directional taper so jerk-gated unwind cuts can't step the FF in one frame + ioniq_6_directional_taper = self.ioniq_6_directional_taper_filter.update( + get_ioniq_6_directional_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo)) + ff *= get_ioniq_6_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo, + directional_taper_scale=ioniq_6_directional_taper) * ioniq_6_center_taper friction_threshold = get_ioniq_6_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) / max(ioniq_6_center_taper, 1e-3) friction_scale = get_ioniq_6_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * ioniq_6_center_taper) diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 7d1c0080b..bd7b893a5 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -517,14 +517,20 @@ 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_UNWIND_LEFT = 2.15 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 4.25 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.10 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.10 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_LEFT = 0.48 IONIQ_6_DIRECTIONAL_TAPER_FLOOR_RIGHT = 0.52 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_LEFT = 0.20 IONIQ_6_DIRECTIONAL_TAPER_UNWIND_FLOOR_RIGHT = 0.10 -IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 0.60 -IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.14 +IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET = 1.00 +IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH = 0.30 +# Unwind detection needs a softer phase transition and time smoothing than the shared +# PHASE_SCALE: desired lateral jerk noise in a sustained curve (~+/-1-2 m/s^3) otherwise +# chatters the taper between its base value and its floor at ~0.5 Hz (felt as notchy +# steering in highway sweepers). +IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE = 0.45 +IONIQ_6_DIRECTIONAL_TAPER_FILTER_RC = 0.4 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF = 0.98 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED = 11.2 IONIQ_6_DIRECTIONAL_TAPER_LOW_SPEED_RELIEF_SPEED_WIDTH = 1.5 @@ -569,8 +575,8 @@ IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START = 0.90 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH = 0.18 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_LEFT = 0.03 IONIQ_6_HEAVY_DIRECTIONAL_TAPER_BASE_RIGHT = 0.11 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.78 -IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 1.10 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.40 +IONIQ_6_HEAVY_DIRECTIONAL_TAPER_UNWIND_RIGHT = 0.55 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 @@ -1764,7 +1770,8 @@ 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 get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: +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: return 1.0 @@ -1798,8 +1805,10 @@ def get_ioniq_6_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: flo high_speed_lat_cutoff = _ioniq_6_sigmoid((IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_END - abs_lateral_accel) / IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_LAT_WIDTH) high_speed_right_turn_in_scale = IONIQ_6_HIGH_SPEED_RIGHT_TURN_IN_FF_BOOST * high_speed_weight * high_speed_lat_onset * high_speed_lat_cutoff + if directional_taper_scale is None: + directional_taper_scale = get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) return (1.0 + crawl_turn_in_scale + high_speed_right_turn_in_scale + - (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * get_ioniq_6_directional_taper_scale(desired_lateral_accel, desired_lateral_jerk, v_ego) + (extra_scale * turn_in_boost * max(unwind_taper, 0.0))) * directional_taper_scale def get_ioniq_6_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: @@ -1859,7 +1868,7 @@ def get_ioniq_6_directional_taper_scale(desired_lateral_accel: float, desired_la cutoff = _ioniq_6_sigmoid((IONIQ_6_DIRECTIONAL_TAPER_LAT_END - abs_lateral_accel) / IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH) band_weight = onset * cutoff heavy_band_weight = _ioniq_6_sigmoid((abs_lateral_accel - IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_START) / IONIQ_6_HEAVY_DIRECTIONAL_TAPER_LAT_WIDTH) - phase = _ioniq_6_transition_phase(desired_lateral_accel, desired_lateral_jerk) + phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / IONIQ_6_DIRECTIONAL_TAPER_PHASE_SCALE) unwind_weight = max(-phase, 0.0) * _ioniq_6_sigmoid((abs(desired_lateral_jerk) - IONIQ_6_DIRECTIONAL_TAPER_JERK_ONSET) / IONIQ_6_DIRECTIONAL_TAPER_JERK_WIDTH) low_speed_relief_weight = 0.0 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index fdf0b695f..e03f7a98f 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -509,7 +509,7 @@ class TestLatControl: assert get_ioniq_6_directional_taper_scale(1.2, -0.40) > get_ioniq_6_directional_taper_scale(1.2, -0.7) assert get_ioniq_6_directional_taper_scale(-1.2, -0.40, 8.0) > get_ioniq_6_directional_taper_scale(-1.2, -0.40, 25.0) assert get_ioniq_6_directional_taper_scale(1.2, 0.40, 8.0) > get_ioniq_6_directional_taper_scale(1.2, 0.40, 25.0) - assert get_ioniq_6_directional_taper_scale(-1.2, 0.7, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 0.7, 25.0), abs=0.02) + assert get_ioniq_6_directional_taper_scale(-1.2, 1.6, 8.0) == pytest.approx(get_ioniq_6_directional_taper_scale(-1.2, 1.6, 25.0), abs=0.02) assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) assert get_ioniq_6_directional_taper_scale(-0.18, -0.40, 9.0) > get_ioniq_6_directional_taper_scale(-0.18, -0.40, 20.0) assert get_ioniq_6_directional_taper_scale(-0.50, -0.40, 3.0) > get_ioniq_6_directional_taper_scale(-0.50, -0.40, 6.0)