From 7404d4683e4d182883d1bf5c1462b91a2047aa46 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 10 Jun 2026 21:08:28 +0200 Subject: [PATCH] OP Upstream PID Curvature Controller (#13) * implement openpilot upstream controller and adapt * Update opendbc_repo * Update latcontrol_curvature.py * Update latcontrol_curvature.py * Update opendbc_repo * Update opendbc_repo --------- Co-authored-by: infiniteCable <75014343+infiniteCable@users.noreply.github.com> --- opendbc_repo | 2 +- selfdrive/controls/controlsd.py | 12 +- .../controls/lib/latcontrol_curvature.py | 103 ++++++++---------- 3 files changed, 52 insertions(+), 65 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index 98f66a9c4..da233d865 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 98f66a9c4fed41c1262f991f0433aff8a10ef6e6 +Subproject commit da233d86567ddd96a4d38a9d8fcc65623a80ab14 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6d0967c18..5a29745ba 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,7 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl 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.latcontrol_curvature import LatControlCurvature, LAT_CURVATURE_SATURATION_ACCEL +from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature 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 @@ -190,16 +190,15 @@ class Controls(ControlsExt): self.model_desired_curvature = float(model_v2.action.desiredCurvature) if self.enable_smooth_steer: new_desired_curvature = self.smooth_steer.update(new_desired_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 - - # CurvatureD correction routed as additive feedforward to PID (not setpoint shift) if self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + # 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(['liveCurvatureParameters']): correction = self.curvatured.get_correction(self.desired_curvature, CS.vEgo) else: correction = 0.0 self.LaC.set_curvature_correction(correction) + 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 steer, steeringAngleDeg, output_curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, @@ -266,9 +265,6 @@ class Controls(ControlsExt): if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD - elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: - self.steer_limited_by_safety = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) * CS.vEgo ** 2 > \ - LAT_CURVATURE_SATURATION_ACCEL else: self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 50923f40c..dafe5b3e1 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -2,79 +2,70 @@ import math import numpy as np from cereal import log +from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.common.pid import MultiplicativeUnwindPID +from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE -LAT_CURVATURE_SATURATION_ACCEL = 0.1 +CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m class LatControlCurvature(LatControl): def __init__(self, CP, CP_SP, CI, dt): super().__init__(CP, CP_SP, CI, dt) - ct = CP.lateralTuning.curvature - self.pid = MultiplicativeUnwindPID((ct.kpBP, ct.kpV), - (ct.kiBP, ct.kiV), - k_f=ct.kf, - pos_limit=self.curvature_max, neg_limit=-self.curvature_max) - self.useCarSteerCurvature = ct.useCarSteerCurvature - self.curvature_correction = 0.0 + self.sat_check_min_speed = 5. self.enable_pid = False + self.curvature_correction = 0.0 + if CP.lateralTuning.which() == 'pid': + ct = CP.lateralTuning.pid + self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV), + pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt) + self.kf = ct.kf + else: + self.pid = None + self.kf = 1. + + def set_pid_enabled(self, enabled: bool) -> None: + self.enable_pid = enabled def set_curvature_correction(self, correction: float) -> None: self.curvature_correction = correction - def set_pid_enabled(self, enabled: bool) -> None: - self.enable_pid = enabled - def reset(self): super().reset() - self.pid.reset() + if self.pid is not None: + self.pid.reset() def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay): - pid_log = log.ControlsState.LateralCurvatureState.new_message() + curvature_log = log.ControlsState.LateralCurvatureState.new_message() + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + if calibrated_pose is not None: + actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo + actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature, actual_curvature_pose]) + error = desired_curvature - actual_curvature + if not active: output_curvature = 0.0 - pid_log.active = False - self.pid.reset() + curvature_log.active = False + if self.pid is not None: + self.pid.reset() + elif self.pid is None or CS.steeringPressed or not self.enable_pid: + # no PID or override: feedforward only + if self.pid is not None: + self.pid.reset() + output_curvature = self.kf * desired_curvature + curvature_log.active = True else: - roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) - actual_curvature_vm_no_roll = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, 0.) - actual_curvature_vm = actual_curvature_vm_no_roll - roll_compensation + output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature) + curvature_log.p = float(self.pid.p) + curvature_log.i = float(self.pid.i) + curvature_log.f = float(self.pid.f) + curvature_log.active = True - actual_curvature = actual_curvature_vm - if calibrated_pose is not None and CS.vEgo > 5.0: - actual_curvature_pose = calibrated_pose.angular_velocity.yaw / max(CS.vEgo, 0.1) - actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) - - desired_curvature_eff = desired_curvature + self.curvature_correction - feedforward = desired_curvature_eff - roll_compensation - - if self.enable_pid: - pid_log.error = float(desired_curvature_eff - actual_curvature) - freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 or CS.steeringPressed - - pid_curvature = self.pid.update(pid_log.error, speed=CS.vEgo, - feedforward=feedforward, - freeze_integrator=freeze_integrator, override=CS.steeringPressed) - else: - pid_curvature = feedforward - pid_log.error = float(desired_curvature_eff - actual_curvature) - pid_log.p = 0.0 - pid_log.i = 0.0 - pid_log.f = float(feedforward) - - output_curvature = pid_curvature + (CS.steeringCurvature - actual_curvature_vm_no_roll) if self.useCarSteerCurvature else pid_curvature - - saturated = abs(output_curvature) >= self.curvature_max or abs(pid_curvature) >= self.curvature_max - - pid_log.active = True - if self.enable_pid: - pid_log.p = float(self.pid.p) - pid_log.i = float(self.pid.i) - pid_log.f = float(self.pid.f) - pid_log.output = float(output_curvature) - pid_log.actualCurvature = float(actual_curvature) - pid_log.desiredCurvature = float(desired_curvature) - pid_log.saturated = bool(self._check_saturation(saturated, CS, steer_limited_by_safety, curvature_limited)) - - return 0.0, 0.0, output_curvature, pid_log + curvature_log.error = float(error) + curvature_log.actualCurvature = float(actual_curvature) + curvature_log.desiredCurvature = float(desired_curvature) + output_curvature = output_curvature + self.curvature_correction + curvature_log.output = float(output_curvature) + curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS, + False, curvature_limited)) + return 0.0, 0.0, float(output_curvature), curvature_log