pid for pri

openpilot/common/params_keys.h

@@ -191,6 +191,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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]

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

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)

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)

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

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"

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

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",

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