mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-03 12:32:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

honda

Browse Source
This commit is contained in:
firestar5683
2026-05-03 20:37:16 -05:00
parent b9ab07d7cd
commit da5a028057
2 changed files with 13 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_pid.py
+7
View File
@@ -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)
selfdrive/controls/tests/test_latcontrol.py
+6 -5
View File
@@ -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