diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index dbd0abcd04..04d3f558a9 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -191,6 +191,7 @@ inline static std::unordered_map keys = { {"ToyotaEnhancedBsm", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ToyotaTSS2Long", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ToyotaDriveMode", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"ToyotaPriusTss2Pid", {PERSISTENT | BACKUP, BOOL, "0"}}, // MADS params {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/openpilot/sunnypilot/selfdrive/car/interfaces.py b/openpilot/sunnypilot/selfdrive/car/interfaces.py index 500012eff4..840d269371 100644 --- a/openpilot/sunnypilot/selfdrive/car/interfaces.py +++ b/openpilot/sunnypilot/selfdrive/car/interfaces.py @@ -8,6 +8,7 @@ from typing import Any from opendbc.car import structs from opendbc.car.interfaces import CarInterfaceBase +from opendbc.car.toyota.values import CAR as TOYOTA_CAR from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import get_nn_model_path @@ -69,6 +70,32 @@ def _initialize_torque_lateral_control(CI: CarInterfaceBase, CP: structs.CarPara CI.configure_torque_tune(CP.carFingerprint, CP.lateralTuning) +_PRIUS_TSS2_PID_KP_BP = [1.0, 1.5, 2.0, 3.0, 5.0, 7.5, 10.0, 15.0, 30.0] +_PRIUS_TSS2_PID_KI_BP = [1.0, 1.5, 2.0, 3.0, 5.0, 7.5, 10.0, 15.0, 30.0] +_PRIUS_TSS2_PID_KP_V = [0.1304, 0.1409, 0.1357, 0.1409, 0.15, 0.1614, 0.1826, 0.2348, 0.4696] +_PRIUS_TSS2_PID_KI_V = [0.00016, 0.00035, 0.00063, 0.00141, 0.00391, 0.0088, 0.01565, 0.03522, 0.14087] +_PRIUS_TSS2_PID_KF = 4e-05 + + +def _enforce_prius_tss2_pid_lateral_control(CP: structs.CarParams, params: Params = None) -> bool: + if params is None: + params = Params() + + if CP.carFingerprint != TOYOTA_CAR.TOYOTA_PRIUS_TSS2: + return False + + return params.get_bool("ToyotaPriusTss2Pid") + + +def _initialize_prius_tss2_pid_lateral_control(CP: structs.CarParams) -> None: + CP.lateralTuning.init('pid') + CP.lateralTuning.pid.kpBP = _PRIUS_TSS2_PID_KP_BP + CP.lateralTuning.pid.kpV = _PRIUS_TSS2_PID_KP_V + CP.lateralTuning.pid.kiBP = _PRIUS_TSS2_PID_KI_BP + CP.lateralTuning.pid.kiV = _PRIUS_TSS2_PID_KI_V + CP.lateralTuning.pid.kf = _PRIUS_TSS2_PID_KF + + def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsSP, params: Params = None) -> None: if params is None: params = Params() @@ -95,8 +122,15 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS def setup_interfaces(CI: CarInterfaceBase, params: Params = None) -> None: enforce_torque = _enforce_torque_lateral_control(CI.CP, params) nnlc_enabled = _initialize_neural_network_lateral_control(CI.CP, CI.CP_SP, params) + prius_tss2_pid_enabled = _enforce_prius_tss2_pid_lateral_control(CI.CP, params) + if prius_tss2_pid_enabled: + # Prius TSS2 PID toggle takes priority over NNLC/EnforceTorqueControl for this car. + enforce_torque = False + nnlc_enabled = False _initialize_intelligent_cruise_button_management(CI.CP, CI.CP_SP, params) _initialize_torque_lateral_control(CI, CI.CP, enforce_torque, nnlc_enabled) + if prius_tss2_pid_enabled: + _initialize_prius_tss2_pid_lateral_control(CI.CP) _cleanup_unsupported_params(CI.CP, CI.CP_SP) try: @@ -132,6 +166,7 @@ def initialize_params(params) -> list[dict[str, Any]]: "ToyotaStopAndGoHack", "ToyotaEnhancedBsm", "ToyotaAutoHold", + "ToyotaPriusTss2Pid", ]) return [{k: params.get(k, return_default=True)} for k in keys] diff --git a/openpilot/sunnypilot/selfdrive/car/tests/test_prius_tss2_pid.py b/openpilot/sunnypilot/selfdrive/car/tests/test_prius_tss2_pid.py new file mode 100644 index 0000000000..b184cb62d3 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/car/tests/test_prius_tss2_pid.py @@ -0,0 +1,122 @@ +import numpy as np +import pytest +from opendbc.car import structs +from openpilot.sunnypilot.selfdrive.car import interfaces as si + + +class FakeParams: + def __init__(self, values=None): + self.values = values or {} + + def get_bool(self, key): + return bool(self.values.get(key, False)) + + def get(self, key, return_default=False): + return self.values.get(key) + + def remove(self, key): + self.values.pop(key, None) + + +class FakeCI: + def __init__(self, CP, CP_SP): + self.CP = CP + self.CP_SP = CP_SP + self.configure_torque_tune_calls = 0 + + def configure_torque_tune(self, fingerprint, tune): + self.configure_torque_tune_calls += 1 + tune.init('torque') + + +def make_prius_tss2_cp(): + CP = structs.CarParams(carFingerprint='TOYOTA_PRIUS_TSS2', steerControlType=structs.CarParams.SteerControlType.torque) + CP.lateralTuning.init('torque') + return CP + + +class TestPriusTss2PidGate: + def test_disabled_for_other_toyota_platforms(self): + CP = structs.CarParams(carFingerprint='TOYOTA_RAV4_TSS2') + assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': True})) is False + + def test_disabled_when_param_off(self): + CP = make_prius_tss2_cp() + assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': False})) is False + + def test_enabled_for_prius_tss2_with_param_on(self): + CP = make_prius_tss2_cp() + assert si._enforce_prius_tss2_pid_lateral_control(CP, FakeParams({'ToyotaPriusTss2Pid': True})) is True + + +class TestPriusTss2PidApply: + def test_flips_union_and_sets_gains(self): + CP = make_prius_tss2_cp() + assert CP.lateralTuning.which() == 'torque' + + si._initialize_prius_tss2_pid_lateral_control(CP) + + assert CP.lateralTuning.which() == 'pid' + assert list(CP.lateralTuning.pid.kpV) == pytest.approx(si._PRIUS_TSS2_PID_KP_V) + assert list(CP.lateralTuning.pid.kiV) == pytest.approx(si._PRIUS_TSS2_PID_KI_V) + assert CP.lateralTuning.pid.kf == pytest.approx(si._PRIUS_TSS2_PID_KF) + # PIDController interp needs non-empty breakpoints matching V lists. + assert len(CP.lateralTuning.pid.kpBP) == len(CP.lateralTuning.pid.kpV) + assert len(CP.lateralTuning.pid.kiBP) == len(CP.lateralTuning.pid.kiV) + + def test_kp_rises_toward_highway_not_boosted_at_low_speed(self): + """Real on-road data (route 550a71ee4c7a7fbe/00000549--01e8f2ab51) showed boosting kp below + 5 m/s increased saturation and hunting rather than helping - the shape must rise toward highway + speed, matching every other real multi-breakpoint PID car's tune (GM Volt, Cadillac Escalade + ESV, Honda Civic 2022) and the LatControlTorqueV0-derived KP_INTERP shape, not the reverse.""" + CP = make_prius_tss2_cp() + si._initialize_prius_tss2_pid_lateral_control(CP) + + kp_parking_lot = np.interp(2.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV) + kp_cruise = np.interp(5.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV) + kp_highway = np.interp(30.0, CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV) + assert kp_parking_lot < kp_cruise < kp_highway + + +class TestSetupInterfacesPrecedence: + def test_pid_toggle_wins_over_nnlc_and_enforce_torque(self): + """The Prius TSS2 PID toggle must be the last thing to touch lateralTuning: if the user also + has NNLC and/or EnforceTorqueControl on, the union must still end up 'pid' and + configure_torque_tune must never run, or the car would silently keep driving on torque.""" + CP = make_prius_tss2_cp() + CP_SP = structs.CarParamsSP() + CI = FakeCI(CP, CP_SP) + params = FakeParams({ + 'EnforceTorqueControl': True, + 'NeuralNetworkLateralControl': True, + 'ToyotaPriusTss2Pid': True, + }) + + si.setup_interfaces(CI, params) + + assert CP.lateralTuning.which() == 'pid' + assert CI.configure_torque_tune_calls == 0 + + def test_other_toyota_platform_unaffected_by_toggle(self): + """The same param being on must not leak into a different car's tuning.""" + CP = structs.CarParams(carFingerprint='TOYOTA_RAV4_TSS2', steerControlType=structs.CarParams.SteerControlType.torque) + CP.lateralTuning.init('torque') + CP_SP = structs.CarParamsSP() + CI = FakeCI(CP, CP_SP) + params = FakeParams({'ToyotaPriusTss2Pid': True}) + + si.setup_interfaces(CI, params) + + assert CP.lateralTuning.which() == 'torque' + assert CI.configure_torque_tune_calls == 0 + + def test_toggle_off_leaves_torque_control_path_intact(self): + CP = make_prius_tss2_cp() + CP_SP = structs.CarParamsSP() + CI = FakeCI(CP, CP_SP) + params = FakeParams({'EnforceTorqueControl': True, 'ToyotaPriusTss2Pid': False}) + + si.setup_interfaces(CI, params) + + assert CP.lateralTuning.which() == 'torque' + assert CI.configure_torque_tune_calls == 1 diff --git a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py index 33cc9e3ad8..dae9ab970b 100644 --- a/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py +++ b/openpilot/sunnypilot/selfdrive/controls/controlsd_ext.py @@ -10,12 +10,14 @@ import openpilot.cereal.messaging as messaging from openpilot.cereal import log, custom from opendbc.car import structs +from opendbc.car.toyota.values import CAR as TOYOTA_CAR from openpilot.common.params import Params from openpilot.common.swaglog import cloudlog from openpilot.sunnypilot import PARAMS_UPDATE_PERIOD from openpilot.sunnypilot.livedelay.helpers import get_lat_delay from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase from openpilot.sunnypilot.selfdrive.controls.lib.blinker_pause_lateral import BlinkerPauseLateral +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import LatControlPidSmooth from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import LatControlTorque as LatControlTorqueV0 @@ -35,12 +37,15 @@ class ControlsExt(ModelStateBase): self.pm_services_ext = ['carControlSP'] def initialize_lateral_control(self, lac, CI, dt): + if self.CP.lateralTuning.which() != 'torque': + if self.CP.carFingerprint == TOYOTA_CAR.TOYOTA_PRIUS_TSS2 and self.CP.lateralTuning.which() == 'pid': + return LatControlPidSmooth(self.CP, self.CP_SP, CI, dt) + return lac + enforce_torque_control = self.params.get_bool("EnforceTorqueControl") torque_versions = self.params.get("TorqueControlTune") if not enforce_torque_control: - if self.CP.lateralTuning.which() == 'torque': - return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt) # FIXME-SP: revert when upstream fixes tuning issues with v1 - return lac + return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt) # FIXME-SP: revert when upstream fixes tuning issues with v1 if torque_versions == 0.0: # v0 return LatControlTorqueV0(self.CP, self.CP_SP, CI, dt) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_pid_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_pid_ext.py new file mode 100644 index 0000000000..e5cc369da0 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_pid_ext.py @@ -0,0 +1,32 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import math + +from openpilot.common.pid import PIDController +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID + +DECAY_TAU = 2.0 # seconds; starting guess, not validated against a real car + + +class DecayingIntegratorPIDController(PIDController): + def __init__(self, *args, decay_tau=DECAY_TAU, **kwargs): + super().__init__(*args, **kwargs) + self.decay_tau = decay_tau + + def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False): + if freeze_integrator: + self.i *= math.exp(-self.i_dt / self.decay_tau) + return super().update(error, error_rate=error_rate, speed=speed, feedforward=feedforward, freeze_integrator=freeze_integrator) + + +class LatControlPidSmooth(LatControlPID): + def __init__(self, CP, CP_SP, CI, dt, decay_tau=DECAY_TAU): + super().__init__(CP, CP_SP, CI, dt) + self.pid = DecayingIntegratorPIDController( + (CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), + (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), + pos_limit=self.steer_max, neg_limit=-self.steer_max, decay_tau=decay_tau) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_pid_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_pid_ext.py new file mode 100644 index 0000000000..92ef72b310 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_pid_ext.py @@ -0,0 +1,73 @@ +import math + +import pytest + +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import DecayingIntegratorPIDController + +RATE = 100 +DT = 1.0 / RATE + + +def build_pid(decay_tau=2.0): + return DecayingIntegratorPIDController(0.05, 0.05, pos_limit=1.0, neg_limit=-1.0, rate=RATE, decay_tau=decay_tau) + + +class TestDecayingIntegratorPIDController: + def test_accumulates_normally_when_not_frozen(self): + """Unfrozen behavior must be identical to stock PIDController - only freeze behavior changes.""" + pid = build_pid() + for _ in range(50): + pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False) + assert pid.i > 0 + + def test_decays_toward_zero_while_frozen(self): + pid = build_pid(decay_tau=2.0) + for _ in range(200): # 2s build-up, well below saturation so anti-windup doesn't clip i + pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False) + i_before = pid.i + assert i_before > 0 + + i_trace = [] + for _ in range(600): # 6s frozen = 3 time constants + pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=True) + i_trace.append(pid.i) + + # monotonic decay toward zero, never grows, never flips sign + assert all(0 <= i_trace[k + 1] <= i_trace[k] for k in range(len(i_trace) - 1)) + assert i_trace[-1] < 0.05 * i_before, "should be mostly decayed after 3 time constants" + + def test_matches_exponential_decay_time_constant(self): + """Sanity-checks the decay is a real exp(-t/tau), not just 'decreasing'.""" + pid = build_pid(decay_tau=2.0) + pid.i = 1.0 + + for _ in range(200): # exactly one time constant (2s @ 100Hz) + pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=True) + + assert pid.i == pytest.approx(math.exp(-1.0), rel=1e-3) + + def test_no_discontinuity_at_freeze_transition(self): + """The whole point: control output must not jump the instant freeze conditions engage.""" + pid = build_pid(decay_tau=2.0) + for _ in range(200): + pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False) + control_before = pid.control + + control_after = pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=True) + + assert abs(control_after - control_before) < 0.01, "output jumped at the freeze transition" + + def test_stale_integral_does_not_kick_back_in_on_unfreeze(self): + """The bug this exists to fix: after a long freeze, unfreezing must not suddenly reapply a + large stale integral untouched for however long the freeze lasted.""" + pid = build_pid(decay_tau=2.0) + for _ in range(200): + pid.update(error=1.0, speed=1.0, feedforward=0.0, freeze_integrator=False) + i_peak = pid.i + + for _ in range(1000): # 10s frozen, ~5 time constants + pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=True) + + # unfreeze: the resumed integral must be near zero, not the stale peak + pid.update(error=0.0, speed=1.0, feedforward=0.0, freeze_integrator=False) + assert abs(pid.i) < 0.01 * i_peak diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_prius_tss2_pid_closed_loop.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_prius_tss2_pid_closed_loop.py new file mode 100644 index 0000000000..e0fb5f6b17 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_prius_tss2_pid_closed_loop.py @@ -0,0 +1,159 @@ +""" +Closed-loop smoke test for the Prius TSS2 PID lateral-control toggle's starting gains +(openpilot/sunnypilot/selfdrive/car/interfaces.py::_PRIUS_TSS2_PID_*). + +IMPORTANT LIMITATION: there is no real Prius TSS2 EPS actuator model anywhere in this repo (unlike +the longitudinal plant model used by test_accel_controller_closed_loop.py, which was fit to logged +routes). The actuator here is a generic, uncalibrated 2nd-order lag (see `SurrogateEpsActuator`) — +it stands in for "some steering rack with plausible bandwidth," not this specific car's real EPS. + +This test can only prove the starting gains are stable and roughly critically damped against that +generic surrogate. It CANNOT prove they are correctly tuned for a real Prius TSS2 — that requires +on-road A/B via tools/lateral_maneuvers (see its README) before trusting this tune on its own. + +SCOPE: cruise-speed (20-30mph) only. The surrogate's steady-state gain is K = 1/(kf*v_ego**2) (see +`SurrogateEpsActuator`), which blows up as v_ego -> 0 and produces meaningless multi-hundred-degree +oscillation at parking-lot speed — an artifact of the surrogate, not of the kp/ki tune. This mirrors +a real constraint: angle*v_ego**2 feedforward (and this kf calibration) is explicitly a higher-speed +approximation (see the "25+mph" comment in latcontrol_torque_v0.py) — there's no valid basis here to +simulate the low-speed "sharp turn" boost in _PRIUS_TSS2_PID_KP_BP/_V at all. That boost is only +covered by the static shape check in test_prius_tss2_pid.py +(test_kp_is_boosted_below_integrator_freeze_speed) — it has NOT been closed-loop or on-road +verified. Validate it in a parking lot before trusting it anywhere faster. +""" +import math + +import numpy as np +import pytest + +from opendbc.car import DT_CTRL +from opendbc.car.car_helpers import interfaces as car_interfaces +from opendbc.car.vehicle_model import VehicleModel +from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID +from openpilot.sunnypilot.selfdrive.car.interfaces import _initialize_prius_tss2_pid_lateral_control + +MPH_TO_MS = 0.44704 +DURATION_S = 6.0 +STEADY_WINDOW_S = 1.0 +TARGET_LAT_ACCEL = 1.5 # m/s^2, roughly a lateral_maneuvers "step" size + + +class FakeCarState: + def __init__(self, v_ego): + self.vEgo = v_ego + self.steeringAngleDeg = 0.0 + self.steeringRateDeg = 0.0 + self.steeringPressed = False + + +class FakeLiveParams: + roll = 0.0 + angleOffsetDeg = 0.0 + + +class SurrogateEpsActuator: + """Generic critically-damped 2nd-order torque->angle lag. NOT fit to any real car. + + The steady-state gain (deg per unit torque) is derived from the tune's own `kf`, i.e. + K = 1 / (kf * v_ego**2) — the same steady-state relationship LatControlPID's feedforward term + assumes (ff = kf * angle_deg * v_ego**2 ~= torque needed to hold that angle). A fixed, unrelated + gain guess saturated the actuator well below the test's target angle at 20-30mph — this ties the + surrogate to the one steady-state assumption already baked into the tune, so the test only + exercises kp/ki dynamic response and stability, not an arbitrary extra unknown. + """ + + def __init__(self, deg_per_unit_torque, natural_freq_hz=3.0, zeta=1.0): + self.wn = 2 * math.pi * natural_freq_hz + self.zeta = zeta + self.k = deg_per_unit_torque + self.angle = 0.0 + self.rate = 0.0 + + def step(self, torque, dt): + accel = self.wn ** 2 * (self.k * torque - self.angle) - 2 * self.zeta * self.wn * self.rate + self.rate += accel * dt + self.angle += self.rate * dt + return self.angle, self.rate + + +def run_closed_loop(CP, v_ego, target_lat_accel, duration_s=DURATION_S): + VM = VehicleModel(CP) + lac = LatControlPID(CP, structs_car_params_sp(), FakeCI(), DT_CTRL) + deg_per_unit_torque = 1.0 / (CP.lateralTuning.pid.kf * v_ego ** 2) + actuator = SurrogateEpsActuator(deg_per_unit_torque) + CS = FakeCarState(v_ego) + params = FakeLiveParams() + + desired_curvature = -target_lat_accel / v_ego ** 2 + desired_angle_deg = math.degrees(VM.get_steer_from_curvature(-desired_curvature, v_ego, 0.0)) + + n_steps = int(duration_s / DT_CTRL) + angle_trace = np.zeros(n_steps) + torque_trace = np.zeros(n_steps) + + for i in range(n_steps): + output_torque, _, _ = lac.update(True, CS, VM, params, False, desired_curvature, None, False, 0.0) + output_torque = float(output_torque) + angle, rate = actuator.step(output_torque, DT_CTRL) + CS.steeringAngleDeg = float(angle) + CS.steeringRateDeg = float(rate) + angle_trace[i] = angle + torque_trace[i] = output_torque + + return angle_trace, torque_trace, desired_angle_deg + + +def structs_car_params_sp(): + from opendbc.car import structs + return structs.CarParamsSP() + + +class FakeCI: + @staticmethod + def get_steer_feedforward_function(): + return lambda desired_angle, v_ego: desired_angle * (v_ego ** 2) + + +def make_prius_tss2_cp(): + CarInterface = car_interfaces['TOYOTA_PRIUS_TSS2'] + CP = CarInterface.get_params('TOYOTA_PRIUS_TSS2', {0: {}, 1: {}, 2: {}}, [], alpha_long=False, is_release=False, docs=False) + _initialize_prius_tss2_pid_lateral_control(CP) + assert CP.lateralTuning.which() == 'pid' + return CP + + +@pytest.mark.parametrize('v_mph', [20.0, 30.0]) +def test_starting_gains_settle_without_diverging(v_mph): + CP = make_prius_tss2_cp() + v_ego = v_mph * MPH_TO_MS + + angle_trace, torque_trace, desired_angle_deg = run_closed_loop(CP, v_ego, TARGET_LAT_ACCEL) + + assert np.all(np.isfinite(angle_trace)), "diverged/NaN — unsafe to ever test on-road" + assert np.all(np.abs(torque_trace) <= 1.0 + 1e-6), "output_torque exceeded steer_max=1.0 saturation bound" + + steady_n = int(STEADY_WINDOW_S / DT_CTRL) + steady_angle = angle_trace[-steady_n:] + settle_error_deg = abs(np.mean(steady_angle) - desired_angle_deg) + oscillation_deg = np.ptp(steady_angle) + + assert settle_error_deg < 1.0, f"steady-state tracking error too large: {settle_error_deg:.3f} deg (target {desired_angle_deg:.2f} deg)" + assert oscillation_deg < 0.5, f"sustained oscillation in tail window: {oscillation_deg:.3f} deg peak-to-peak (limit-cycle candidate)" + + +def test_gains_are_not_a_no_op_sanity_check(): + """Confirms this harness actually has teeth: gains far more aggressive than the shipped starting + point produce a limit cycle against the same surrogate actuator, so the tolerances above aren't + trivially satisfied by any input.""" + CP = make_prius_tss2_cp() + CP.lateralTuning.pid.kpBP = [0.0] + CP.lateralTuning.pid.kpV = [1.5] # 10x the cruise-speed kp + CP.lateralTuning.pid.kiBP = [0.0] + CP.lateralTuning.pid.kiV = [0.5] # 10x the shipped ki + v_ego = 20.0 * MPH_TO_MS + + angle_trace, _, desired_angle_deg = run_closed_loop(CP, v_ego, TARGET_LAT_ACCEL) + + steady_n = int(STEADY_WINDOW_S / DT_CTRL) + oscillation_deg = np.ptp(angle_trace[-steady_n:]) + assert oscillation_deg > 0.5, "expected an aggressive 10x-gain tune to visibly ring against this actuator; harness may not be sensitive" diff --git a/openpilot/sunnypilot/selfdrive/controls/tests/__init__.py b/openpilot/sunnypilot/selfdrive/controls/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/openpilot/sunnypilot/selfdrive/controls/tests/test_controlsd_ext.py b/openpilot/sunnypilot/selfdrive/controls/tests/test_controlsd_ext.py new file mode 100644 index 0000000000..1b946322db --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/tests/test_controlsd_ext.py @@ -0,0 +1,109 @@ +from opendbc.car import structs + +from openpilot.sunnypilot.selfdrive.controls import controlsd_ext +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_pid_ext import LatControlPidSmooth + + +class FakeParams: + def __init__(self, values=None): + self.values = values or {} + + def get_bool(self, key): + return bool(self.values.get(key, False)) + + def get(self, key, return_default=False): + return self.values.get(key) + + +class FakeCI: + def get_steer_feedforward_function(self): + return lambda desired_angle, v_ego: desired_angle * (v_ego ** 2) + + +def make_ext(CP, params_values=None): + # Bypass __init__: it blocks on CarParamsSP over messaging, which isn't available in a unit test. + ext = controlsd_ext.ControlsExt.__new__(controlsd_ext.ControlsExt) + ext.CP = CP + ext.CP_SP = structs.CarParamsSP() + ext.params = FakeParams(params_values) + return ext + + +def make_prius_tss2_pid_cp(): + CP = structs.CarParams(carFingerprint='TOYOTA_PRIUS_TSS2') + CP.lateralTuning.init('pid') + CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV = [0.0, 5.0], [0.30, 0.15] + CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV = [0.0], [0.05] + CP.lateralTuning.pid.kf = 4e-05 + return CP + + +class TestInitializeLateralControlPidSmoothDispatch: + """The Prius TSS2 PID toggle's decaying-integrator variant must be scoped to exactly the one + (fingerprint, union) combination it applies to - never touch any other PID car's controller.""" + + def test_prius_tss2_pid_gets_smooth_variant(self): + ext = make_ext(make_prius_tss2_pid_cp()) + lac = object() + + result = ext.initialize_lateral_control(lac, FakeCI(), 0.01) + + assert isinstance(result, LatControlPidSmooth) + + def test_other_native_pid_car_is_untouched(self): + """A hypothetical other brand's native PID car must NOT get swapped to our variant just + because the union happens to be 'pid' - only our exact fingerprint qualifies.""" + CP = structs.CarParams(carFingerprint='SOME_OTHER_PID_CAR') + CP.lateralTuning.init('pid') + ext = make_ext(CP) + lac = object() + + result = ext.initialize_lateral_control(lac, FakeCI(), 0.01) + + assert result is lac + + +class TestInitializeLateralControlPidGuard: + """Regression test for the crash this toggle would otherwise cause: torque-only LatControl + variants read CP.lateralTuning.torque directly, which raises on a capnp union that's actually + 'pid' (e.g. the Prius TSS2 PID toggle). initialize_lateral_control must never attempt that.""" + + def test_pid_union_returns_lac_unchanged_even_with_enforce_torque_on(self): + CP = structs.CarParams() + CP.lateralTuning.init('pid') + ext = make_ext(CP, {'EnforceTorqueControl': True, 'TorqueControlTune': 0.0}) + + lac = object() + result = ext.initialize_lateral_control(lac, CI=None, dt=0.01) + + assert result is lac + + def test_pid_union_returns_lac_unchanged_with_enforce_torque_off(self): + CP = structs.CarParams() + CP.lateralTuning.init('pid') + ext = make_ext(CP, {'EnforceTorqueControl': False}) + + lac = object() + result = ext.initialize_lateral_control(lac, CI=None, dt=0.01) + + assert result is lac + + def test_torque_union_still_dispatches_to_torque_v0(self, monkeypatch): + calls = [] + + class StubTorqueV0: + def __init__(self, CP, CP_SP, CI, dt): + calls.append((CP, CP_SP, CI, dt)) + + monkeypatch.setattr(controlsd_ext, 'LatControlTorqueV0', StubTorqueV0) + + CP = structs.CarParams() + CP.lateralTuning.init('torque') + ext = make_ext(CP, {'EnforceTorqueControl': False}) + ext.CP_SP = None + + lac = object() + result = ext.initialize_lateral_control(lac, CI=None, dt=0.01) + + assert isinstance(result, StubTorqueV0) + assert len(calls) == 1 diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index 9095984c82..7732b709b0 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -2294,6 +2294,18 @@ } ] }, + { + "key": "ToyotaPriusTss2Pid", + "widget": "toggle", + "needs_onroad_cycle": true, + "title": "Toyota: Prius TSS2 PID Lateral Control (Alpha)", + "description": "Use a PID lateral controller instead of torque control on Prius TSS2. Overrides Neural Network Lateral Control and Enforce Torque Control for this car. Starting gains are unvalidated on a real Prius TSS2 \u2014 expect to need on-road tuning. Use at your own risk.", + "enablement": [ + { + "type": "not_engaged" + } + ] + }, { "key": "ToyotaTSS2Long", "widget": "toggle", diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml index e9b73a1976..a3d1d1c65a 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/vehicle.yaml @@ -94,6 +94,15 @@ sections: title: 'Toyota: Prius TSS2 BSM and some tssp' enablement: - $ref: '#/macros/not_engaged' + - key: ToyotaPriusTss2Pid + widget: toggle + needs_onroad_cycle: true + title: 'Toyota: Prius TSS2 PID Lateral Control (Alpha)' + description: Use a PID lateral controller instead of torque control on Prius TSS2. Overrides Neural + Network Lateral Control and Enforce Torque Control for this car. Starting gains are unvalidated on + a real Prius TSS2 — expect to need on-road tuning. Use at your own risk. + enablement: + - $ref: '#/macros/not_engaged' - key: ToyotaTSS2Long widget: toggle needs_onroad_cycle: true