From 7de5c17290ad537a4fbec59987822f3364ae8e98 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Mon, 31 Aug 2026 18:25:22 +0200 Subject: [PATCH] Slow Soft Touch Unwind (#26) * slightly press * refactoring * like upstream custom carstate --- openpilot/common/pid.py | 29 +++--- openpilot/selfdrive/controls/controlsd.py | 4 +- .../controls/lib/latcontrol_curvature.py | 17 +++- .../tests/test_latcontrol_curvature.py | 90 +++++++++++++++++++ 4 files changed, 124 insertions(+), 16 deletions(-) create mode 100644 openpilot/selfdrive/controls/tests/test_latcontrol_curvature.py diff --git a/openpilot/common/pid.py b/openpilot/common/pid.py index f2765fdd7..e1027bf53 100644 --- a/openpilot/common/pid.py +++ b/openpilot/common/pid.py @@ -60,8 +60,10 @@ class PIDController: class MultiplicativeUnwindPID: + DEFAULT_UNWIND_TIME = 0.1 # seconds + def __init__(self, k_p: Gain, k_i: Gain, k_f=0., k_d: Gain = 0., pos_limit=1e308, neg_limit=-1e308, - rate=100, min_cmd=1e-10, ki_red_time=1.0): + rate=100, min_cmd=1e-10, unwind_time=DEFAULT_UNWIND_TIME): self._k_p = ([0], [k_p]) if isinstance(k_p, (int, float)) else k_p self._k_i = ([0], [k_i]) if isinstance(k_i, (int, float)) else k_i self._k_d = ([0], [k_d]) if isinstance(k_d, (int, float)) else k_d @@ -71,10 +73,8 @@ class MultiplicativeUnwindPID: self.rate = float(rate) self.i_dt = 1.0 / rate self.min_cmd = abs(min_cmd) - self.ki_red_time = float(ki_red_time) - self.override_prev = False - self.i_unwind_factor = 1.0 self.speed = 0.0 + self.set_unwind_time(unwind_time) self.reset() @property @@ -95,17 +95,22 @@ class MultiplicativeUnwindPID: self.d = 0.0 self.f = 0.0 self.control = 0 + self.unwind_time_prev: float | None = None + self.i_unwind_factor = 1.0 - def _calc_unwind_factor(self, override): - if not override or self.override_prev: - return - if self.ki_red_time <= 0.0: - self.i_unwind_factor = 1.0 + def set_unwind_time(self, unwind_time: float) -> None: + if unwind_time <= 0.0: + raise ValueError("unwind_time must be greater than zero") + self.unwind_time = float(unwind_time) + + def _calc_unwind_factor(self): + # Recalculate when unwinding starts or the requested duration changes. + if self.unwind_time == self.unwind_time_prev: return if abs(self.i) <= self.min_cmd: self.i_unwind_factor = 0.0 return - steps = max(int(self.ki_red_time * self.rate), 1) + steps = max(int(self.unwind_time * self.rate), 1) factor = (self.min_cmd / abs(self.i)) ** (1.0 / steps) self.i_unwind_factor = min(factor, 1.0) @@ -117,7 +122,7 @@ class MultiplicativeUnwindPID: self.d = error_rate * self.k_d if override: - self._calc_unwind_factor(override) + self._calc_unwind_factor() self.i *= self.i_unwind_factor if abs(self.i) < self.min_cmd: self.i = 0.0 @@ -133,5 +138,5 @@ class MultiplicativeUnwindPID: control = self.p + self.i + self.d + self.f self.control = np.clip(control, self.neg_limit, self.pos_limit) - self.override_prev = override + self.unwind_time_prev = self.unwind_time if override else None return self.control diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index f3a5ed831..a7f3d539a 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -51,7 +51,7 @@ class Controls(ControlsExt): self.CI = interfaces[self.CP.carFingerprint](self.CP, self.CP_SP, self.CP_IC) - ic_sm_services = ['lateralCurvatureParameters', 'longitudinalPlanIC'] + ic_sm_services = ['carStateIC', 'lateralCurvatureParameters', 'longitudinalPlanIC'] ic_pm_services = ['carControlIC', 'controlsStateIC'] self.sm = messaging.SubMaster(['lateralDelay', 'vehicleParameters', 'lateralTorqueParameters', 'modelV2', 'selfdriveState', 'extrinsicsCalibration', 'deviceMotion', 'longitudinalPlan', 'lateralManeuverPlan', 'carState', 'carOutput', @@ -120,6 +120,7 @@ class Controls(ControlsExt): def state_control(self): CS = self.sm['carState'] + CS_IC = self.sm['carStateIC'] # Update VehicleModel lp = self.sm['vehicleParameters'] @@ -197,6 +198,7 @@ class Controls(ControlsExt): if self.enable_smooth_steer: new_desired_curvature = self.smooth_steer.update(new_desired_curvature) if self.CP.steerControlType == car.CarParams.SteerControlType.curvature: + self.LaC.set_steering_slightly_pressed(CS_IC.steeringSlightlyPressed) # CurvatureD correction routed as additive term on the controller output (not setpoint shift) if CC.latActive and self.enable_curvatured and self.sm.all_checks(['lateralCurvatureParameters']): correction = self.curvatured.get_correction(self.desired_curvature, CS.vEgo) diff --git a/openpilot/selfdrive/controls/lib/latcontrol_curvature.py b/openpilot/selfdrive/controls/lib/latcontrol_curvature.py index 694203999..9524f0ec8 100644 --- a/openpilot/selfdrive/controls/lib/latcontrol_curvature.py +++ b/openpilot/selfdrive/controls/lib/latcontrol_curvature.py @@ -6,6 +6,8 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE LAT_ACCEL_SATURATION_THRESHOLD = 0.4 # m/s^2 +STEERING_OVERRIDE_UNWIND_TIME = MultiplicativeUnwindPID.DEFAULT_UNWIND_TIME +SLIGHT_STEERING_OVERRIDE_UNWIND_TIME = 10.0 # seconds class LatControlCurvature(LatControl): @@ -14,12 +16,13 @@ class LatControlCurvature(LatControl): self.sat_check_min_speed = 5. self.enable_pid = False self.curvature_correction = 0.0 + self.steering_slightly_pressed = False if CP.lateralTuning.which() == 'pid': ct = CP.lateralTuning.pid self.pid = MultiplicativeUnwindPID((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV), k_f=ct.kf, pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, - rate=1 / dt, min_cmd=1e-6, ki_red_time=0.1) + rate=1 / dt, min_cmd=1e-6) self.kf = ct.kf else: self.pid = None @@ -31,6 +34,9 @@ class LatControlCurvature(LatControl): def set_curvature_correction(self, correction: float) -> None: self.curvature_correction = correction + def set_steering_slightly_pressed(self, pressed: bool) -> None: + self.steering_slightly_pressed = pressed + def reset(self): super().reset() if self.pid is not None: @@ -55,9 +61,14 @@ class LatControlCurvature(LatControl): curvature_log.active = True else: freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 + if self.steering_slightly_pressed and not CS.steeringPressed: + unwind_time = SLIGHT_STEERING_OVERRIDE_UNWIND_TIME + else: + unwind_time = STEERING_OVERRIDE_UNWIND_TIME + self.pid.set_unwind_time(unwind_time) output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, - override=CS.steeringPressed, - freeze_integrator=freeze_integrator) + freeze_integrator=freeze_integrator, + override=CS.steeringPressed or self.steering_slightly_pressed) curvature_log.p = float(self.pid.p) curvature_log.i = float(self.pid.i) curvature_log.f = float(self.pid.f) diff --git a/openpilot/selfdrive/controls/tests/test_latcontrol_curvature.py b/openpilot/selfdrive/controls/tests/test_latcontrol_curvature.py new file mode 100644 index 000000000..9f4a44fa4 --- /dev/null +++ b/openpilot/selfdrive/controls/tests/test_latcontrol_curvature.py @@ -0,0 +1,90 @@ +from types import SimpleNamespace + +import numpy as np + +from openpilot.selfdrive.controls.lib.latcontrol_curvature import (SLIGHT_STEERING_OVERRIDE_UNWIND_TIME, + STEERING_OVERRIDE_UNWIND_TIME, LatControlCurvature) + + +DT = 0.01 +INITIAL_I = 1e-3 +MIN_I = 1e-6 + + +class DummyLateralTuning: + kpBP = [0.0] + kpV = [0.0] + kiBP = [0.0] + kiV = [0.0] + kf = 0.0 + + @staticmethod + def which(): + return 'pid' + + @property + def pid(self): + return self + + +class DummyVehicleModel: + @staticmethod + def roll_compensation(roll, v_ego): + return 0.0 + + @staticmethod + def calc_curvature(steering_angle, v_ego, roll): + return 0.0 + + +def make_controller(): + CP = SimpleNamespace(steerLimitTimer=1.0, lateralTuning=DummyLateralTuning()) + controller = LatControlCurvature(CP, None, None, DT) + controller.set_pid_enabled(True) + controller.pid.i = INITIAL_I + return controller + + +def update_controller(controller, steering_pressed=False): + CS = SimpleNamespace(vEgo=20.0, steeringAngleDeg=0.0, steeringPressed=steering_pressed) + params = SimpleNamespace(roll=0.0, angleOffsetDeg=0.0) + controller.update(True, CS, DummyVehicleModel(), params, False, 0.0, None, False, 0.0) + + +def test_slight_steering_override_unwinds_integrator_over_ten_seconds(): + controller = make_controller() + controller.set_steering_slightly_pressed(True) + + half_steps = round(SLIGHT_STEERING_OVERRIDE_UNWIND_TIME / DT / 2) + for _ in range(half_steps): + update_controller(controller) + + assert np.isclose(controller.pid.i, np.sqrt(INITIAL_I * MIN_I), rtol=1e-6) + + for _ in range(half_steps): + update_controller(controller) + + assert abs(controller.pid.i) <= MIN_I * (1.0 + 1e-12) + + +def test_full_steering_override_keeps_fast_unwind(): + controller = make_controller() + controller.set_steering_slightly_pressed(True) + + for _ in range(round(1.0 / DT)): + update_controller(controller) + assert controller.pid.i > MIN_I + + for _ in range(round(STEERING_OVERRIDE_UNWIND_TIME / DT)): + update_controller(controller, steering_pressed=True) + + assert abs(controller.pid.i) <= MIN_I * (1.0 + 1e-12) + + +def test_override_uses_default_fast_unwind_time(): + controller = make_controller() + + for _ in range(round(STEERING_OVERRIDE_UNWIND_TIME / DT)): + controller.pid.update(0.0, override=True) + + assert abs(controller.pid.i) <= MIN_I * (1.0 + 1e-12)