Update disturbance_controller.py

selfdrive/controls/lib/disturbance_controller.py

@@ -13,6 +13,12 @@ class DisturbanceController:
     self.desired_curvature_prev = 0.0
     self.pid = PIDController(1, 1, k_f=0, pos_limit=0.2, neg_limit=-0.2)
 
+  def reset(self):
+    self.lowpass_filtered = 0.0
+    self.alpha_prev = ALPHA_MIN
+    self.desired_curvature_prev = 0.0
+    self.pid.reset()
+
   def compute_dynamic_alpha(self, desired_curvature, dt=DT_CTRL, A=0.02, n=2.0, beta=3.0, k=2.0):
     d_desired = abs(desired_curvature - self.desired_curvature_prev) / dt
     alpha_reactive = d_desired**n / (k * A) if A > 0 else 0.0