diff --git a/common/filter_simple.py b/common/filter_simple.py index 9ea6fe3070..212e1a8f40 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -15,3 +15,20 @@ class FirstOrderFilter: self.initialized = True self.x = x return self.x + + +class BounceFilter(FirstOrderFilter): + def __init__(self, x0, rc, dt, initialized=True, bounce=2): + self.velocity = FirstOrderFilter(0.0, 0.15, dt) + self.bounce = bounce + super().__init__(x0, rc, dt, initialized) + + def update(self, x): + super().update(x) + scale = self.dt / (1.0 / 60.0) # tuned at 60 fps + self.velocity.x += (x - self.x) * self.bounce * scale * self.dt + self.velocity.update(0.0) + if abs(self.velocity.x) < 1e-5: + self.velocity.x = 0.0 + self.x += self.velocity.x + return self.x