fCircularBuffer.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "Filter.h"
00004 
00005 fCircularBuffer::fCircularBuffer(int length, bool use_median) {
00006     _length = length;
00007     _use_median = use_median;
00008     
00009     oldest_index = 0;
00010     newest_index = -1;
00011     num = 0;
00012     sum = 0.0f;
00013     
00014     buf = (float*)malloc(_length * sizeof(float));
00015     sorted = (float*)malloc(_length * sizeof(float));
00016     for (int i = 0; i < _length; i++) {
00017         buf[i] = 0.0f;
00018         sorted[i] = 0.0f;
00019     }
00020 }
00021 
00022 float &fCircularBuffer::at(int index) {
00023     int actual = oldest_index + index;
00024     if (actual >= _length) actual -= _length;
00025     return buf[actual];
00026 }
00027 
00028 void fCircularBuffer::add(float x) {
00029     if (num < _length) {
00030         newest_index++;
00031         buf[newest_index] = x;
00032         sum += x;
00033         num++;
00034 
00035         if (!_use_median || _length < 4) return;
00036 
00037         /*insert x into sorted array*/
00038         int i = num - 1;
00039         while (i > 0 && sorted[i - 1] > x) {
00040             sorted[i] = sorted[i - 1];
00041             i--;
00042         }
00043         sorted[i] = x;
00044     } else {
00045         /*update circular buffer*/
00046         float oldest = buf[oldest_index];
00047 
00048         sum -= buf[oldest_index];
00049         oldest_index++;
00050         if (oldest_index >= _length) oldest_index -= _length;
00051 
00052         newest_index++;
00053         if (newest_index >= _length) newest_index -= _length;
00054         buf[newest_index] = x;
00055 
00056         sum += x;
00057 
00058         if (!_use_median || _length < 4) return;
00059 
00060         /*find sorted index of oldest element*/
00061         int removed;
00062         for (removed = 0; removed < _length; removed++) {
00063             if (sorted[removed] == oldest) break;
00064         }
00065 
00066         /*insert x*/
00067         int i;
00068         if (removed == _length - 1) {
00069             i = _length - 1;
00070             while (i > 0 && sorted[i - 1] > x) {
00071                 sorted[i] = sorted[i - 1];
00072                 i--;
00073             }
00074             sorted[i] = x;
00075         } else if (removed == 0) {
00076             i = 0;
00077             while (i < _length - 1 && sorted[i + 1] < x) {
00078                 sorted[i] = sorted[i + 1];
00079                 i++;
00080             }
00081             sorted[i] = x;
00082         } else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
00083             sorted[removed] = x;
00084         } else if (sorted[removed - 1] > x) {
00085             i = removed;
00086             while (i > 0 && sorted[i - 1] > x) {
00087                 sorted[i] = sorted[i - 1];
00088                 i--;
00089             }
00090             sorted[i] = x;
00091         } else {
00092             i = removed;
00093             while (i < _length - 1 && sorted[i + 1] < x) {
00094                 sorted[i] = sorted[i + 1];
00095                 i++;
00096             }
00097             sorted[i] = x;
00098         }
00099     }
00100 }
00101 
00102 float fCircularBuffer::mean() {
00103     return sum / num;
00104 }
00105 
00106 float median3(float *buf) {
00107     if (buf[0] > buf[1]) {
00108         if (buf[1] > buf[2]) {
00109             return buf[1];
00110         } else if (buf[0] > buf[2]) {
00111             return buf[2];
00112         } else {
00113             return buf[0];
00114         }
00115     } else {
00116         if (buf[0] > buf[2]) {
00117             return buf[0];
00118         } else if (buf[1] > buf[2]) {
00119             return buf[2];
00120         } else {
00121             return buf[1];
00122         }
00123     }
00124 }
00125 float fCircularBuffer::median() {
00126     if (_length == 1) {
00127         return buf[0];
00128     }
00129     if (_length == 2) {
00130         if (num < 2) return buf[0];
00131         return (buf[0] + buf[1]) / 2.0f;
00132     }
00133     if (_length == 3) {
00134         if (num < 2) return buf[0];
00135         if (num == 2) return (buf[0] + buf[1]) / 2.0f;
00136         return median3(buf);
00137     }
00138     if (num < _length) {
00139         if (num % 2 == 1) {
00140             return sorted[(num - 1) / 2];
00141         } else {
00142             return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
00143         }
00144     }
00145     else {
00146         if (_length % 2 == 1) {
00147             return sorted[(_length - 1) / 2];
00148         } else {
00149             return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
00150         }
00151     }
00152 }