From da5a028057a875b778a3dffe986b3749688773e9 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 3 May 2026 20:37:16 -0500 Subject: [PATCH] honda --- selfdrive/controls/lib/latcontrol_pid.py | 7 +++++++ selfdrive/controls/tests/test_latcontrol.py | 11 ++++++----- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 20546a10c..b1230f241 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -17,20 +17,27 @@ def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_ speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) center_speed_weight = 0.70 + (0.30 * speed_weight) center_weight = min(max((18.0 - abs_angle) / 18.0, 0.0), 1.0) + mid_turn_weight = min(max((abs_angle - 10.0) / 10.0, 0.0), 1.0) angle_weight = min(max((abs_angle - 18.0) / 10.0, 0.0), 1.0) phase = desired_angle_deg * desired_angle_delta_deg is_left = desired_angle_deg > 0.0 center_taper = 0.32 + mid_turn_scale = 0.08 if is_left else -0.06 + mid_turn_turn_in_scale = 0.05 if is_left else -0.04 + mid_turn_unwind_scale = -0.03 if is_left else 0.06 base_scale = 0.12 if is_left else 0.10 turn_in_scale = 0.12 if is_left else 0.12 unwind_scale = 0.14 if is_left else 0.18 scale = 1.0 - (center_speed_weight * center_weight * center_taper) + scale += speed_weight * mid_turn_weight * mid_turn_scale scale += speed_weight * angle_weight * base_scale if phase > 0.2: + scale += speed_weight * mid_turn_weight * mid_turn_turn_in_scale scale += speed_weight * angle_weight * turn_in_scale elif phase < -0.2: + scale += speed_weight * mid_turn_weight * mid_turn_unwind_scale scale -= speed_weight * angle_weight * unwind_scale return max(scale, 0.70) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index cd6b70ac8..8d7ca78ca 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -388,15 +388,16 @@ class TestLatControl: assert get_civic_bosch_modified_pid_output_scale(10.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(12.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(14.0, 0.0, 12.0) < 1.0 - assert get_civic_bosch_modified_pid_output_scale(16.0, 0.0, 12.0) < 1.0 + assert get_civic_bosch_modified_pid_output_scale(-16.0, 0.0, 12.0) < 1.0 + assert get_civic_bosch_modified_pid_output_scale(16.0, 0.0, 12.0) > get_civic_bosch_modified_pid_output_scale(-16.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 6.0) < 0.9 assert get_civic_bosch_modified_pid_output_scale(18.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) 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 abs(get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) - get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0)) < 5e-3 - assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 4.0) < get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 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.5, 12.0) + assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 4.0) > get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) def test_civic_bosch_modified_pid_output_alpha_curve(self): assert get_civic_bosch_modified_pid_output_alpha(0.0, 0.0, 12.0, 0.2, 0.1) == 1.0