diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 2a6b40484..5605d72e2 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -53,15 +53,15 @@ def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_ onset = min(max((abs_angle - 0.75) / 5.25, 0.0), 1.0) cutoff = min(max((22.0 - abs_angle) / 8.0, 0.0), 1.0) band_weight = onset * cutoff - small_curve_weight = min(max((10.0 - abs_angle) / 4.0, 0.0), 1.0) + small_curve_weight = min(max((12.0 - abs_angle) / 6.0, 0.0), 1.0) large_turn_weight = min(max((abs_angle - 16.0) / 6.0, 0.0), 1.0) transition_weight = min(abs(desired_angle_delta_deg) / 0.35, 1.0) sign_change_weight = 1.0 if (output_torque * prev_output_torque) < 0.0 else 0.0 smoothing = band_weight * (0.36 + (0.22 * speed_weight) + (0.12 * transition_weight) + (0.12 * sign_change_weight)) - smoothing *= 1.0 + (0.25 * small_curve_weight) + smoothing *= 1.0 + (0.35 * small_curve_weight) smoothing *= 1.0 - (0.50 * large_turn_weight) - return min(max(1.0 - smoothing, 0.16), 1.0) + return min(max(1.0 - smoothing, 0.14), 1.0) class LatControlPID(LatControl): diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 4e58f5546..b1f8acd57 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -1106,9 +1106,6 @@ class LatControlTorque(LatControl): output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk, CS.vEgo) elif volt_standard_test_active: output_torque *= volt_standard_center_taper - elif ioniq_6_test_active: - output_torque *= get_ioniq_6_output_taper_scale(setpoint, desired_lateral_jerk, CS.vEgo) - pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 81b047a44..e9f9d4463 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -1,3 +1,4 @@ +import pytest from parameterized import parameterized from types import SimpleNamespace @@ -366,6 +367,16 @@ class TestLatControl: assert lac_log.active + def test_ioniq_6_update_path_does_not_post_taper_output(self, monkeypatch): + base_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6) + base_output, _, _ = base_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + monkeypatch.setattr(latcontrol_torque, "get_ioniq_6_output_taper_scale", lambda *_args: 0.01) + tapered_controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_6) + tapered_output, _, _ = tapered_controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert tapered_output == pytest.approx(base_output) + def test_kia_ev6_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6) monkeypatch.setattr(latcontrol_torque, "kia_ev6_lateral_testing_ground_active", lambda: True)