diff --git a/openpilot/selfdrive/controls/lib/latcontrol_torque.py b/openpilot/selfdrive/controls/lib/latcontrol_torque.py index 60eb1b9c54..2b07a370a6 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_torque.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_torque.py @@ -12,6 +12,7 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController from openpilot.starpilot.lateral.controller_selection import ControllerMode, default_selection, policy_for +from openpilot.starpilot.lateral.torque_shaping import KP as STARPILOT_KP from openpilot.starpilot.lateral.torque_extension import apply_turn_assist, create_extension, create_turn_assist from openpilot.starpilot.flm import live as flm @@ -62,6 +63,8 @@ class LatControlTorque(LatControl): self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt) self.turn_assist = create_turn_assist(CP, turn_assist) self.starpilot_extension = create_extension(self, CP, self.controller_mode, self.controller_policy, turn_assist=turn_assist) + if self.starpilot_extension is not None: + self.pid._k_p = [[0.0], [STARPILOT_KP]] self.flm_source = flm.GmLiveSource(CP, self.controller_mode) self.flm_surface = None from openpilot.starpilot.flm.runtime_adapter_registry import baseline_values diff --git a/openpilot/starpilot/lateral/tests/test_effective_starpilot_gain.py b/openpilot/starpilot/lateral/tests/test_effective_starpilot_gain.py new file mode 100644 index 0000000000..0320ef14d2 --- /dev/null +++ b/openpilot/starpilot/lateral/tests/test_effective_starpilot_gain.py @@ -0,0 +1,70 @@ +from types import SimpleNamespace + +import numpy as np +import pytest + +from opendbc.car.car_helpers import interfaces +from opendbc.car.hyundai.values import CAR +from opendbc.car.gm.values import CAR as GM_CAR +from opendbc.car.vehicle_model import VehicleModel +from openpilot.cereal import log +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.starpilot.lateral.controller_selection import ControllerMode, policy_for +from openpilot.starpilot.lateral.gain_runtime import create_gain_owner, gain_basis +from openpilot.starpilot.lateral.torque_settings import DOCUMENT_KEY, replace_gain, serialize_document + + +def controller(car=CAR.KIA_EV6, mode=ControllerMode.STARPILOT): + cp = interfaces[car].get_non_essential_params(car) + return cp, LatControlTorque(cp.as_reader(), interfaces[car](cp), .01, controller_mode=mode) + + +def test_every_starpilot_torque_profile_uses_effective_stable_gain(): + checked = [] + wrong = [] + for car, interface in interfaces.items(): + cp = interface.get_non_essential_params(car) + if cp.lateralTuning.which() != 'torque' or policy_for(cp) is None: + continue + before = cp.to_dict() + selected = LatControlTorque(cp.as_reader(), interface(cp), .01, controller_mode=ControllerMode.STARPILOT) + assert cp.to_dict() == before + checked.append(str(car)) + for speed in (.3, 1., 2., 5., 10., 15., 30.): + actual = float(np.interp(speed, selected.pid._k_p[0], selected.pid._k_p[1])) + if actual != pytest.approx(.6): + wrong.append((str(car), speed, actual)) + assert {'KIA_EV6', 'HYUNDAI_IONIQ_5', 'KIA_NIRO_EV_2ND_GEN'} <= set(checked) + assert not wrong, wrong + + +def test_ev6_low_speed_proportional_term_matches_stable_effective_loop(): + cp, selected = controller() + params = log.VehicleParameters.new_message(angleOffsetDeg=-.53, roll=.015) + state = SimpleNamespace(vEgo=2.3547, steeringAngleDeg=-38.1, steeringPressed=False) + vm = VehicleModel(cp) + for frame in range(100): + _, _, result = selected.update(frame > 5, state, vm, params, False, -.001, False, .3) + if result.active: + raw_error = result.desiredLateralAccel - result.actualLateralAccel + low_speed_factor = (np.interp(state.vEgo, [0., 10., 20., 30.], [12., 10.5, 8., 5.]) / state.vEgo) ** 2 + assert result.p == pytest.approx(raw_error * (.6 + low_speed_factor), abs=1e-6) + + +def test_preexisting_manual_gain_keeps_ownership_and_withdraws_to_stable(tmp_path): + cp, selected = controller(GM_CAR.CHEVROLET_BOLT_ACC_2022_2023) + basis = gain_basis(cp, selected) + raw = serialize_document(replace_gain({}, str(cp.carFingerprint), basis, 'custom', .7)) + file = tmp_path / DOCUMENT_KEY + file.write_bytes(raw) + (tmp_path / "AdvancedLateralTune").write_bytes(b"1") + params = SimpleNamespace(get_param_path=lambda key: str(tmp_path / key)) + owner = create_gain_owner(params, cp, selected) + assert owner is not None + assert selected.pid._k_p == [[0.], [.7]] + assert file.read_bytes() == raw + selected.pid.i = .123 + file.unlink() + owner.refresh(now_ns=1_000_000_000) + assert selected.pid._k_p == [[0.], [.6]] + assert selected.pid.i == .123 diff --git a/prebuilt.json b/prebuilt.json index a0c0a17d77..04ac3cc9b7 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "0fbaa39f00cef954a60bf801f0a0d1981355394fbf5e4870f106bac14d219f2b", "schema_version": 1} +{"protected_inventory_sha256": "525678c0d1155fc613717e32fcd31216bcdc0fc98c63b907e0fe5a02cb849db0", "schema_version": 1}