From c9f7db4197cd5cb5854f0d1d3b324b6388cebff7 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 10:44:31 +0200 Subject: [PATCH 1/6] Update params_keys.h --- common/params_keys.h | 1 + 1 file changed, 1 insertion(+) diff --git a/common/params_keys.h b/common/params_keys.h index acbeaa3bd..fceee5b1c 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -37,6 +37,7 @@ inline static std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, + {"EnableCurvatureController", PERSISTENT}, {"EnableScreenEvent", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION}, {"EnableSmoothSteer", PERSISTENT}, {"EnableSpeedLimitControl", PERSISTENT}, From 1fac1d571981818f5d93803eae45733ac82e6172 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 10:46:29 +0200 Subject: [PATCH 2/6] Update settings.cc --- selfdrive/ui/qt/offroad/settings.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 7ddbe3111..7a20782d6 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -47,6 +47,13 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "../assets/icons/disengage_on_accelerator.svg", false, }, + { + "EnableCurvatureController", + tr("Enable Curvature Controller"), + tr("Enables curvature post-processing
"), + "../assets/icons/chffr_wheel.png", + false, + }, { "EnableSpeedLimitControl", tr("Enable Speed Limit Control"), From 737eeaa3b28554f529d556a3fb77013dc1b0b2fe Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 10:48:21 +0200 Subject: [PATCH 3/6] Update controlsd.py --- selfdrive/controls/controlsd.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e2e9488a1..bc5bf4802 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -58,6 +58,7 @@ class Controls(ControlsExt): 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") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") @@ -89,6 +90,7 @@ class Controls(ControlsExt): self.param_counter += 1 if self.param_counter >= 100: self.param_counter = 0 + self.enable_curvature_controller = self.params.get_bool("EnableCurvatureController") self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") self.enable_speed_limit_control = self.params.get_bool("EnableSpeedLimitControl") self.enable_speed_limit_predicative = self.params.get_bool("EnableSpeedLimitPredicative") @@ -160,7 +162,7 @@ class Controls(ControlsExt): steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, self.desired_curvature, self.calibrated_pose, curvature_limited) # TODO what if not available - actuators.curvature = float(curvature) + actuators.curvature = float(curvature) if self.enable_curvature_controller else self.desired_curvature actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) # Ensure no NaNs/Infs From 7e6b6039a5e31b92a4e86f74a123e3248304b6b5 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 11:02:23 +0200 Subject: [PATCH 4/6] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index f52dc1d94..62c066621 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -5,6 +5,8 @@ from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController +CURVATURE_SATURATION_THRESHOLD = 0.005 + class LatControlCurvature(LatControl): def __init__(self, CP, CP_SP, CI): @@ -39,6 +41,6 @@ class LatControlCurvature(LatControl): 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(self.curvature_max - abs(output_curvature) < 1e-5, CS, steer_limited_by_controls, curvature_limited)) + pid_log.saturated = bool(self._check_saturation(self.curvature_max - abs(output_curvature) < CURVATURE_SATURATION_THRESHOLD, CS, steer_limited_by_controls, curvature_limited)) return 0.0, 0.0, output_curvature, pid_log From d82945a1da2eb5e13e28e89cb1122a462521c6c7 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 11:05:04 +0200 Subject: [PATCH 5/6] Update controlsd.py --- selfdrive/controls/controlsd.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index bc5bf4802..a0c46f73a 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -18,7 +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 +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 @@ -220,7 +220,7 @@ class Controls(ControlsExt): self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED: - self.steer_limited_by_controls = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > 1e-5 + self.steer_limited_by_controls = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > CURVATURE_SATURATION_THRESHOLD else: self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 From 6922caa7bd5c4a97e03e265e1818a38c014c7048 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Mon, 9 Jun 2025 11:06:03 +0200 Subject: [PATCH 6/6] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 62c066621..c35cf337b 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -5,7 +5,7 @@ from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController -CURVATURE_SATURATION_THRESHOLD = 0.005 +CURVATURE_SATURATION_THRESHOLD = 0.4e-5 # rad/m class LatControlCurvature(LatControl):