Diff: Filter/Filter.cpp

Revision:

144:a9e7fa1c98d7

Child:

145:37ffa3ba3862

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Filter/Filter.cpp	Wed May 03 12:24:28 2017 +0000
@@ -0,0 +1,121 @@
+#include "mbed.h"
+#include "math.h"
+#include "Filter.h"
+
+using namespace std;
+
+CircularBuffer::CircularBuffer(int length) {
+    _length = length;
+    oldest_index = 0;
+    newest_index = -1;
+    num = 0;
+    sum = 0.0f;
+
+    buf = (float*)malloc(_length * sizeof(float));
+    sorted = (float*)malloc(_length * sizeof(float));
+    for (int i = 0; i < _length; i++) {
+        buf[i] = 0.0f;
+        sorted[i] = 0.0f;
+    }
+}
+
+float &CircularBuffer::at(int index) {
+    int actual = oldest_index + index;
+    if (actual >= _length) actual -= _length;
+    return buf[actual];
+}
+
+void CircularBuffer::add(float x) {
+    if (num < _length) {
+        newest_index++;
+        buf[newest_index] = x;
+        sum += x;
+
+        /*insert x into sorted array*/
+        int i = num;
+        while (i > 0 && sorted[i] > x) {
+            sorted[i] = sorted[i - 1];
+            i--;
+        }
+        sorted[i] = x;
+
+        num++;
+    }
+    else {
+        /*find sorted index of oldest element*/
+        int removed;
+        for (removed = 0; removed < _length; removed++) {
+            if (sorted[removed] == buf[oldest_index]) break;
+        }
+
+        /*update circular buffer*/
+        sum -= buf[oldest_index];
+        oldest_index++;
+        if (oldest_index >= _length) oldest_index -= _length;
+        newest_index++;
+        if (newest_index >= _length) newest_index -= _length;
+        buf[newest_index] = x;
+        sum += x;
+
+        /*insert x*/
+        int i;
+        if (removed == _length - 1) {
+            i = _length - 1;
+            while (i > 0 && sorted[i] > x) {
+                sorted[i] = sorted[i - 1];
+                i--;
+            }
+            sorted[i] = x;
+        }
+        else if (removed == 0) {
+            i = 0;
+            while (i < _length - 1 && sorted[i] < x) {
+                sorted[i] = sorted[i + 1];
+                i++;
+            }
+            sorted[i] = x;
+        }
+        else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
+            sorted[removed] = x;
+        }
+        else if (sorted[removed - 1] > x) {
+            i = removed;
+            while (i > 0 && sorted[i] > x) {
+                sorted[i] = sorted[i - 1];
+                i--;
+            }
+            sorted[i] = x;
+        }
+        else {
+            i = removed;
+            while (i < _length - 1 && sorted[i] < x) {
+                sorted[i] = sorted[i + 1];
+                i++;
+            }
+            sorted[i] = x;
+        }
+    }
+}
+
+float CircularBuffer::mean() {
+    return sum / num;
+}
+
+float CircularBuffer::median() {
+    if (num < _length) {
+        if (num % 2 == 1) {
+            return sorted[(num - 1) / 2];
+        }
+        else {
+            return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
+        }
+    }
+    else {
+        if (_length % 2 == 1) {
+            return sorted[(_length - 1) / 2];
+        }
+        else {
+            return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
+        }
+    }
+}
\ No newline at end of file