mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Latcontrol: refactor pid error to factor out lateral jerk component (#36280)

Browse Source
This commit is contained in:
felsager
2025-10-08 18:29:54 -07:00
committed by GitHub
parent 0b62dbe16b
commit 226465e882
7 changed files with 34 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/controls/controlsd.py
View File

@@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
State = log.SelfdriveState.OpenpilotState
@@ -35,7 +36,7 @@ class Controls:
self.CI = interfaces[self.CP.carFingerprint](self.CP)
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
self.sm = messaging.SubMaster(['liveDelay', 'liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
@@ -117,11 +118,12 @@ class Controls:
# Reset desired curvature to current to avoid violating the limits on engage
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 + LAT_SMOOTH_SECONDS
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

2
selfdrive/controls/lib/latcontrol.py
View File

@@ -14,7 +14,7 @@ class LatControl(ABC):
self.steer_max = 1.0
@abstractmethod
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, calibrated_pose, curvature_limited: bool):
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float):
pass
def reset(self):

2
selfdrive/controls/lib/latcontrol_angle.py
View File

@@ -13,7 +13,7 @@ class LatControlAngle(LatControl):
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):
angle_log = log.ControlsState.LateralAngleState.new_message()
if not active:

2
selfdrive/controls/lib/latcontrol_pid.py
View File

@@ -13,7 +13,7 @@ class LatControlPID(LatControl):
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):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)

34
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -1,9 +1,11 @@
import math
import numpy as np
from collections import deque
from cereal import log
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.common.pid import PIDController
@@ -29,9 +31,11 @@ class LatControlTorque(LatControl):
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)
k_f=self.torque_params.kf, rate=1/self.dt)
self.update_limits()
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
@@ -43,7 +47,7 @@ class LatControlTorque(LatControl):
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):
pid_log = log.ControlsState.LateralTorqueState.new_message()
if not active:
output_torque = 0.0
@@ -53,21 +57,29 @@ class LatControlTorque(LatControl):
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(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES))
expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
# TODO factor out lateral jerk from error to later replace it with delay independent alternative
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.requested_lateral_accel_buffer.append(future_desired_lateral_accel)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
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
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2 / (np.clip(CS.vEgo, MIN_SPEED, np.inf) ** 2)
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
measurement = actual_lateral_accel
error = setpoint - measurement
error_lsf = error + low_speed_factor * error
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
pid_log.error = float(setpoint - measurement)
ff = gravity_adjusted_lateral_accel
pid_log.error = float(error_lsf)
ff = gravity_adjusted_future_lateral_accel
# 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)
# 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
output_lataccel = self.pid.update(pid_log.error,
@@ -83,7 +95,7 @@ class LatControlTorque(LatControl):
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(expected_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

6
selfdrive/controls/tests/test_latcontrol.py
View File

@@ -33,13 +33,13 @@ class TestLatControl:
# 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.2)
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.2)
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.2)
assert lac_log.saturated

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
afcab1abb62b9d5678342956cced4712f44e909e
4c2f4e9d3d6667c990900ad63f7f5a6b23a8bcdf