Merge branch 'master' of https://github.com/infiniteCable2/openpilot

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

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)

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)

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)