diff --git a/common/filter_simple.py b/common/filter_simple.py index 9ea6fe3070..24de3447f7 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -1,3 +1,6 @@ +import numpy as np + + class FirstOrderFilter: def __init__(self, x0, rc, dt, initialized=True): self.x = x0 @@ -15,3 +18,41 @@ class FirstOrderFilter: self.initialized = True self.x = x return self.x + + +class BounceFilter(FirstOrderFilter): + def __init__(self, x0, rc, dt, initialized=True): + super().__init__(x0, rc, dt, initialized) + # self.velocity = FirstOrderFilter(0.0, rc * 1.5, dt) + self.velocity = FirstOrderFilter(0.0, 0.1, dt, initialized) + + def update(self, x): + prev_x = self.x + # new_x = super().update(x) + self.velocity.update((x - prev_x) / 10) + # self.velocity.x *= 0.9 + self.velocity.update(0.0) + # self.velocity.update(0.0) + self.x += self.velocity.x + print(self.velocity.x) + # self.x = np.interp(abs(vel), [0, 100], [prev_x, new_x]) + # print(vel) + # new_x += vel + # return new_x + # new_x = super().update(x) + # slow the initial move with velocity + return self.x + + # + # def update(self, x): + # prev_x = self.x + # new_x = super().update(x) + # vel = self.velocity.update(x - prev_x) + # print(vel) + # self.x = np.interp(abs(vel), [0, 100], [prev_x, new_x]) + # # print(vel) + # # new_x += vel + # # return new_x + # # new_x = super().update(x) + # # slow the initial move with velocity + # return self.x diff --git a/system/ui/widgets/network.py b/system/ui/widgets/network.py index b5124a3709..c32ec7b77b 100644 --- a/system/ui/widgets/network.py +++ b/system/ui/widgets/network.py @@ -3,7 +3,7 @@ from functools import partial from typing import cast import pyray as rl -from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.common.filter_simple import FirstOrderFilter, BounceFilter from openpilot.system.ui.lib.application import gui_app, DEFAULT_FPS from openpilot.system.ui.lib.scroll_panel import GuiScrollPanel from openpilot.system.ui.lib.wifi_manager import WifiManager, SecurityType, Network, MeteredType @@ -47,12 +47,16 @@ class NavButton(Widget): super().__init__() self.text = text self.set_rect(rl.Rectangle(0, 0, 400, 100)) - self._x_pos_filter = FirstOrderFilter(0.0, 0.05, 1 / DEFAULT_FPS, initialized=False) - self._y_pos_filter = FirstOrderFilter(0.0, 0.05, 1 / DEFAULT_FPS, initialized=False) + # self._x_pos_filter = FirstOrderFilter(0.0, 0.05, 1 / DEFAULT_FPS, initialized=False) + # self._y_pos_filter = FirstOrderFilter(0.0, 0.05, 1 / DEFAULT_FPS, initialized=False) + self._x_pos_filter = BounceFilter(0.0, 0.1, 1 / DEFAULT_FPS, initialized=False) + self._y_pos_filter = BounceFilter(0.0, 0.1, 1 / DEFAULT_FPS, initialized=False) + # self._x_pos_filter = SecondOrderFilter(0.0, response=0.05, dt=1 / DEFAULT_FPS, initialized=False) + # self._y_pos_filter = SecondOrderFilter(0.0, response=0.28, dt=1 / DEFAULT_FPS, initialized=False) def set_position(self, x: float, y: float) -> None: x = self._x_pos_filter.update(x) - y = self._y_pos_filter.update(y) + y = y # self._y_pos_filter.update(y) changed = (self._rect.x != x or self._rect.y != y) self._rect.x, self._rect.y = x, y if changed: