Continuously update offset between TSS2 angle sensors (#21943)

* Continuously update offset between TSS2 angle sensors

* add comment

* less lines

* only when initialized

* init to None

* update ref
old-commit-hash: c15a616ac4

common/filter_simple.py

@@ -1,13 +1,18 @@
 class FirstOrderFilter:
   # first order filter
-  def __init__(self, x0, rc, dt):
+  def __init__(self, x0, rc, dt, initialized=True):
     self.x = x0
     self.dt = dt
     self.update_alpha(rc)
+    self.initialized = initialized
 
   def update_alpha(self, rc):
     self.alpha = self.dt / (rc + self.dt)
 
   def update(self, x):
-    self.x = (1. - self.alpha) * self.x + self.alpha * x
+    if self.initialized:
+      self.x = (1. - self.alpha) * self.x + self.alpha * x
+    else:
+      self.initialized = True
+      self.x = x
     return self.x