diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4be345374..81bac7f8b 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -20,6 +20,8 @@ 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 +from openpilot.common.pt2 import PT2Filter +from openpilot.common.realtime import DT_CTRL State = log.SelfdriveState.OpenpilotState LaneChangeState = log.LaneChangeState @@ -54,6 +56,8 @@ class Controls: self.enable_disturbance_correction = self.params.get_bool("EnableDisturbanceCorrection") self.disturbance_controller = DisturbanceController(self.CP) + self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer") + self.smooth_steer = PT2Filter(46.0, 1.0, DT_CTRL) self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -128,6 +132,7 @@ class Controls: if not CC.latActive: self.LaC.reset() self.disturbance_controller.reset() + self.smooth_steer.reset() if not CC.longActive: self.LoC.reset() @@ -138,6 +143,8 @@ 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_smooth_steer: + new_desired_curvature = self.smooth_steer.update(new_desired_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)