IQ.Pilot Release Commit @ b79954e

common/filter_simple.py

@@ -1,3 +1,8 @@
+from collections import deque
+
+import numpy as np
+
+
 class FirstOrderFilter:
   def __init__(self, x0, rc, dt, initialized=True):
     self.x = x0
@@ -32,3 +37,35 @@ class BounceFilter(FirstOrderFilter):
       self.velocity.x = 0.0
     self.x += self.velocity.x
     return self.x
+
+
+class MyMovingAverage:
+  def __init__(self, window_size, value=None):
+    self.window_size = window_size
+    if value is not None:
+      self.values = deque([value] * window_size, maxlen=window_size)
+      self.sum = value * window_size
+      self.result = value
+    else:
+      self.values = deque(maxlen=window_size)
+      self.sum = 0
+      self.result = 0
+
+  def set(self, value):
+    self.values.clear()
+    self.values.append(value)
+    self.sum = value
+    self.result = value
+    return value
+
+  def set_all(self, value):
+    self.values = deque([value] * self.window_size, maxlen=self.window_size)
+    self.sum = value * self.window_size
+    self.result = value
+    return value
+
+  def process(self, value, median=False):
+    self.values.append(value)
+    self.sum = sum(self.values)
+    self.result = float(np.median(self.values)) if median else float(self.sum) / len(self.values)
+    return self.result