Diff: Filter/CircularBuffer.cpp

Revision:

149:c51c0258c923

Parent:

148:9bca96f7be5c

--- a/Filter/CircularBuffer.cpp	Wed May 03 13:59:51 2017 +0000
+++ b/Filter/CircularBuffer.cpp	Thu May 04 12:54:00 2017 +0000
@@ -32,7 +32,7 @@
         sum += x;
         num++;
 
-        if (!_use_median) return;
+        if (!_use_median || _length < 4) return;
 
         /*insert x into sorted array*/
         int i = num - 1;
@@ -55,7 +55,7 @@
 
         sum += x;
 
-        if (!_use_median) return;
+        if (!_use_median || _length < 4) return;
 
         /*find sorted index of oldest element*/
         int removed;
@@ -103,7 +103,38 @@
     return sum / num;
 }
 
+float median3(float *buf) {
+    if (buf[0] > buf[1]) {
+        if (buf[1] > buf[2]) {
+            return buf[1];
+        } else if (buf[0] > buf[2]) {
+            return buf[2];
+        } else {
+            return buf[0];
+        }
+    } else {
+        if (buf[0] > buf[2]) {
+            return buf[0];
+        } else if (buf[1] > buf[2]) {
+            return buf[2];
+        } else {
+            return buf[1];
+        }
+    }
+}
 float CircularBuffer::median() {
+    if (_length == 1) {
+        return buf[0];
+    }
+    if (_length == 2) {
+        if (num < 2) return buf[0];
+        return (buf[0] + buf[1]) / 2.0f;
+    }
+    if (_length == 3) {
+        if (num < 2) return buf[0];
+        if (num == 2) return (buf[0] + buf[1]) / 2.0f;
+        return median3(buf);
+    }
     if (num < _length) {
         if (num % 2 == 1) {
             return sorted[(num - 1) / 2];