From 085ebe148a0bae9113fb115d6f3cd2fdce1de559 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 3 Sep 2025 17:25:55 +0200 Subject: [PATCH] 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