Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Filter/CircularBuffer.cpp@145:37ffa3ba3862, 2017-05-03 (annotated)
- Committer:
- bwang
- Date:
- Wed May 03 12:28:05 2017 +0000
- Revision:
- 145:37ffa3ba3862
- Child:
- 147:c1b2379b8874
05/03/2017 08:27 - CircularBuffer moved to its own .cpp file
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bwang | 145:37ffa3ba3862 | 1 | #include "mbed.h" |
bwang | 145:37ffa3ba3862 | 2 | #include "math.h" |
bwang | 145:37ffa3ba3862 | 3 | #include "Filter.h" |
bwang | 145:37ffa3ba3862 | 4 | |
bwang | 145:37ffa3ba3862 | 5 | CircularBuffer::CircularBuffer(int length) { |
bwang | 145:37ffa3ba3862 | 6 | _length = length; |
bwang | 145:37ffa3ba3862 | 7 | oldest_index = 0; |
bwang | 145:37ffa3ba3862 | 8 | newest_index = -1; |
bwang | 145:37ffa3ba3862 | 9 | num = 0; |
bwang | 145:37ffa3ba3862 | 10 | sum = 0.0f; |
bwang | 145:37ffa3ba3862 | 11 | |
bwang | 145:37ffa3ba3862 | 12 | buf = (float*)malloc(_length * sizeof(float)); |
bwang | 145:37ffa3ba3862 | 13 | sorted = (float*)malloc(_length * sizeof(float)); |
bwang | 145:37ffa3ba3862 | 14 | for (int i = 0; i < _length; i++) { |
bwang | 145:37ffa3ba3862 | 15 | buf[i] = 0.0f; |
bwang | 145:37ffa3ba3862 | 16 | sorted[i] = 0.0f; |
bwang | 145:37ffa3ba3862 | 17 | } |
bwang | 145:37ffa3ba3862 | 18 | } |
bwang | 145:37ffa3ba3862 | 19 | |
bwang | 145:37ffa3ba3862 | 20 | float &CircularBuffer::at(int index) { |
bwang | 145:37ffa3ba3862 | 21 | int actual = oldest_index + index; |
bwang | 145:37ffa3ba3862 | 22 | if (actual >= _length) actual -= _length; |
bwang | 145:37ffa3ba3862 | 23 | return buf[actual]; |
bwang | 145:37ffa3ba3862 | 24 | } |
bwang | 145:37ffa3ba3862 | 25 | |
bwang | 145:37ffa3ba3862 | 26 | void CircularBuffer::add(float x) { |
bwang | 145:37ffa3ba3862 | 27 | if (num < _length) { |
bwang | 145:37ffa3ba3862 | 28 | newest_index++; |
bwang | 145:37ffa3ba3862 | 29 | buf[newest_index] = x; |
bwang | 145:37ffa3ba3862 | 30 | sum += x; |
bwang | 145:37ffa3ba3862 | 31 | |
bwang | 145:37ffa3ba3862 | 32 | /*insert x into sorted array*/ |
bwang | 145:37ffa3ba3862 | 33 | int i = num; |
bwang | 145:37ffa3ba3862 | 34 | while (i > 0 && sorted[i] > x) { |
bwang | 145:37ffa3ba3862 | 35 | sorted[i] = sorted[i - 1]; |
bwang | 145:37ffa3ba3862 | 36 | i--; |
bwang | 145:37ffa3ba3862 | 37 | } |
bwang | 145:37ffa3ba3862 | 38 | sorted[i] = x; |
bwang | 145:37ffa3ba3862 | 39 | |
bwang | 145:37ffa3ba3862 | 40 | num++; |
bwang | 145:37ffa3ba3862 | 41 | } |
bwang | 145:37ffa3ba3862 | 42 | else { |
bwang | 145:37ffa3ba3862 | 43 | /*find sorted index of oldest element*/ |
bwang | 145:37ffa3ba3862 | 44 | int removed; |
bwang | 145:37ffa3ba3862 | 45 | for (removed = 0; removed < _length; removed++) { |
bwang | 145:37ffa3ba3862 | 46 | if (sorted[removed] == buf[oldest_index]) break; |
bwang | 145:37ffa3ba3862 | 47 | } |
bwang | 145:37ffa3ba3862 | 48 | |
bwang | 145:37ffa3ba3862 | 49 | /*update circular buffer*/ |
bwang | 145:37ffa3ba3862 | 50 | sum -= buf[oldest_index]; |
bwang | 145:37ffa3ba3862 | 51 | oldest_index++; |
bwang | 145:37ffa3ba3862 | 52 | if (oldest_index >= _length) oldest_index -= _length; |
bwang | 145:37ffa3ba3862 | 53 | newest_index++; |
bwang | 145:37ffa3ba3862 | 54 | if (newest_index >= _length) newest_index -= _length; |
bwang | 145:37ffa3ba3862 | 55 | buf[newest_index] = x; |
bwang | 145:37ffa3ba3862 | 56 | sum += x; |
bwang | 145:37ffa3ba3862 | 57 | |
bwang | 145:37ffa3ba3862 | 58 | /*insert x*/ |
bwang | 145:37ffa3ba3862 | 59 | int i; |
bwang | 145:37ffa3ba3862 | 60 | if (removed == _length - 1) { |
bwang | 145:37ffa3ba3862 | 61 | i = _length - 1; |
bwang | 145:37ffa3ba3862 | 62 | while (i > 0 && sorted[i] > x) { |
bwang | 145:37ffa3ba3862 | 63 | sorted[i] = sorted[i - 1]; |
bwang | 145:37ffa3ba3862 | 64 | i--; |
bwang | 145:37ffa3ba3862 | 65 | } |
bwang | 145:37ffa3ba3862 | 66 | sorted[i] = x; |
bwang | 145:37ffa3ba3862 | 67 | } |
bwang | 145:37ffa3ba3862 | 68 | else if (removed == 0) { |
bwang | 145:37ffa3ba3862 | 69 | i = 0; |
bwang | 145:37ffa3ba3862 | 70 | while (i < _length - 1 && sorted[i] < x) { |
bwang | 145:37ffa3ba3862 | 71 | sorted[i] = sorted[i + 1]; |
bwang | 145:37ffa3ba3862 | 72 | i++; |
bwang | 145:37ffa3ba3862 | 73 | } |
bwang | 145:37ffa3ba3862 | 74 | sorted[i] = x; |
bwang | 145:37ffa3ba3862 | 75 | } |
bwang | 145:37ffa3ba3862 | 76 | else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) { |
bwang | 145:37ffa3ba3862 | 77 | sorted[removed] = x; |
bwang | 145:37ffa3ba3862 | 78 | } |
bwang | 145:37ffa3ba3862 | 79 | else if (sorted[removed - 1] > x) { |
bwang | 145:37ffa3ba3862 | 80 | i = removed; |
bwang | 145:37ffa3ba3862 | 81 | while (i > 0 && sorted[i] > x) { |
bwang | 145:37ffa3ba3862 | 82 | sorted[i] = sorted[i - 1]; |
bwang | 145:37ffa3ba3862 | 83 | i--; |
bwang | 145:37ffa3ba3862 | 84 | } |
bwang | 145:37ffa3ba3862 | 85 | sorted[i] = x; |
bwang | 145:37ffa3ba3862 | 86 | } |
bwang | 145:37ffa3ba3862 | 87 | else { |
bwang | 145:37ffa3ba3862 | 88 | i = removed; |
bwang | 145:37ffa3ba3862 | 89 | while (i < _length - 1 && sorted[i] < x) { |
bwang | 145:37ffa3ba3862 | 90 | sorted[i] = sorted[i + 1]; |
bwang | 145:37ffa3ba3862 | 91 | i++; |
bwang | 145:37ffa3ba3862 | 92 | } |
bwang | 145:37ffa3ba3862 | 93 | sorted[i] = x; |
bwang | 145:37ffa3ba3862 | 94 | } |
bwang | 145:37ffa3ba3862 | 95 | } |
bwang | 145:37ffa3ba3862 | 96 | } |
bwang | 145:37ffa3ba3862 | 97 | |
bwang | 145:37ffa3ba3862 | 98 | float CircularBuffer::mean() { |
bwang | 145:37ffa3ba3862 | 99 | return sum / num; |
bwang | 145:37ffa3ba3862 | 100 | } |
bwang | 145:37ffa3ba3862 | 101 | |
bwang | 145:37ffa3ba3862 | 102 | float CircularBuffer::median() { |
bwang | 145:37ffa3ba3862 | 103 | if (num < _length) { |
bwang | 145:37ffa3ba3862 | 104 | if (num % 2 == 1) { |
bwang | 145:37ffa3ba3862 | 105 | return sorted[(num - 1) / 2]; |
bwang | 145:37ffa3ba3862 | 106 | } |
bwang | 145:37ffa3ba3862 | 107 | else { |
bwang | 145:37ffa3ba3862 | 108 | return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f; |
bwang | 145:37ffa3ba3862 | 109 | } |
bwang | 145:37ffa3ba3862 | 110 | } |
bwang | 145:37ffa3ba3862 | 111 | else { |
bwang | 145:37ffa3ba3862 | 112 | if (_length % 2 == 1) { |
bwang | 145:37ffa3ba3862 | 113 | return sorted[(_length - 1) / 2]; |
bwang | 145:37ffa3ba3862 | 114 | } |
bwang | 145:37ffa3ba3862 | 115 | else { |
bwang | 145:37ffa3ba3862 | 116 | return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f; |
bwang | 145:37ffa3ba3862 | 117 | } |
bwang | 145:37ffa3ba3862 | 118 | } |
bwang | 145:37ffa3ba3862 | 119 | } |