diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py index f7874cc539..d33cd52b33 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_v0.py +++ b/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) diff --git a/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_v0.py b/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_v0.py new file mode 100644 index 0000000000..f297a9890a --- /dev/null +++ b/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)