From 659963931b3d73f44f8adf69a04f33309470780d Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 15:12:14 +0200 Subject: [PATCH 01/14] prepare neccessary variables for curv correction in vw car controller --- selfdrive/controls/controlsd.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5aa2491b9..72cb5a014 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -58,9 +58,8 @@ class Controls(ControlsExt, ModelStateBase): self.steer_limited_by_safety = False self.curvature = 0.0 - self.curvature_no_roll = 0.0 + self.roll_compensation = 0.0 self.desired_curvature = 0.0 - self.roll = 0.0 self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") @@ -110,8 +109,7 @@ class Controls(ControlsExt, ModelStateBase): steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) - self.curvature_no_roll = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, 0.0) - self.roll = lp.roll + self.roll_compensation = -VM.roll_compensation(lp.roll, CS.vEgo) # Update Torque Params if self.CP.lateralTuning.which() == 'torque': @@ -191,13 +189,13 @@ class Controls(ControlsExt, ModelStateBase): CS = self.sm['carState'] 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 CC.currentCurvature = self.curvature + CC.rollCompensation = self.roll_compensation + if self.calibrated_pose is not None: CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() From d7fa2684323c5054e8c20460daa0dc38b4ad640a Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 15:48:34 +0200 Subject: [PATCH 02/14] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 7115553b6..c66f2e5eb 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 7115553b65531feccff128edd892b6842018b9e5 +Subproject commit c66f2e5ebe4fa5fe0b6447b56969a766d299be44 From d17ba5a56268a715490041fa680c60f56145494c Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sun, 7 Sep 2025 16:32:17 +0200 Subject: [PATCH 03/14] Update controlsd.py --- selfdrive/controls/controlsd.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 72cb5a014..602d214c5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -109,7 +109,7 @@ class Controls(ControlsExt, ModelStateBase): steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) - self.roll_compensation = -VM.roll_compensation(lp.roll, CS.vEgo) + self.roll_compensation = -self.VM.roll_compensation(lp.roll, CS.vEgo) # Update Torque Params if self.CP.lateralTuning.which() == 'torque': From 8812fde77c27da1db22d4f512f1b28540f7d60bd Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 17:22:27 +0200 Subject: [PATCH 04/14] pass yaw directly --- selfdrive/controls/controlsd.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 72cb5a014..aca2910f5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -197,6 +197,7 @@ class Controls(ControlsExt, ModelStateBase): CC.rollCompensation = self.roll_compensation if self.calibrated_pose is not None: + CC.yawRate = self.calibrated_pose.angular_velocity.yaw CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() From 8b3bcc55563947e01579e91372919d9910e6ad68 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 17:22:37 +0200 Subject: [PATCH 05/14] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index c66f2e5eb..194d12f8b 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit c66f2e5ebe4fa5fe0b6447b56969a766d299be44 +Subproject commit 194d12f8b1aa062ffb348ffeb0575fd12cf6af16 From 5d057a1f03d98b073b80ed2cb25773b4687f0a03 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 17:31:06 +0200 Subject: [PATCH 06/14] Revert "pass yaw directly" This reverts commit 8812fde77c27da1db22d4f512f1b28540f7d60bd. --- selfdrive/controls/controlsd.py | 1 - 1 file changed, 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c45a46ca1..602d214c5 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -197,7 +197,6 @@ class Controls(ControlsExt, ModelStateBase): CC.rollCompensation = self.roll_compensation if self.calibrated_pose is not None: - CC.yawRate = self.calibrated_pose.angular_velocity.yaw CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() From 4c66affb36f4e6b4e8ee227971acb5ce49ecabfb Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 17:32:58 +0200 Subject: [PATCH 07/14] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 194d12f8b..48a56638f 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 194d12f8b1aa062ffb348ffeb0575fd12cf6af16 +Subproject commit 48a56638f90a2eb2a49a23e6af9d1f5250df9479 From 57c26b0755dedcb04403a393a06d955da30dbc57 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 17:49:16 +0200 Subject: [PATCH 08/14] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 48a56638f..08d34ae1a 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 48a56638f90a2eb2a49a23e6af9d1f5250df9479 +Subproject commit 08d34ae1a8b8282b32724ac476ece0c99dd4fd2e From 13328b968e116e3b0342318680e9dc6e61555751 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 18:36:24 +0200 Subject: [PATCH 09/14] Update opendbc_repo --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index 08d34ae1a..cccf8b643 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 08d34ae1a8b8282b32724ac476ece0c99dd4fd2e +Subproject commit cccf8b643dddb6f729134d0ad36f87db66abc575 From 6728f9f3738065e647c6086f26b5899356288806 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 18:48:01 +0200 Subject: [PATCH 10/14] revert more to stock angle control logic (curvature CO with offset in CC leaves to wrong limit detection), but still use curv type --- selfdrive/controls/controlsd.py | 19 ++++++++----------- .../controls/lib/latcontrol_curvature.py | 1 + 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 602d214c5..76307a00b 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 @@ -73,10 +72,9 @@ class Controls(ControlsExt, ModelStateBase): self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) self.LaC: LatControl - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle + or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: 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': @@ -167,10 +165,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 @@ -252,10 +250,9 @@ class Controls(ControlsExt, ModelStateBase): (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle + or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: 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_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index e56e8c3f7..0544085c6 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -9,6 +9,7 @@ CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m class LatControlCurvature(LatControl): def __init__(self, CP, CP_SP, CI): super().__init__(CP, CP_SP, CI) + assert False def reset(self): super().reset() From 2d5573c1d6d2b9cc6297e6cafa1d487d1dc362cf Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 18:50:13 +0200 Subject: [PATCH 11/14] fix output --- selfdrive/controls/lib/latcontrol_angle.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 2 +- selfdrive/controls/lib/latcontrol_torque.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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_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 8505e93bc2989805a616774c970827b84d3ca10c Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 18:55:33 +0200 Subject: [PATCH 12/14] Update controlsd.py --- selfdrive/controls/controlsd.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 76307a00b..dde6d3a2a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -72,8 +72,8 @@ class Controls(ControlsExt, ModelStateBase): self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) self.LaC: LatControl - if self.CP.steerControlType == car.CarParams.SteerControlType.angle - or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI) @@ -222,11 +222,10 @@ class Controls(ControlsExt, ModelStateBase): if self.sm['selfdriveState'].active: CO = self.sm['carOutput'] - if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: 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 @@ -250,8 +249,8 @@ class Controls(ControlsExt, ModelStateBase): (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() - if self.CP.steerControlType == car.CarParams.SteerControlType.angle - or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + if self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: cs.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log From e825639fc12ef5b7e295d195b65ae1a401673912 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 18:58:48 +0200 Subject: [PATCH 13/14] Update controlsd.py --- selfdrive/controls/controlsd.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dde6d3a2a..bd4735815 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -72,8 +72,8 @@ class Controls(ControlsExt, ModelStateBase): self.LoC = LongControl(self.CP) self.VM = VehicleModel(self.CP) self.LaC: LatControl - if self.CP.steerControlType == car.CarParams.SteerControlType.angle or - self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED): self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI) @@ -222,8 +222,8 @@ class Controls(ControlsExt, ModelStateBase): if self.sm['selfdriveState'].active: CO = self.sm['carOutput'] - if self.CP.steerControlType == car.CarParams.SteerControlType.angle or - self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED): self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD else: @@ -249,8 +249,8 @@ class Controls(ControlsExt, ModelStateBase): (self.sm['selfdriveState'].state == State.softDisabling)) lat_tuning = self.CP.lateralTuning.which() - if self.CP.steerControlType == car.CarParams.SteerControlType.angle or - self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: + if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or + self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED): cs.lateralControlState.angleState = lac_log elif lat_tuning == 'pid': cs.lateralControlState.pidState = lac_log From f855ce8b5dc7a4d94cc4f0842b878ee5e2a8701d Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 7 Sep 2025 19:22:26 +0200 Subject: [PATCH 14/14] sign.... --- opendbc_repo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/opendbc_repo b/opendbc_repo index cccf8b643..adecffed2 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit cccf8b643dddb6f729134d0ad36f87db66abc575 +Subproject commit adecffed2b1bca59d5d9e5b2a1c3a6bb54c16397