From a756987ca1c42b9dea9d5386adcd883f8269aef7 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Tue, 8 Apr 2025 21:00:55 +0200 Subject: [PATCH] Update controlsd.py --- selfdrive/controls/controlsd.py | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 45ad56009..fbc5dcf35 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -19,7 +19,6 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque 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 -from selfdrive.controls.lib.disturbance_controller import DisturbanceController State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState @@ -52,9 +51,6 @@ class Controls: self.desired_curvature = 0.0 self.roll = 0.0 - self.enable_disturbance_correction = self.params.get_bool("EnableDisturbanceCorrection") - self.disturbance_controller = DisturbanceController(self.CP) - self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -86,7 +82,7 @@ class Controls: self.VM.update_params(x, sr) steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) - self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, 0) + self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) self.roll = lp.roll # Update Torque Params @@ -127,7 +123,6 @@ class Controls: if not CC.latActive: self.LaC.reset() - self.disturbance_controller.reset() if not CC.longActive: self.LoC.reset() @@ -138,8 +133,6 @@ class Controls: # Steering PID loop and lateral MPC # Reset desired curvature to current to avoid violating the limits on engage new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature - if self.enable_disturbance_correction: - new_desired_curvature = self.disturbance_controller.compensate(CS, self.VM, lp, self.calibrated_pose, 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