Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/CircularBuffer.cpp@147:c1b2379b8874, 2017-05-03 (annotated)

Committer:
bwang
Date:
Wed May 03 13:27:12 2017 +0000
Revision:
147:c1b2379b8874
Parent:
145:37ffa3ba3862
Child:
148:9bca96f7be5c
05/03/2017 09:26 - added use_median option to circular buffers (since average filters may not want to recalculate the median every sample)

Who changed what in which revision?

UserRevisionLine numberNew 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 147:c1b2379b8874 5 CircularBuffer::CircularBuffer(int length, bool use_median) {
bwang 145:37ffa3ba3862 6 _length = length;
bwang 147:c1b2379b8874 7 _use_median = use_median;
bwang 147:c1b2379b8874 8
bwang 145:37ffa3ba3862 9 oldest_index = 0;
bwang 145:37ffa3ba3862 10 newest_index = -1;
bwang 145:37ffa3ba3862 11 num = 0;
bwang 145:37ffa3ba3862 12 sum = 0.0f;
bwang 145:37ffa3ba3862 13
bwang 145:37ffa3ba3862 14 buf = (float*)malloc(_length * sizeof(float));
bwang 145:37ffa3ba3862 15 sorted = (float*)malloc(_length * sizeof(float));
bwang 145:37ffa3ba3862 16 for (int i = 0; i < _length; i++) {
bwang 145:37ffa3ba3862 17 buf[i] = 0.0f;
bwang 145:37ffa3ba3862 18 sorted[i] = 0.0f;
bwang 145:37ffa3ba3862 19 }
bwang 145:37ffa3ba3862 20 }
bwang 145:37ffa3ba3862 21
bwang 145:37ffa3ba3862 22 float &CircularBuffer::at(int index) {
bwang 145:37ffa3ba3862 23 int actual = oldest_index + index;
bwang 145:37ffa3ba3862 24 if (actual >= _length) actual -= _length;
bwang 145:37ffa3ba3862 25 return buf[actual];
bwang 145:37ffa3ba3862 26 }
bwang 145:37ffa3ba3862 27
bwang 145:37ffa3ba3862 28 void CircularBuffer::add(float x) {
bwang 145:37ffa3ba3862 29 if (num < _length) {
bwang 145:37ffa3ba3862 30 newest_index++;
bwang 145:37ffa3ba3862 31 buf[newest_index] = x;
bwang 145:37ffa3ba3862 32 sum += x;
bwang 147:c1b2379b8874 33 num++;
bwang 147:c1b2379b8874 34
bwang 147:c1b2379b8874 35 if (!_use_median) return;
bwang 147:c1b2379b8874 36
bwang 145:37ffa3ba3862 37 /*insert x into sorted array*/
bwang 147:c1b2379b8874 38 int i = num - 1;
bwang 145:37ffa3ba3862 39 while (i > 0 && sorted[i] > x) {
bwang 145:37ffa3ba3862 40 sorted[i] = sorted[i - 1];
bwang 145:37ffa3ba3862 41 i--;
bwang 145:37ffa3ba3862 42 }
bwang 145:37ffa3ba3862 43 sorted[i] = x;
bwang 145:37ffa3ba3862 44 }
bwang 145:37ffa3ba3862 45 else {
bwang 147:c1b2379b8874 46 /*update circular buffer*/
bwang 147:c1b2379b8874 47 float oldest = buf[oldest_index];
bwang 147:c1b2379b8874 48
bwang 147:c1b2379b8874 49 sum -= buf[oldest_index];
bwang 147:c1b2379b8874 50 oldest_index++;
bwang 147:c1b2379b8874 51 if (oldest_index >= _length) oldest_index -= _length;
bwang 147:c1b2379b8874 52
bwang 147:c1b2379b8874 53 newest_index++;
bwang 147:c1b2379b8874 54 if (newest_index >= _length) newest_index -= _length;
bwang 147:c1b2379b8874 55 buf[newest_index] = x;
bwang 147:c1b2379b8874 56
bwang 147:c1b2379b8874 57 sum += x;
bwang 147:c1b2379b8874 58
bwang 147:c1b2379b8874 59 if (!_use_median) return;
bwang 147:c1b2379b8874 60
bwang 145:37ffa3ba3862 61 /*find sorted index of oldest element*/
bwang 145:37ffa3ba3862 62 int removed;
bwang 145:37ffa3ba3862 63 for (removed = 0; removed < _length; removed++) {
bwang 147:c1b2379b8874 64 if (sorted[removed] == oldest) break;
bwang 145:37ffa3ba3862 65 }
bwang 145:37ffa3ba3862 66
bwang 145:37ffa3ba3862 67 /*insert x*/
bwang 145:37ffa3ba3862 68 int i;
bwang 145:37ffa3ba3862 69 if (removed == _length - 1) {
bwang 145:37ffa3ba3862 70 i = _length - 1;
bwang 145:37ffa3ba3862 71 while (i > 0 && sorted[i] > x) {
bwang 145:37ffa3ba3862 72 sorted[i] = sorted[i - 1];
bwang 145:37ffa3ba3862 73 i--;
bwang 145:37ffa3ba3862 74 }
bwang 145:37ffa3ba3862 75 sorted[i] = x;
bwang 145:37ffa3ba3862 76 }
bwang 145:37ffa3ba3862 77 else if (removed == 0) {
bwang 145:37ffa3ba3862 78 i = 0;
bwang 145:37ffa3ba3862 79 while (i < _length - 1 && sorted[i] < x) {
bwang 145:37ffa3ba3862 80 sorted[i] = sorted[i + 1];
bwang 145:37ffa3ba3862 81 i++;
bwang 145:37ffa3ba3862 82 }
bwang 145:37ffa3ba3862 83 sorted[i] = x;
bwang 145:37ffa3ba3862 84 }
bwang 145:37ffa3ba3862 85 else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
bwang 145:37ffa3ba3862 86 sorted[removed] = x;
bwang 145:37ffa3ba3862 87 }
bwang 145:37ffa3ba3862 88 else if (sorted[removed - 1] > x) {
bwang 145:37ffa3ba3862 89 i = removed;
bwang 145:37ffa3ba3862 90 while (i > 0 && sorted[i] > x) {
bwang 145:37ffa3ba3862 91 sorted[i] = sorted[i - 1];
bwang 145:37ffa3ba3862 92 i--;
bwang 145:37ffa3ba3862 93 }
bwang 145:37ffa3ba3862 94 sorted[i] = x;
bwang 145:37ffa3ba3862 95 }
bwang 145:37ffa3ba3862 96 else {
bwang 145:37ffa3ba3862 97 i = removed;
bwang 145:37ffa3ba3862 98 while (i < _length - 1 && sorted[i] < x) {
bwang 145:37ffa3ba3862 99 sorted[i] = sorted[i + 1];
bwang 145:37ffa3ba3862 100 i++;
bwang 145:37ffa3ba3862 101 }
bwang 145:37ffa3ba3862 102 sorted[i] = x;
bwang 145:37ffa3ba3862 103 }
bwang 145:37ffa3ba3862 104 }
bwang 145:37ffa3ba3862 105 }
bwang 145:37ffa3ba3862 106
bwang 145:37ffa3ba3862 107 float CircularBuffer::mean() {
bwang 145:37ffa3ba3862 108 return sum / num;
bwang 145:37ffa3ba3862 109 }
bwang 145:37ffa3ba3862 110
bwang 145:37ffa3ba3862 111 float CircularBuffer::median() {
bwang 145:37ffa3ba3862 112 if (num < _length) {
bwang 145:37ffa3ba3862 113 if (num % 2 == 1) {
bwang 145:37ffa3ba3862 114 return sorted[(num - 1) / 2];
bwang 145:37ffa3ba3862 115 }
bwang 145:37ffa3ba3862 116 else {
bwang 145:37ffa3ba3862 117 return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 118 }
bwang 145:37ffa3ba3862 119 }
bwang 145:37ffa3ba3862 120 else {
bwang 145:37ffa3ba3862 121 if (_length % 2 == 1) {
bwang 145:37ffa3ba3862 122 return sorted[(_length - 1) / 2];
bwang 145:37ffa3ba3862 123 }
bwang 145:37ffa3ba3862 124 else {
bwang 145:37ffa3ba3862 125 return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 126 }
bwang 145:37ffa3ba3862 127 }
bwang 145:37ffa3ba3862 128 }