High-pass filter (#2816)

* commit

* flip

* can revert this back to how it was before 0fe56bc289

* chadder says this is a bandpass

opendbc/car/common/filter_simple.py

@@ -2,17 +2,43 @@ class FirstOrderFilter:
   # first order filter
   def __init__(self, x0, rc, dt, initialized=True):
     self.x = x0
-    self.dt = dt
+    self._dt = dt
     self.update_alpha(rc)
     self.initialized = initialized
 
+  def update_dt(self, dt):
+    self._dt = dt
+    self.update_alpha(self._rc)
+
   def update_alpha(self, rc):
-    self.alpha = self.dt / (rc + self.dt)
+    self._rc = rc
+    self._alpha = self._dt / (self._rc + self._dt)
 
   def update(self, x):
     if self.initialized:
-      self.x = (1. - self.alpha) * self.x + self.alpha * x
+      self.x = (1. - self._alpha) * self.x + self._alpha * x
     else:
       self.initialized = True
       self.x = x
     return self.x
+
+
+class HighPassFilter:
+  # technically a band-pass filter
+  def __init__(self, x0, rc1, rc2, dt, initialized=True):
+    self.x = x0
+    self._f1 = FirstOrderFilter(x0, rc1, dt, initialized)
+    self._f2 = FirstOrderFilter(x0, rc2, dt, initialized)
+    assert rc2 > rc1, "rc2 must be greater than rc1"
+
+  def update_dt(self, dt):
+    self._f1.update_dt(dt)
+    self._f2.update_dt(dt)
+
+  def update_alpha(self, rc1, rc2):
+    self._f1.update_alpha(rc1)
+    self._f2.update_alpha(rc2)
+
+  def update(self, x):
+    self.x = self._f1.update(x) - self._f2.update(x)
+    return self.x

opendbc/car/toyota/carcontroller.py

@@ -4,7 +4,7 @@ from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, ACCEL
 from opendbc.car.lateral import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance
 from opendbc.car.can_definitions import CanData
 from opendbc.car.carlog import carlog
-from opendbc.car.common.filter_simple import FirstOrderFilter
+from opendbc.car.common.filter_simple import FirstOrderFilter, HighPassFilter
 from opendbc.car.common.pid import PIDController
 from opendbc.car.secoc import add_mac, build_sync_mac
 from opendbc.car.interfaces import CarControllerBase
@@ -65,8 +65,8 @@ class CarController(CarControllerBase):
     # *** start long control state ***
     self.long_pid = get_long_tune(self.CP, self.params)
     self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
-    self.pitch = FirstOrderFilter(0, 0.25, DT_CTRL)
-    self.pitch_slow = FirstOrderFilter(0, 1.5, DT_CTRL)
+    self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
+    self.pitch_hp = HighPassFilter(0.0, 0.25, 1.5, DT_CTRL)
 
     self.accel = 0
     self.prev_accel = 0
@@ -87,7 +87,7 @@ class CarController(CarControllerBase):
 
     if len(CC.orientationNED) == 3:
       self.pitch.update(CC.orientationNED[1])
-      self.pitch_slow.update(CC.orientationNED[1])
+      self.pitch_hp.update(CC.orientationNED[1])
 
     # *** control msgs ***
     can_sends = []
@@ -228,8 +228,7 @@ class CarController(CarControllerBase):
           if not stopping:
             # Toyota's PCM slowly responds to changes in pitch. On change, we amplify our
             # acceleration request to compensate for the undershoot and following overshoot
-            high_pass_pitch = self.pitch.x - self.pitch_slow.x
-            pitch_compensation = float(np.clip(math.sin(high_pass_pitch) * ACCELERATION_DUE_TO_GRAVITY,
+            pitch_compensation = float(np.clip(math.sin(self.pitch_hp.x) * ACCELERATION_DUE_TO_GRAVITY,
                                                -MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
             pcm_accel_cmd += pitch_compensation