From 48ea42c0262d1931cf00acb34407bd23cce74af1 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Sun, 3 May 2026 12:51:37 -0500 Subject: [PATCH] honda --- selfdrive/controls/lib/latcontrol_pid.py | 20 ++++++++++++++++++++ selfdrive/controls/tests/test_latcontrol.py | 13 ++++++++++++- 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index b4a89c75e..684570682 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -36,6 +36,23 @@ def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_ return max(scale, 0.70) +def get_civic_bosch_modified_pid_output_alpha(desired_angle_deg: float, desired_angle_delta_deg: float, + v_ego: float, output_torque: float, prev_output_torque: float) -> float: + abs_angle = abs(desired_angle_deg) + if abs_angle < 3.0 or abs_angle > 20.0: + return 1.0 + + speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) + onset = min(max((abs_angle - 3.0) / 5.0, 0.0), 1.0) + cutoff = min(max((20.0 - abs_angle) / 6.0, 0.0), 1.0) + band_weight = onset * cutoff + 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.32 + (0.22 * speed_weight) + (0.10 * transition_weight) + (0.10 * sign_change_weight)) + return min(max(1.0 - smoothing, 0.22), 1.0) + + class LatControlPID(LatControl): def __init__(self, CP, CI, dt): super().__init__(CP, CI, dt) @@ -93,6 +110,9 @@ class LatControlPID(LatControl): if self.is_civic_bosch_modified and civic_bosch_modified_lateral_testing_ground_active(): desired_angle_delta = angle_steers_des_no_offset - self.prev_angle_steers_des_no_offset output_torque *= get_civic_bosch_modified_pid_output_scale(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo) + output_alpha = get_civic_bosch_modified_pid_output_alpha(angle_steers_des_no_offset, desired_angle_delta, CS.vEgo, + output_torque, self.prev_output_torque) + output_torque = self.prev_output_torque + (output_alpha * (output_torque - self.prev_output_torque)) output_torque = float(max(min(output_torque, self.steer_max), -self.steer_max)) pid_log.active = True diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 379dd5620..266d3deeb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -13,7 +13,11 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI from opendbc.car.vehicle_model import VehicleModel from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle -from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID, get_civic_bosch_modified_pid_output_scale +from openpilot.selfdrive.controls.lib.latcontrol_pid import ( + LatControlPID, + get_civic_bosch_modified_pid_output_alpha, + get_civic_bosch_modified_pid_output_scale, +) from openpilot.selfdrive.controls.lib.latcontrol_torque import ( LatControlTorque, get_bolt_2017_center_taper_scale, @@ -395,6 +399,13 @@ class TestLatControl: 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 + assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.0, 12.0, 0.2, 0.1) < 1.0 + assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 12.0, 0.2, 0.1) < get_civic_bosch_modified_pid_output_alpha(8.0, 0.0, 12.0, 0.2, 0.1) + assert get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 20.0, -0.2, 0.2) < get_civic_bosch_modified_pid_output_alpha(8.0, 0.4, 8.0, -0.2, 0.2) + assert get_civic_bosch_modified_pid_output_alpha(24.0, 0.0, 12.0, 0.2, 0.1) == 1.0 + def test_civic_bosch_modified_pid_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_pid_controller(HONDA.HONDA_CIVIC_BOSCH) monkeypatch.setattr(latcontrol_pid, "civic_bosch_modified_lateral_testing_ground_active", lambda: True)