try a deque for smoothing

commaai · Dec 3, 2024 · 89e3b42 · 89e3b42
1 parent 0cd215e
commit 89e3b42
Showing 1 changed file with 20 additions and 10 deletions.
diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py
@@ -76,7 +76,8 @@ def __init__(self, dbc_names, CP):
 
     self.error_rate = FirstOrderFilter(0.0, 0.5, DT_CTRL * 3)
     self.error_rate2 = FirstOrderFilter(0.0, 1, DT_CTRL * 3)
-    self.error_rate3 = FirstOrderFilter(0.0, 1, DT_CTRL)
+    self.error_rate3 = FirstOrderFilter(0.0, 0.75, DT_CTRL)
+    self.error_rate4 = FirstOrderFilter(0.0, 0.15, DT_CTRL)
     self.d = deque([0.0] * 100, maxlen=100)
     self.prev_error = 0.0
     self.prev_error2 = 0.0
@@ -297,7 +298,7 @@ def update(self, CC, CS, now_nanos):
             self.prev_error = error
 
             error = pcm_accel_cmd - future_aego
-            pcm_accel_cmd = self.long_pid.update(error, error_rate=self.error_rate.x,
+            pcm_accel_cmd = self.long_pid.update(error, error_rate=self.error_rate4.x,  # self.error_rate.x,
                                                  speed=CS.out.vEgo,
                                                  feedforward=pcm_accel_cmd)
           else:
@@ -306,6 +307,14 @@ def update(self, CC, CS, now_nanos):
             self.error_rate2.x = 0.0
             self.prev_error = 0.0
 
+        error = pcm_accel_cmd - a_ego_blended
+        # self.error_rate3.update((error - self.prev_error2) / (DT_CTRL))
+        # self.debug3 = self.error_rate3.x
+        self.debug3 = (error - self.d[-15 // 3]) / (DT_CTRL * 25)
+        self.debug3 = self.error_rate4.update(self.debug3)
+        self.prev_error2 = error
+        self.d.append(error)
+
         self.debug = self.error_rate.x
         self.debug2 = self.error_rate2.x
 
@@ -362,14 +371,15 @@ def update(self, CC, CS, now_nanos):
     if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
       can_sends.append(make_tester_present_msg(0x750, 0, 0xF))
 
-    pcm_accel_cmd = actuators.accel
-    error = pcm_accel_cmd - a_ego_blended
-    self.error_rate3.update((error - self.prev_error2) / (DT_CTRL))
-    self.d.append(error)
-    # self.debug3 = self.error_rate3.x
-    if self.frame % 3 == 0:
-      self.debug3 = (error - self.d[-100])
-    self.prev_error2 = error
+    # pcm_accel_cmd = actuators.accel
+    # error = pcm_accel_cmd - a_ego_blended
+    # self.error_rate3.update((error - self.prev_error2) / (DT_CTRL))
+    # self.d.append(error)
+    # # self.debug3 = self.error_rate3.x
+    # if self.frame % 3 == 0:
+    #   self.debug3 = (error - self.d[-10]) / (DT_CTRL * 10)
+    #   # self.debug3 = self.error_rate4.update(self.debug3)
+    # self.prev_error2 = error
 
     new_actuators = actuators.as_builder()
     new_actuators.steer = apply_steer / self.params.STEER_MAX