Lateral torque controller: use current desired curvature for PI error

common/pid.py

@@ -16,7 +16,7 @@ def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308,
 
     self.set_limits(pos_limit, neg_limit)
 
-    self.i_rate = 1.0 / rate
+    self.i_dt = 1.0 / rate
     self.speed = 0.0
 
     self.reset()
@@ -46,12 +46,12 @@ def set_limits(self, pos_limit, neg_limit):
 
   def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
     self.speed = speed
-    self.p = float(error) * self.k_p
-    self.f = feedforward * self.k_f
-    self.d = error_rate * self.k_d
+    self.p = self.k_p * float(error)
+    self.d = self.k_d * error_rate
+    self.f = self.k_f * feedforward
 
     if not freeze_integrator:
-      i = self.i + error * self.k_i * self.i_rate
+      i = self.i + self.k_i * self.i_dt * error
 
       # Don't allow windup if already clipping
       test_control = self.p + i + self.d + self.f

selfdrive/controls/controlsd.py

@@ -36,7 +36,7 @@ def __init__(self) -> None:
     self.CI = interfaces[self.CP.carFingerprint](self.CP)
 
     self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
-                                   'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
+                                   'liveCalibration', 'liveDelay', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
                                    'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
     self.pm = messaging.PubMaster(['carControl', 'controlsState'])
 
@@ -118,10 +118,12 @@ def state_control(self):
     new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
     self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
 
+    lat_delay = self.sm["liveDelay"].lateralDelay
+
     actuators.curvature = self.desired_curvature
     steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
                                                        self.steer_limited_by_safety, self.desired_curvature,
-                                                       curvature_limited)  # TODO what if not available
+                                                       curvature_limited, lat_delay)
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
     # Ensure no NaNs/Infs

selfdrive/controls/lib/drive_helpers.py

@@ -22,7 +22,7 @@ def smooth_value(val, prev_val, tau, dt=DT_MDL):
   alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1
   return alpha * val + (1 - alpha) * prev_val
 
-def clip_curvature(v_ego, prev_curvature, new_curvature, roll):
+def clip_curvature(v_ego, prev_curvature, new_curvature, roll) -> tuple[float, bool]:
   # This function respects ISO lateral jerk and acceleration limits + a max curvature
   v_ego = max(v_ego, MIN_SPEED)
   max_curvature_rate = MAX_LATERAL_JERK / (v_ego ** 2)  # inexact calculation, check https://github.com/commaai/openpilot/pull/24755

selfdrive/controls/lib/latcontrol.py

@@ -5,27 +5,28 @@
 
 
 class LatControl(ABC):
-  def __init__(self, CP, CI):
-    self.sat_count_rate = 1.0 * DT_CTRL
+  def __init__(self, CP, CI, dt=DT_CTRL):
+    self.dt = dt
+    self.sat_time_dt = 1.0 * dt
     self.sat_limit = CP.steerLimitTimer
-    self.sat_count = 0.
+    self.sat_time = 0.
     self.sat_check_min_speed = 10.
 
     # we define the steer torque scale as [-1.0...1.0]
     self.steer_max = 1.0
 
   @abstractmethod
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float):
     pass
 
   def reset(self):
-    self.sat_count = 0.
+    self.sat_time = 0.
 
   def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
     # Saturated only if control output is not being limited by car torque/angle rate limits
     if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
-      self.sat_count += self.sat_count_rate
+      self.sat_time += self.sat_time_dt
     else:
-      self.sat_count -= self.sat_count_rate
-    self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
-    return self.sat_count > (self.sat_limit - 1e-3)
+      self.sat_time -= self.sat_time_dt
+    self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
+    return self.sat_time > (self.sat_limit - 1e-3)

selfdrive/controls/lib/latcontrol_angle.py

@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
     self.sat_check_min_speed = 5.
     self.use_steer_limited_by_safety = CP.brand == "tesla"
 
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
     if not active:

selfdrive/controls/lib/latcontrol_pid.py

@@ -13,7 +13,7 @@ def __init__(self, CP, CI):
                              k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
     self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float):
     pid_log = log.ControlsState.LateralPIDState.new_message()
     pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
     pid_log.steeringRateDeg = float(CS.steeringRateDeg)

selfdrive/controls/lib/latcontrol_torque.py

@@ -1,10 +1,13 @@
+from collections import deque
 import math
 import numpy as np
 
 from cereal import log
 from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
+from opendbc.car.tests.test_lateral_limits import MAX_LAT_JERK_UP
 from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
+from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.pid import PIDController
 
 # At higher speeds (25+mph) we can assume:
@@ -21,17 +24,23 @@
 LOW_SPEED_X = [0, 10, 20, 30]
 LOW_SPEED_Y = [15, 13, 10, 5]
 
-
 class LatControlTorque(LatControl):
-  def __init__(self, CP, CI):
-    super().__init__(CP, CI)
+  def __init__(self, CP, CI, dt):
+    super().__init__(CP, CI, dt)
     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(self.torque_params.kp, self.torque_params.ki,
-                             k_f=self.torque_params.kf)
+    self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, k_d=self.torque_params.kd, k_f=self.torque_params.kf, rate=1/self.dt)
     self.update_limits()
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
+    lataccel_request_buffer_size = int(1 / self.dt)
+    self.requested_lateral_accel_buffer = deque([0.] * lataccel_request_buffer_size , maxlen=lataccel_request_buffer_size)
+    self.error_pre = 0.0
+    self.measurement_pre = 0.0
+    self.gravity_adjusted_lateral_accel_pre = 0.0
+    self.error_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt)
+    self.ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt)
+    self.jerk_ff_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * MAX_LAT_JERK_UP), self.dt)
 
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelFactor = latAccelFactor
@@ -43,7 +52,7 @@ def update_limits(self):
     self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
                         self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
 
-  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited):
+  def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay: float):
     pid_log = log.ControlsState.LateralTorqueState.new_message()
     if not active:
       output_torque = 0.0
@@ -53,27 +62,42 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat
       roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
       curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
 
-      desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+      delay_frames = int(lat_delay / self.dt)
+      plan_future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
+      self.requested_lateral_accel_buffer.append(plan_future_desired_lateral_accel)
+      current_expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
+      current_expected_curvature = current_expected_lateral_accel / (CS.vEgo ** 2)
       actual_lateral_accel = actual_curvature * CS.vEgo ** 2
       lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
 
       low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
-      setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
+      # pid error calculated as difference between expected and measured lateral acceleration
+      setpoint_expected = current_expected_lateral_accel + low_speed_factor * current_expected_curvature
       measurement = actual_lateral_accel + low_speed_factor * actual_curvature
-      gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
-
+      gravity_adjusted_lateral_accel = plan_future_desired_lateral_accel - roll_compensation
+      error = float(setpoint_expected - measurement)
+      meas_rate = (measurement - self.measurement_pre) / self.dt
+      meas_rate_filtered = self.error_rate_filter.update(meas_rate)
+      jerk_ff = (gravity_adjusted_lateral_accel - self.gravity_adjusted_lateral_accel_pre) / self.dt
+      jerk_ff_filtered = self.jerk_ff_filter.update(jerk_ff)
+      self.error_pre = error
+      self.measurement_pre = measurement
+      self.gravity_adjusted_lateral_accel_pre = gravity_adjusted_lateral_accel
       # do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
-      pid_log.error = float(setpoint - measurement)
+      pid_log.error = float(error)
       ff = gravity_adjusted_lateral_accel
+      ff += 0.0 * jerk_ff_filtered
       # latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
       ff -= self.torque_params.latAccelOffset
-      ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
+      ff += get_friction(plan_future_desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
 
       freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
       output_lataccel = self.pid.update(pid_log.error,
-                                      feedforward=ff,
-                                      speed=CS.vEgo,
-                                      freeze_integrator=freeze_integrator)
+                                        meas_rate_filtered,
+                                        feedforward=ff,
+                                        speed=CS.vEgo,
+                                        freeze_integrator=freeze_integrator,)
+                                        #error_expected=error_expected)
       output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
 
       pid_log.active = True
@@ -83,7 +107,7 @@ def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvat
       pid_log.f = float(self.pid.f)
       pid_log.output = float(-output_torque)  # TODO: log lat accel?
       pid_log.actualLateralAccel = float(actual_lateral_accel)
-      pid_log.desiredLateralAccel = float(desired_lateral_accel)
+      pid_log.desiredLateralAccel = float(plan_future_desired_lateral_accel)
       pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
 
     # TODO left is positive in this convention

selfdrive/controls/tests/test_latcontrol.py

@@ -32,13 +32,13 @@ def test_saturation(self, car_name, controller):
 
     # Saturate for curvature limited and controller limited
     for _ in range(1000):
-      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True)
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, True, 0)
     assert lac_log.saturated
 
     for _ in range(1000):
-      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False)
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 0, False, 0)
     assert not lac_log.saturated
 
     for _ in range(1000):
-      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False)
+      _, _, lac_log = controller.update(True, CS, VM, params, False, 1, False, 0)
     assert lac_log.saturated

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-afcab1abb62b9d5678342956cced4712f44e909e
+4e2926cc457a057980633163eabe062e421e9ff6