From 56eab9cc966262973b9b370b0a973a6fbd55679c Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 18:23:20 +0200 Subject: [PATCH 01/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 2db655692..473852e75 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 2db655692f014b42364703924313f5fa47fe9c05 +Subproject commit 473852e754a846d6628fce58310b18f9cb90e033 From 4350bd0e9866706bbc13f23621f221facac4a30d Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 18:32:45 +0200 Subject: [PATCH 02/43] curv controller only for logging and saturation --- selfdrive/controls/controlsd.py | 2 +- .../controls/lib/latcontrol_curvature.py | 51 ++++--------------- 2 files changed, 11 insertions(+), 42 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e756cfc6a..5ba636468 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -172,7 +172,7 @@ class Controls(ControlsExt, ModelStateBase): steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_safety, self.desired_curvature, self.calibrated_pose, curvature_limited) # TODO what if not available - actuators.curvature = float(curvature) if self.enable_curvature_controller else self.desired_curvature + actuators.curvature = float(curvature) actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 7f0a86453..4b4eb8f21 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -1,9 +1,5 @@ -import math -import numpy as np - from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.common.pid_mu import MultiplicativeUnwindPID CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m @@ -11,46 +7,19 @@ CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m class LatControlCurvature(LatControl): def __init__(self, CP, CP_SP, CI): super().__init__(CP, CP_SP, CI) - self.useCarSteerCurvature = CP.useCarSteerCurvature - self.pid = MultiplicativeUnwindPID((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), - (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=self.curvature_max, neg_limit=-self.curvature_max) def reset(self): super().reset() - self.pid.reset() def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): - pid_log = log.ControlsState.LateralCurvatureState.new_message() - if not active: - output_curvature = 0.0 - pid_log.active = False - self.pid.reset() - 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 - assert 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_vm, actual_curvature_pose]) + curvature_log = log.ControlsState.LateralCurvatureState.new_message() + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + output_curvature = desired_curvature + + curvature_log.active = active + curvature_log.output = float(output_curvature) + curvature_log.actualCurvature = float(actual_curvature) + curvature_log.desiredCurvature = float(output_curvature) + curvature_log.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited)) if active else False - desired_curvature_corr = desired_curvature - roll_compensation - - pid_log.error = float(desired_curvature - actual_curvature) - freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 or CS.steeringPressed - - pid_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature_corr, speed=CS.vEgo, - freeze_integrator=freeze_integrator, override=CS.steeringPressed) - - output_curvature = pid_curvature + (CS.steeringCurvature - actual_curvature_vm_no_roll) if self.useCarSteerCurvature else pid_curvature - - pid_log.active = True - 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(steer_limited_by_safety, CS, False, curvature_limited)) - - return 0.0, 0.0, output_curvature, pid_log + return 0.0, 0.0, output_curvature, curvature_log From d9fbfe4747d07c41467876d94e4d4e1bd5148a28 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 18:34:54 +0200 Subject: [PATCH 03/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 473852e75..5adac528d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 473852e754a846d6628fce58310b18f9cb90e033 +Subproject commit 5adac528d2287c59186b90ddc4cf152fd6864da8 From bc9ef6231aa36e73d21f5b1fd2879ab71bcd835f Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 18:57:58 +0200 Subject: [PATCH 04/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 5adac528d..ae6323184 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 5adac528d2287c59186b90ddc4cf152fd6864da8 +Subproject commit ae63231842bc5bddd57f50849299561a52f8e081 From 524d9e32f335cc058944442ba7af064db6c4e25a Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:01:57 +0200 Subject: [PATCH 05/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index ae6323184..6a4db2055 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ae63231842bc5bddd57f50849299561a52f8e081 +Subproject commit 6a4db20556e4d7c82cc75e514a88fcee5e35bd2f From b787ea671a5f5950b10fff7e5e2b649ae9d41df7 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:04:20 +0200 Subject: [PATCH 06/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 6a4db2055..063aae142 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 6a4db20556e4d7c82cc75e514a88fcee5e35bd2f +Subproject commit 063aae1429a2189a3e235144e3e63f0cb114b5a0 From 8b7054d9db63426dc5faeec8d494c1d100a2854b Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:10:16 +0200 Subject: [PATCH 07/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 063aae142..36ce7cdff 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 063aae1429a2189a3e235144e3e63f0cb114b5a0 +Subproject commit 36ce7cdff7f3cee78588aee7b981c89af80159b0 From 33ee0a5368ef20848950a08a0c79e18873e72619 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:19:54 +0200 Subject: [PATCH 08/43] Update controlsd.py --- selfdrive/controls/controlsd.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5ba636468..5aa2491b9 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -193,6 +193,7 @@ class Controls(ControlsExt, ModelStateBase): CC.curvatureControllerActive = self.enable_curvature_controller # for car controller curvature correction activation CC.currentCurvatureNoRoll = self.curvature_no_roll CC.rollDEPRECATED = self.roll # for lateral iso limit calculation + CC.steerLimited = self.steer_limited_by_safety # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller From 76d078eac0ed9c1ff0f5e0ecfad91cf05f424143 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:35:46 +0200 Subject: [PATCH 09/43] fixing --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 36ce7cdff..6edb570ee 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 36ce7cdff7f3cee78588aee7b981c89af80159b0 +Subproject commit 6edb570ee3beca9782edb6fbc5b07627764dc396 From ac7457e8a8521be68ccd17d0d11dd7fcbe93e2c4 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:46:22 +0200 Subject: [PATCH 10/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 6edb570ee..3fccc4bd2 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 6edb570ee3beca9782edb6fbc5b07627764dc396 +Subproject commit 3fccc4bd2a4ce72827fd9a0e5e0c43d354fd27a0 From d7be1f7b283ff765b4f94eab420a1fad17223c57 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:55:48 +0200 Subject: [PATCH 11/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 3fccc4bd2..b43cdd371 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 3fccc4bd2a4ce72827fd9a0e5e0c43d354fd27a0 +Subproject commit b43cdd371d7d08140e94d94d7f9b30107917ded0 From 35745b38e8274d0139fc7aa4b90d7ffb09c3f9ad Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 19:58:40 +0200 Subject: [PATCH 12/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index b43cdd371..31f02bd3c 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit b43cdd371d7d08140e94d94d7f9b30107917ded0 +Subproject commit 31f02bd3c5d2c013574ef103a99ef00efdee34c5 From a675d4abe0a6cbed9e4c935dd907f289a61db817 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:01:22 +0200 Subject: [PATCH 13/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 31f02bd3c..dfe6730d9 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 31f02bd3c5d2c013574ef103a99ef00efdee34c5 +Subproject commit dfe6730d9553e9fbcfb14a000360653126aa43d1 From c4009b0f907d8a98726ef199b81da2ec9b5bc637 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:04:29 +0200 Subject: [PATCH 14/43] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 4b4eb8f21..1d9eb97e4 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -1,3 +1,4 @@ +import math from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl From 8b9f29138c8fb29e546806f0783f3f4ba508d081 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:04:43 +0200 Subject: [PATCH 15/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index dfe6730d9..eac2e1c1d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit dfe6730d9553e9fbcfb14a000360653126aa43d1 +Subproject commit eac2e1c1d361a580ac6ff59d2a257afff64b217d From f57abba6b5ebe9ef8195eddd7be5a097a8e62ba0 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:06:08 +0200 Subject: [PATCH 16/43] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 1d9eb97e4..e56e8c3f7 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -1,4 +1,5 @@ import math + from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl From 1a621fc074df3bd9ae07f3164b59bfc9ce34bddf Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:07:25 +0200 Subject: [PATCH 17/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index eac2e1c1d..498ddbd23 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit eac2e1c1d361a580ac6ff59d2a257afff64b217d +Subproject commit 498ddbd23ba6c5e7bb82ba428b47054989c3a1b9 From 1ec7637102afe28c68734b31b49b07d779ae4e80 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:11:11 +0200 Subject: [PATCH 18/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 498ddbd23..6e51eeffb 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 498ddbd23ba6c5e7bb82ba428b47054989c3a1b9 +Subproject commit 6e51eeffb280296c0bdbb3864af3ea6c004be249 From 1809150dc0b46b89a1e343a97adb3adcf76405e8 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:22:37 +0200 Subject: [PATCH 19/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 6e51eeffb..dd9d2dc22 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 6e51eeffb280296c0bdbb3864af3ea6c004be249 +Subproject commit dd9d2dc22de4188030e3d016684cfff87641f771 From 6198f2a26124d96d303aad683773b023a80d4c23 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 20:29:18 +0200 Subject: [PATCH 20/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index dd9d2dc22..0bf7d88ff 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit dd9d2dc22de4188030e3d016684cfff87641f771 +Subproject commit 0bf7d88ff66926fd20e5de94fdd8048ad94a6693 From 6d00fec2ea029de7d6411aa39fb02c0025a84d86 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Tue, 2 Sep 2025 21:37:19 +0200 Subject: [PATCH 21/43] pid first --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 0bf7d88ff..0b61f0008 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 0bf7d88ff66926fd20e5de94fdd8048ad94a6693 +Subproject commit 0b61f0008451918af2ba5535a787d2318723ad2e From 085ebe148a0bae9113fb115d6f3cd2fdce1de559 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 17:25:55 +0200 Subject: [PATCH 22/43] remove curvature controller --- common/pid_mu.py | 32 ------------------- selfdrive/controls/controlsd.py | 15 +++------ selfdrive/controls/lib/latcontrol_angle.py | 2 +- .../controls/lib/latcontrol_curvature.py | 27 ---------------- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 6 files changed, 7 insertions(+), 73 deletions(-) delete mode 100644 common/pid_mu.py delete mode 100644 selfdrive/controls/lib/latcontrol_curvature.py diff --git a/common/pid_mu.py b/common/pid_mu.py deleted file mode 100644 index 49dd0a42c..000000000 --- a/common/pid_mu.py +++ /dev/null @@ -1,32 +0,0 @@ -from openpilot.common.pid import PIDController -import numpy as np - -class MultiplicativeUnwindPID(PIDController): - def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): - super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate) - - self.i_unwind_rate = 0.3 / rate - - def update(self, error, error_rate=0.0, speed=0.0, override=False, 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 - - if override: - self.i *= (1.0 - self.i_unwind_rate) - if abs(self.i) < 1e-10: - self.i = 0.0 - else: - if not freeze_integrator: - i = self.i + error * self.k_i * self.i_rate - - # Don't allow windup if already clipping - test_control = self.p + i + self.d + self.f - i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit - i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit - self.i = np.clip(i, i_lowerbound, i_upperbound) - - control = self.p + self.i + self.d + self.f - self.control = np.clip(control, self.neg_limit, self.pos_limit) - return self.control \ No newline at end of file diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5aa2491b9..35fa621af 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -18,7 +18,6 @@ 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, CURVATURE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW @@ -76,8 +75,6 @@ class Controls(ControlsExt, ModelStateBase): self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI) - elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: - self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'torque': @@ -169,10 +166,10 @@ class Controls(ControlsExt, ModelStateBase): 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) - steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_safety, self.desired_curvature, - self.calibrated_pose, curvature_limited) # TODO what if not available - actuators.curvature = float(curvature) + 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, + self.calibrated_pose, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs @@ -229,8 +226,6 @@ class Controls(ControlsExt, ModelStateBase): 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) > CURVATURE_SATURATION_THRESHOLD else: self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 @@ -256,8 +251,6 @@ class Controls(ControlsExt, ModelStateBase): lat_tuning = self.CP.lateralTuning.which() if self.CP.steerControlType == car.CarParams.SteerControlType.angle: cs.lateralControlState.angleState = lac_log - elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: - cs.lateralControlState.curvatureStateDEPRECATED = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 206078bdf..787bd2dbe 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -34,4 +34,4 @@ class LatControlAngle(LatControl): angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des - return 0, float(angle_steers_des), desired_curvature, angle_log + return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py deleted file mode 100644 index e56e8c3f7..000000000 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ /dev/null @@ -1,27 +0,0 @@ -import math - -from cereal import log -from openpilot.selfdrive.controls.lib.latcontrol import LatControl - -CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m - - -class LatControlCurvature(LatControl): - def __init__(self, CP, CP_SP, CI): - super().__init__(CP, CP_SP, CI) - - def reset(self): - super().reset() - - def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): - curvature_log = log.ControlsState.LateralCurvatureState.new_message() - actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - output_curvature = desired_curvature - - curvature_log.active = active - curvature_log.output = float(output_curvature) - curvature_log.actualCurvature = float(actual_curvature) - curvature_log.desiredCurvature = float(output_curvature) - curvature_log.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited)) if active else False - - return 0.0, 0.0, output_curvature, curvature_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 22eaff789..fd79c29bd 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -45,4 +45,4 @@ class LatControlPID(LatControl): pid_log.output = float(output_torque) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) - return output_torque, angle_steers_des, desired_curvature, pid_log + return output_torque, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index af0fe14bc..e4554ae46 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -97,4 +97,4 @@ class LatControlTorque(LatControl): 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 - return -output_torque, 0.0, desired_curvature, pid_log + return -output_torque, 0.0, pid_log From f524c6ef6d46460e453cba2f2511e8ee3360cf0e Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 17:56:25 +0200 Subject: [PATCH 23/43] cleanup --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 0b61f0008..f1a95418b 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 0b61f0008451918af2ba5535a787d2318723ad2e +Subproject commit f1a95418b1944a0db1acb58ac33a3690e303b9b4 From 5670bb044e9851ae06bc7246bed652882f04f6aa Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:00:16 +0200 Subject: [PATCH 24/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index f1a95418b..283324fbd 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit f1a95418b1944a0db1acb58ac33a3690e303b9b4 +Subproject commit 283324fbdae7dd71df9d6840a972ad398d834ae6 From ca9173f1224031b3db1933f117dcfac9bb6dab6c Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:04:24 +0200 Subject: [PATCH 25/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 283324fbd..ef61b30b8 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 283324fbdae7dd71df9d6840a972ad398d834ae6 +Subproject commit ef61b30b89b28fca9c4391f40ca6428bc8b47b36 From 4fa998d1bd3437bb11fe63d6e0fdb9101a8df7b3 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:11:26 +0200 Subject: [PATCH 26/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index ef61b30b8..c40ae3fe0 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ef61b30b89b28fca9c4391f40ca6428bc8b47b36 +Subproject commit c40ae3fe0ade548c40d37fd27e2df49f36bcfe27 From 1211b79e8e5ce4fdeafe9053996bac3c15012128 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:20:25 +0200 Subject: [PATCH 27/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index c40ae3fe0..0d27c9401 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit c40ae3fe0ade548c40d37fd27e2df49f36bcfe27 +Subproject commit 0d27c940121d3c6fbb58c025f262e0e2677c0538 From e85397485cd0cebea18172edf93cb387b00caa20 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:26:03 +0200 Subject: [PATCH 28/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 0d27c9401..89a2daa99 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 0d27c940121d3c6fbb58c025f262e0e2677c0538 +Subproject commit 89a2daa99799b185f491a0535324b923b92d35f7 From 82fc6cca1a3091503a3e62fad336c551ac291b2a Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:39:30 +0200 Subject: [PATCH 29/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 89a2daa99..95144f1e9 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 89a2daa99799b185f491a0535324b923b92d35f7 +Subproject commit 95144f1e9d649362eb7a7860badcbb56a37d4951 From 7c0da9aaa5beb31d220511ed01743518e7ad2c6c Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:20 +0200 Subject: [PATCH 30/43] Revert "Update opendbc_repo" This reverts commit 82fc6cca1a3091503a3e62fad336c551ac291b2a. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 95144f1e9..89a2daa99 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 95144f1e9d649362eb7a7860badcbb56a37d4951 +Subproject commit 89a2daa99799b185f491a0535324b923b92d35f7 From 7a279c8ce1778113e20c910ba069d9484342a408 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:24 +0200 Subject: [PATCH 31/43] Revert "Update opendbc_repo" This reverts commit e85397485cd0cebea18172edf93cb387b00caa20. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 89a2daa99..0d27c9401 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 89a2daa99799b185f491a0535324b923b92d35f7 +Subproject commit 0d27c940121d3c6fbb58c025f262e0e2677c0538 From 2b780fc414269f74c266342c96050b92ecc96604 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:28 +0200 Subject: [PATCH 32/43] Revert "Update opendbc_repo" This reverts commit 1211b79e8e5ce4fdeafe9053996bac3c15012128. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 0d27c9401..c40ae3fe0 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 0d27c940121d3c6fbb58c025f262e0e2677c0538 +Subproject commit c40ae3fe0ade548c40d37fd27e2df49f36bcfe27 From b4176471929627811d5e0526e0a043afe3d79874 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:32 +0200 Subject: [PATCH 33/43] Revert "Update opendbc_repo" This reverts commit 4fa998d1bd3437bb11fe63d6e0fdb9101a8df7b3. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index c40ae3fe0..ef61b30b8 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit c40ae3fe0ade548c40d37fd27e2df49f36bcfe27 +Subproject commit ef61b30b89b28fca9c4391f40ca6428bc8b47b36 From 98fedfecb813333517403f5a5abf063e29aaf211 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:35 +0200 Subject: [PATCH 34/43] Revert "Update opendbc_repo" This reverts commit ca9173f1224031b3db1933f117dcfac9bb6dab6c. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index ef61b30b8..283324fbd 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ef61b30b89b28fca9c4391f40ca6428bc8b47b36 +Subproject commit 283324fbdae7dd71df9d6840a972ad398d834ae6 From 93f954ad8d3a209883d8d9d4d8da243f15e76940 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:40 +0200 Subject: [PATCH 35/43] Revert "Update opendbc_repo" This reverts commit 5670bb044e9851ae06bc7246bed652882f04f6aa. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 283324fbd..f1a95418b 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 283324fbdae7dd71df9d6840a972ad398d834ae6 +Subproject commit f1a95418b1944a0db1acb58ac33a3690e303b9b4 From 9f8ff6d908bab75b8a33ab37c3b79a35896bbadc Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:45 +0200 Subject: [PATCH 36/43] Revert "cleanup" This reverts commit f524c6ef6d46460e453cba2f2511e8ee3360cf0e. --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index f1a95418b..0b61f0008 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit f1a95418b1944a0db1acb58ac33a3690e303b9b4 +Subproject commit 0b61f0008451918af2ba5535a787d2318723ad2e From 1d6ad46b028e7ace3db6e36b7a5420b73718d694 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:49:51 +0200 Subject: [PATCH 37/43] Revert "remove curvature controller" This reverts commit 085ebe148a0bae9113fb115d6f3cd2fdce1de559. --- common/pid_mu.py | 32 +++++++++++++++++++ selfdrive/controls/controlsd.py | 15 ++++++--- selfdrive/controls/lib/latcontrol_angle.py | 2 +- .../controls/lib/latcontrol_curvature.py | 27 ++++++++++++++++ selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 6 files changed, 73 insertions(+), 7 deletions(-) create mode 100644 common/pid_mu.py create mode 100644 selfdrive/controls/lib/latcontrol_curvature.py diff --git a/common/pid_mu.py b/common/pid_mu.py new file mode 100644 index 000000000..49dd0a42c --- /dev/null +++ b/common/pid_mu.py @@ -0,0 +1,32 @@ +from openpilot.common.pid import PIDController +import numpy as np + +class MultiplicativeUnwindPID(PIDController): + def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate) + + self.i_unwind_rate = 0.3 / rate + + def update(self, error, error_rate=0.0, speed=0.0, override=False, 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 + + if override: + self.i *= (1.0 - self.i_unwind_rate) + if abs(self.i) < 1e-10: + self.i = 0.0 + else: + if not freeze_integrator: + i = self.i + error * self.k_i * self.i_rate + + # Don't allow windup if already clipping + test_control = self.p + i + self.d + self.f + i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit + i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit + self.i = np.clip(i, i_lowerbound, i_upperbound) + + control = self.p + self.i + self.d + self.f + self.control = np.clip(control, self.neg_limit, self.pos_limit) + return self.control \ No newline at end of file diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 35fa621af..5aa2491b9 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -18,6 +18,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, CURVATURE_SATURATION_THRESHOLD from openpilot.selfdrive.controls.lib.longcontrol import LongControl from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW @@ -75,6 +76,8 @@ class Controls(ControlsExt, ModelStateBase): self.LaC: LatControl if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI) + elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'torque': @@ -166,10 +169,10 @@ class Controls(ControlsExt, ModelStateBase): 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) - 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, - self.calibrated_pose, curvature_limited) # TODO what if not available + steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, + self.steer_limited_by_safety, self.desired_curvature, + self.calibrated_pose, curvature_limited) # TODO what if not available + actuators.curvature = float(curvature) actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs @@ -226,6 +229,8 @@ class Controls(ControlsExt, ModelStateBase): 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) > CURVATURE_SATURATION_THRESHOLD else: self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 @@ -251,6 +256,8 @@ class Controls(ControlsExt, ModelStateBase): lat_tuning = self.CP.lateralTuning.which() if self.CP.steerControlType == car.CarParams.SteerControlType.angle: cs.lateralControlState.angleState = lac_log + elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + cs.lateralControlState.curvatureStateDEPRECATED = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log elif lat_tuning == 'torque': diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 787bd2dbe..206078bdf 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -34,4 +34,4 @@ class LatControlAngle(LatControl): angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des - return 0, float(angle_steers_des), angle_log + return 0, float(angle_steers_des), desired_curvature, angle_log diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py new file mode 100644 index 000000000..e56e8c3f7 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -0,0 +1,27 @@ +import math + +from cereal import log +from openpilot.selfdrive.controls.lib.latcontrol import LatControl + +CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m + + +class LatControlCurvature(LatControl): + def __init__(self, CP, CP_SP, CI): + super().__init__(CP, CP_SP, CI) + + def reset(self): + super().reset() + + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): + curvature_log = log.ControlsState.LateralCurvatureState.new_message() + actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + output_curvature = desired_curvature + + curvature_log.active = active + curvature_log.output = float(output_curvature) + curvature_log.actualCurvature = float(actual_curvature) + curvature_log.desiredCurvature = float(output_curvature) + curvature_log.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited)) if active else False + + return 0.0, 0.0, output_curvature, curvature_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index fd79c29bd..22eaff789 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -45,4 +45,4 @@ class LatControlPID(LatControl): pid_log.output = float(output_torque) pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) - return output_torque, angle_steers_des, pid_log + return output_torque, angle_steers_des, desired_curvature, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index e4554ae46..af0fe14bc 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -97,4 +97,4 @@ class LatControlTorque(LatControl): 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 - return -output_torque, 0.0, pid_log + return -output_torque, 0.0, desired_curvature, pid_log From 14b845797659dfbda547ea0f3627b4795c003e90 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:52:22 +0200 Subject: [PATCH 38/43] revert curv control and type clean --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 0b61f0008..76cb3d6ed 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 0b61f0008451918af2ba5535a787d2318723ad2e +Subproject commit 76cb3d6edb884ff7e1dff6d00de35a60bbf830d3 From 906135554966d7150d56934711faadd9c230898d Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 18:59:19 +0200 Subject: [PATCH 39/43] bump --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 76cb3d6ed..ef5ddfeb0 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 76cb3d6edb884ff7e1dff6d00de35a60bbf830d3 +Subproject commit ef5ddfeb0d473c63e28dd893a68814cfcc36842c From c1dedd2692506f31c7dae064739e7e90171da839 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 19:19:57 +0200 Subject: [PATCH 40/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index ef5ddfeb0..523152597 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ef5ddfeb0d473c63e28dd893a68814cfcc36842c +Subproject commit 523152597aa6f8b7b65fe14c45a844cee410cb55 From 42dfca1a5c2eca83b5e24e79a2704c29d936b8d4 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Thu, 4 Sep 2025 17:58:51 +0200 Subject: [PATCH 41/43] Create latcontrol_curvature_pid_backup.py leave this here --- .../lib/latcontrol_curvature_pid_backup.py | 56 +++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 selfdrive/controls/lib/latcontrol_curvature_pid_backup.py diff --git a/selfdrive/controls/lib/latcontrol_curvature_pid_backup.py b/selfdrive/controls/lib/latcontrol_curvature_pid_backup.py new file mode 100644 index 000000000..50eb23bf5 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol_curvature_pid_backup.py @@ -0,0 +1,56 @@ +#import math +#import numpy as np +# +#from cereal import log +#from openpilot.selfdrive.controls.lib.latcontrol import LatControl +#from openpilot.common.pid_mu import MultiplicativeUnwindPID +# +#CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m +# +# +#class LatControlCurvature(LatControl): +# def __init__(self, CP, CP_SP, CI): +# super().__init__(CP, CP_SP, CI) +# self.useCarSteerCurvature = CP.useCarSteerCurvature +# self.pid = MultiplicativeUnwindPID((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), +# (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), +# k_f=CP.lateralTuning.pid.kf, pos_limit=self.curvature_max, neg_limit=-self.curvature_max) +# +# def reset(self): +# super().reset() +# self.pid.reset() +# +# def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): +# pid_log = log.ControlsState.LateralCurvatureState.new_message() +# if not active: +# output_curvature = 0.0 +# pid_log.active = False +# self.pid.reset() +# 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 +# assert 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_vm, actual_curvature_pose]) +# +# desired_curvature_corr = desired_curvature - roll_compensation +# +# pid_log.error = float(desired_curvature - actual_curvature) +# freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 or CS.steeringPressed +# +# pid_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature_corr, speed=CS.vEgo, +# freeze_integrator=freeze_integrator, override=CS.steeringPressed) +# +# output_curvature = pid_curvature + (CS.steeringCurvature - actual_curvature_vm_no_roll) if self.useCarSteerCurvature else pid_curvature +# +# pid_log.active = True +# 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(steer_limited_by_safety, CS, False, curvature_limited)) +# +# return 0.0, 0.0, output_curvature, pid_log From e806306fe1c837a4ebee34d181c5aea5827d7c28 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Thu, 4 Sep 2025 18:04:17 +0200 Subject: [PATCH 42/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 523152597..fb0c53533 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 523152597aa6f8b7b65fe14c45a844cee410cb55 +Subproject commit fb0c53533dd899d5058a23f5b49673f64912d46b From 46fe675a74e74697d8ef3d4cdd5da411f6ea3d05 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Thu, 4 Sep 2025 18:27:39 +0200 Subject: [PATCH 43/43] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index fb0c53533..8d855e63d 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit fb0c53533dd899d5058a23f5b49673f64912d46b +Subproject commit 8d855e63d012789ce7acd4d110965315658d5a10