From 26ee33bffdf65d8ff158a70c96ea5d848170a52b Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Sun, 6 Apr 2025 18:02:43 +0200 Subject: [PATCH] Update disturbance_controller.py --- selfdrive/controls/lib/disturbance_controller.py | 6 ++++++ 1 file changed, 6 insertions(+) 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