From b7d76c176b49aff7f1b02f0f1cd8097396ed660f Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 7 Apr 2026 12:11:21 -0500 Subject: [PATCH] patch --- selfdrive/controls/lib/latcontrol_torque.py | 15 ++++---- selfdrive/controls/tests/test_latcontrol.py | 39 +++++++++++++++++++++ 2 files changed, 45 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 11ff7081..3dd640e7 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -151,8 +151,6 @@ class LatControlTorque(LatControl): self.torque_ff_scale_neg = 1.0 self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0)) self.torque_ki_mult = 1.0 - self.bolt_2018_2021_test_active = self.is_bolt_2018_2021 and bolt_2018_2021_lateral_testing_ground_active() - if self.is_bolt: kp_scale = getattr(self.torque_params, "kp", getattr(self.torque_params, "kpDEPRECATED", 1.0)) ki_scale = getattr(self.torque_params, "ki", getattr(self.torque_params, "kiDEPRECATED", 1.0)) @@ -227,14 +225,13 @@ class LatControlTorque(LatControl): ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale + bolt_2018_2021_test_active = self.is_bolt_2018_2021 and bolt_2018_2021_lateral_testing_ground_active() friction_threshold = get_friction_threshold(CS.vEgo) - if self.bolt_2018_2021_test_active: + friction_scale = 1.0 + if bolt_2018_2021_test_active: friction_threshold = get_bolt_2018_2021_friction_threshold(CS.vEgo) - effective_friction = self.torque_params.as_builder() - effective_friction.friction *= BOLT_2018_2021_FRICTION_MULT - else: - effective_friction = self.torque_params - ff += get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, effective_friction) + friction_scale = BOLT_2018_2021_FRICTION_MULT + ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: boost_scale = np.interp(abs(gravity_adjusted_future_lateral_accel), [0.0, DEADZONE_BOOST_LAT_ACCEL], [1.0, 0.0]) @@ -249,7 +246,7 @@ class LatControlTorque(LatControl): output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) if self.is_bolt_2017 and bolt_2017_lateral_testing_ground_active(): output_torque *= get_bolt_2017_torque_scale(setpoint) - elif self.bolt_2018_2021_test_active: + elif bolt_2018_2021_test_active: output_torque *= get_bolt_2018_2021_dynamic_torque_scale(setpoint, desired_lateral_jerk) pid_log.active = True diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index be5ff16e..cb2e3ee8 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -2,6 +2,7 @@ from parameterized import parameterized from types import SimpleNamespace from cereal import car, custom, log +import openpilot.selfdrive.controls.lib.latcontrol_torque as latcontrol_torque from opendbc.car.car_helpers import interfaces from opendbc.car.honda.values import CAR as HONDA from opendbc.car.toyota.values import CAR as TOYOTA @@ -23,6 +24,28 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( class TestLatControl: + @staticmethod + def _build_torque_controller(car_name): + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CI = CarInterface(CP, custom.StarPilotCarParams.new_message()) + controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) + VM = VehicleModel(CP) + + CS = car.CarState.new_message() + CS.vEgo = 22 + CS.steeringPressed = False + CS.steeringAngleDeg = 1.0 + + params = log.LiveParametersData.new_message() + params.steerRatio = CP.steerRatio + params.stiffnessFactor = 1.0 + params.roll = 0.0 + params.angleOffsetDeg = 0.0 + + starpilot_toggles = SimpleNamespace() + return controller, VM, CS, params, starpilot_toggles + def test_bolt_2017_testing_ground_scale_curve(self): assert get_bolt_2017_torque_scale(0.1) == 1.0 assert get_bolt_2017_torque_scale(-0.1) == 1.0 @@ -46,6 +69,22 @@ class TestLatControl: assert high > get_friction_threshold(30.0) assert (low - get_friction_threshold(2.0)) > (mid - get_friction_threshold(10.0)) > (high - get_friction_threshold(30.0)) + def test_bolt_2017_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2017) + monkeypatch.setattr(latcontrol_torque, "bolt_2017_lateral_testing_ground_active", lambda: True) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + + def test_bolt_2018_2021_testing_ground_update_path(self, monkeypatch): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2018_2021) + monkeypatch.setattr(latcontrol_torque, "bolt_2018_2021_lateral_testing_ground_active", lambda: True) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + @parameterized.expand([(HONDA.HONDA_CIVIC, LatControlPID), (TOYOTA.TOYOTA_RAV4, LatControlTorque), (NISSAN.NISSAN_LEAF, LatControlAngle), (GM.CHEVROLET_BOLT_ACC_2022_2023, LatControlTorque)]) def test_saturation(self, car_name, controller):