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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index e2e9488a1..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 @@ -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 @@ -218,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 diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index f52dc1d94..c35cf337b 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.4e-5 # rad/m + 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 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"),