pri lat toruqe

sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py

@@ -33,6 +33,23 @@ LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
 FRICTION_THRESHOLD = 0.3
 VERSION = 0
 
+PRIUS_TSS2 = "TOYOTA_PRIUS_TSS2"
+PRIUS_KP = 0.8
+PRIUS_KI = 0.15
+PRIUS_LAT_ACCEL_FACTOR = 1.65
+PRIUS_LAT_ACCEL_OFFSET = -0.25
+PRIUS_FRICTION = 0.168
+PRIUS_TORQUE_RATE_UP = 1.0
+PRIUS_TORQUE_RATE_DOWN = 5.0 / 3.0
+PRIUS_UNWIND_I_DECAY = 0.99
+PRIUS_UNWIND_JERK_THRESHOLD = 0.1
+
+
+def limit_torque_rate(desired_torque, last_torque, rate_up, rate_down, dt):
+  increasing_magnitude = desired_torque * last_torque >= 0.0 and abs(desired_torque) > abs(last_torque)
+  max_delta = (rate_up if increasing_magnitude else rate_down) * dt
+  return float(np.clip(desired_torque, last_torque - max_delta, last_torque + max_delta))
+
 
 class LatControlTorque(LatControl):
   def __init__(self, CP, CP_SP, CI, dt):
@@ -40,8 +57,19 @@ class LatControlTorque(LatControl):
     self.torque_params = CP.lateralTuning.torque.as_builder()
     self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
     self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
-    self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt)
+    self.prius_smooth_tune = CP.carFingerprint == PRIUS_TSS2
+    if self.prius_smooth_tune:
+      self._set_prius_torque_params()
+
+    kp_interp = KP_INTERP.copy()
+    ki = KI
+    if self.prius_smooth_tune:
+      kp_interp[-1] = PRIUS_KP
+      ki = PRIUS_KI
+
+    self.pid = PIDController([INTERP_SPEEDS, kp_interp], ki, KD, rate=1/self.dt)
     self.update_limits()
+    self.output_torque_last = 0.0
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
     self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
     self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
@@ -50,10 +78,18 @@ class LatControlTorque(LatControl):
 
     self.extension = LatControlTorqueExt(self, CP, CP_SP, CI)
 
+  def _set_prius_torque_params(self):
+    self.torque_params.latAccelFactor = PRIUS_LAT_ACCEL_FACTOR
+    self.torque_params.latAccelOffset = PRIUS_LAT_ACCEL_OFFSET
+    self.torque_params.friction = PRIUS_FRICTION
+
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
-    self.torque_params.latAccelFactor = latAccelFactor
-    self.torque_params.latAccelOffset = latAccelOffset
-    self.torque_params.friction = friction
+    if self.prius_smooth_tune:
+      self._set_prius_torque_params()
+    else:
+      self.torque_params.latAccelFactor = latAccelFactor
+      self.torque_params.latAccelOffset = latAccelOffset
+      self.torque_params.friction = friction
     self.update_limits()
 
   def update_limits(self):
@@ -63,12 +99,15 @@ class LatControlTorque(LatControl):
   def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay):
     # Override torque params from extension
     if self.extension.update_override_torque_params(self.torque_params):
+      if self.prius_smooth_tune:
+        self._set_prius_torque_params()
       self.update_limits()
 
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     pid_log.version = VERSION
     if not active:
       output_torque = 0.0
+      self.output_torque_last = 0.0
       pid_log.active = False
     else:
       measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
@@ -99,7 +138,12 @@ class LatControlTorque(LatControl):
       # TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
       ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
-      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
+      unwinding = ((abs(desired_lateral_jerk) > PRIUS_UNWIND_JERK_THRESHOLD and setpoint * desired_lateral_jerk <= 0.0) or
+                   setpoint * measurement < 0.0)
+      if self.prius_smooth_tune and unwinding:
+        self.pid.i *= PRIUS_UNWIND_I_DECAY
+
+      freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 or (self.prius_smooth_tune and unwinding)
       output_lataccel = self.pid.update(pid_log.error,
                                        -measurement_rate,
                                         feedforward=ff,
@@ -113,6 +157,10 @@ class LatControlTorque(LatControl):
                                                      future_desired_lateral_accel, measurement, lateral_accel_deadzone, gravity_adjusted_future_lateral_accel,
                                                      desired_curvature, measured_curvature, steer_limited_by_safety, output_torque)
 
+      if self.prius_smooth_tune:
+        output_torque = limit_torque_rate(output_torque, self.output_torque_last, PRIUS_TORQUE_RATE_UP, PRIUS_TORQUE_RATE_DOWN, self.dt)
+      self.output_torque_last = output_torque
+
       pid_log.active = True
       pid_log.p = float(self.pid.p)
       pid_log.i = float(self.pid.i)

sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_v0.py

@@ -0,0 +1,76 @@
+import pytest
+
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.toyota.values import CAR as TOYOTA
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.car.helpers import convert_to_capnp
+from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces
+from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_v0 import (
+  KI,
+  KP,
+  PRIUS_FRICTION,
+  PRIUS_KI,
+  PRIUS_KP,
+  PRIUS_LAT_ACCEL_FACTOR,
+  PRIUS_LAT_ACCEL_OFFSET,
+  LatControlTorque,
+  limit_torque_rate,
+)
+
+
+DT = 0.01
+RATE_UP = 1.0
+RATE_DOWN = 5.0 / 3.0
+
+
+def get_controller(car_name):
+  CarInterface = interfaces[car_name]
+  CP = CarInterface.get_non_essential_params(car_name)
+  CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name)
+  CI = CarInterface(CP, CP_SP)
+  sunnypilot_interfaces.setup_interfaces(CI)
+  return LatControlTorque(CP.as_reader(), convert_to_capnp(CP_SP).as_reader(), CI, DT_CTRL)
+
+
+@pytest.mark.parametrize("target", [-1.0, 1.0])
+def test_torque_rate_limit_windup(target):
+  last = 0.0
+  for _ in range(25):
+    output = limit_torque_rate(target, last, RATE_UP, RATE_DOWN, DT)
+    assert abs(output - last) <= RATE_UP * DT + 1e-9
+    last = output
+
+
+@pytest.mark.parametrize("initial", [-0.5, 0.5])
+def test_torque_rate_limit_unwind(initial):
+  output = limit_torque_rate(0.0, initial, RATE_UP, RATE_DOWN, DT)
+  assert abs(output - initial) == pytest.approx(RATE_DOWN * DT)
+  assert abs(output) < abs(initial)
+
+
+def test_torque_rate_limit_reversal():
+  last = 0.2
+  outputs = []
+  for _ in range(20):
+    last = limit_torque_rate(-1.0, last, RATE_UP, RATE_DOWN, DT)
+    outputs.append(last)
+
+  assert all(abs(current - previous) <= RATE_DOWN * DT + 1e-9 for previous, current in zip([0.2] + outputs[:-1], outputs, strict=True))
+  assert outputs[-1] < 0.0
+
+
+def test_prius_tune_is_stable_and_scoped():
+  prius = get_controller(TOYOTA.TOYOTA_PRIUS_TSS2)
+  assert prius.prius_smooth_tune
+  assert prius.pid._k_p[1][-1] == pytest.approx(PRIUS_KP)
+  assert prius.pid._k_i[1][-1] == pytest.approx(PRIUS_KI)
+
+  prius.update_live_torque_params(1.2, -0.5, 0.3)
+  assert prius.torque_params.latAccelFactor == pytest.approx(PRIUS_LAT_ACCEL_FACTOR)
+  assert prius.torque_params.latAccelOffset == pytest.approx(PRIUS_LAT_ACCEL_OFFSET)
+  assert prius.torque_params.friction == pytest.approx(PRIUS_FRICTION)
+
+  rav4 = get_controller(TOYOTA.TOYOTA_RAV4_TSS2)
+  assert not rav4.prius_smooth_tune
+  assert rav4.pid._k_p[1][-1] == pytest.approx(KP)
+  assert rav4.pid._k_i[1][-1] == pytest.approx(KI)