diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py index cfaa37303..d3b66673b 100644 --- a/selfdrive/controls/lib/disturbance_controller.py +++ b/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