diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 04ae4a351..d78d53c3c 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -15,12 +15,12 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool: def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float: abs_angle = abs(desired_angle_deg) speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) - center_weight = min(max((6.0 - abs_angle) / 6.0, 0.0), 1.0) - angle_weight = min(max((abs_angle - 6.0) / 22.0, 0.0), 1.0) + center_weight = min(max((10.0 - abs_angle) / 10.0, 0.0), 1.0) + angle_weight = min(max((abs_angle - 10.0) / 18.0, 0.0), 1.0) phase = desired_angle_deg * desired_angle_delta_deg is_left = desired_angle_deg > 0.0 - center_taper = 0.08 + center_taper = 0.18 base_scale = 0.10 if is_left else 0.12 turn_in_scale = 0.10 if is_left else 0.14 unwind_scale = 0.14 if is_left else 0.20 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index b0ff78626..127e3da4e 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -381,6 +381,8 @@ class TestLatControl: def test_civic_bosch_modified_pid_scale_curve(self): assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) < 1.0 + assert get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) < 1.0 + assert get_civic_bosch_modified_pid_output_scale(12.0, 0.0, 12.0) > get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(20.0, -0.5, 12.0) < get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0)