From 2287d96630b53d3b8a2fbd2e40ea77a7607d7482 Mon Sep 17 00:00:00 2001
From: infiniteCable <75014343+infiniteCable@users.noreply.github.com>
Date: Tue, 8 Apr 2025 20:15:43 +0200
Subject: [PATCH] Update disturbance_controller.py

---
 selfdrive/controls/lib/disturbance_controller.py | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py
index 737c1c6a6..976bd7b83 100644
--- a/selfdrive/controls/lib/disturbance_controller.py
+++ b/selfdrive/controls/lib/disturbance_controller.py
@@ -1,6 +1,7 @@
 import numpy as np
 import math
 
+from collections import deque
 from openpilot.common.pid import PIDController
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
@@ -9,17 +10,20 @@ ALPHA_MIN = 0.004
 ALPHA_MAX = 0.4
 
 class DisturbanceController:
-  def __init__(self):
+  def __init__(self, CP):
     self.lowpass_filtered = 0.0
     self.alpha_prev = ALPHA_MIN
     self.desired_curvature_prev = 0.0
     self.pid = PIDController(1, 0, k_f=0, pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE)
+    self.reaction_hist = deque([0.0], maxlen=int(round(CP.steerActuatorDelay / DT_CTRL))+1)
 
   def reset(self):
     self.lowpass_filtered = 0.0
     self.alpha_prev = ALPHA_MIN
     self.desired_curvature_prev = 0.0
     self.pid.reset()
+    self.reaction_hist.clear()
+    self.reaction_hist.append(0.0)
 
   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
@@ -53,9 +57,12 @@ class DisturbanceController:
                       
     alpha = self.compute_dynamic_alpha(desired_curvature)
     reaction = self.lowpass_filter(actual_curvature, alpha)
+    self.reaction_hist.append(reaction)
     disturbance = self.highpass_filter(actual_curvature, reaction)
 
-    error = desired_curvature - disturbance
+    reaction_delayed = self.reaction_hist[0]
+    error = desired_curvature - (reaction_delayed + disturbance)
+
     output_curvature = self.pid.update(error, feedforward=desired_curvature, speed=CS.vEgo)
     
     return float(output_curvature)