Update controlsd.py

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)