IQ.Pilot Release Commit @ 6d177d4

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

common/params.h

@@ -63,6 +63,14 @@ public:
   inline bool getBool(const std::string &key, bool block = false) {
     return get(key, block) == "1";
   }
+  inline int getInt(const std::string &key, bool block = false) {
+    std::string value = get(key, block);
+    return value.empty() ? 0 : std::stoi(value);
+  }
+  inline float getFloat(const std::string &key, bool block = false) {
+    std::string value = get(key, block);
+    return value.empty() ? 0.0F : std::stof(value);
+  }
   std::map<std::string, std::string> readAll();
 
   // helpers for writing values
@@ -73,10 +81,24 @@ public:
   inline int putBool(const std::string &key, bool val) {
     return put(key.c_str(), val ? "1" : "0", 1);
   }
+  inline int putInt(const std::string &key, int val) {
+    const std::string value = std::to_string(val);
+    return put(key.c_str(), value.c_str(), value.size());
+  }
+  inline int putFloat(const std::string &key, float val) {
+    const std::string value = std::to_string(val);
+    return put(key.c_str(), value.c_str(), value.size());
+  }
   void putNonBlocking(const std::string &key, const std::string &val);
   inline void putBoolNonBlocking(const std::string &key, bool val) {
     putNonBlocking(key, val ? "1" : "0");
   }
+  inline void putIntNonBlocking(const std::string &key, int val) {
+    putNonBlocking(key, std::to_string(val));
+  }
+  inline void putFloatNonBlocking(const std::string &key, float val) {
+    putNonBlocking(key, std::to_string(val));
+  }
 
 private:
   void asyncWriteThread();

common/params.py

@@ -64,6 +64,14 @@ except ImportError:
       except (FileNotFoundError, NotADirectoryError, IsADirectoryError):
         return False
 
+    def get_int(self, key, block: bool = False) -> int:
+      value = self.get(key, block=block)
+      return int(value) if value else 0
+
+    def get_float(self, key, block: bool = False) -> float:
+      value = self.get(key, block=block)
+      return float(value) if value else 0.0
+
     def put(self, key, dat):
       if isinstance(dat, str):
         dat = dat.encode("utf-8")
@@ -80,12 +88,24 @@ except ImportError:
     def put_bool(self, key, val: bool):
       self.put(key, b"1" if val else b"0")
 
+    def put_int(self, key, val: int):
+      self.put(key, str(val))
+
+    def put_float(self, key, val: float):
+      self.put(key, str(val))
+
     def put_nonblocking(self, key, dat):
       self.put(key, dat)
 
     def put_bool_nonblocking(self, key, val: bool):
       self.put_bool(key, val)
 
+    def put_int_nonblocking(self, key, val: int):
+      self.put_int(key, val)
+
+    def put_float_nonblocking(self, key, val: float):
+      self.put_float(key, val)
+
     def remove(self, key):
       try:
         os.remove(self._p(key))

common/params_pyx.pyx

@@ -33,11 +33,17 @@ cdef extern from "common/params.h":
     c_Params(string) except + nogil
     string get(string, bool) nogil
     bool getBool(string, bool) nogil
+    int getInt(string, bool) nogil
+    float getFloat(string, bool) nogil
     int remove(string) nogil
     int put(string, string) nogil
     void putNonBlocking(string, string) nogil
     void putBoolNonBlocking(string, bool) nogil
     int putBool(string, bool) nogil
+    int putInt(string, int) nogil
+    int putFloat(string, float) nogil
+    void putIntNonBlocking(string, int) nogil
+    void putFloatNonBlocking(string, float) nogil
     bool checkKey(string) nogil
     ParamKeyType getKeyType(string) nogil
     optional[string] getKeyDefaultValue(string) nogil
@@ -136,6 +142,20 @@ cdef class Params:
       r = self.p.getBool(k, block)
     return r
 
+  def get_int(self, key, bool block=False):
+    cdef string k = self.check_key(key)
+    cdef int r
+    with nogil:
+      r = self.p.getInt(k, block)
+    return r
+
+  def get_float(self, key, bool block=False):
+    cdef string k = self.check_key(key)
+    cdef float r
+    with nogil:
+      r = self.p.getFloat(k, block)
+    return r
+
   def _put_cast(self, key, dat):
     cdef string k = self.check_key(key)
     cdef ParamKeyType t = self.p.getKeyType(k)
@@ -158,6 +178,16 @@ cdef class Params:
     with nogil:
       self.p.putBool(k, val)
 
+  def put_int(self, key, int val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putInt(k, val)
+
+  def put_float(self, key, float val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putFloat(k, val)
+
   def put_nonblocking(self, key, dat):
     cdef string k = self.check_key(key)
     cdef string dat_bytes = self._put_cast(key, dat)
@@ -169,6 +199,16 @@ cdef class Params:
     with nogil:
       self.p.putBoolNonBlocking(k, val)
 
+  def put_int_nonblocking(self, key, int val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putIntNonBlocking(k, val)
+
+  def put_float_nonblocking(self, key, float val):
+    cdef string k = self.check_key(key)
+    with nogil:
+      self.p.putFloatNonBlocking(k, val)
+
   def remove(self, key):
     cdef string k = self.check_key(key)
     with nogil: