Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 17:58:39 by 1.7.2