Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/CircularBuffer.cpp@148:9bca96f7be5c, 2017-05-03 (annotated)

Committer:
bwang
Date:
Wed May 03 13:59:51 2017 +0000
Revision:
148:9bca96f7be5c
Parent:
147:c1b2379b8874
Child:
149:c51c0258c923
05/03/2017 09:39 - indexing bugfix in median code

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 148:9bca96f7be5c 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 148:9bca96f7be5c 34
bwang 147:c1b2379b8874 35 if (!_use_median) return;
bwang 148:9bca96f7be5c 36
bwang 145:37ffa3ba3862 37 /*insert x into sorted array*/
bwang 147:c1b2379b8874 38 int i = num - 1;
bwang 148:9bca96f7be5c 39 while (i > 0 && sorted[i - 1] > 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 148:9bca96f7be5c 44 } else {
bwang 147:c1b2379b8874 45 /*update circular buffer*/
bwang 147:c1b2379b8874 46 float oldest = buf[oldest_index];
bwang 148:9bca96f7be5c 47
bwang 147:c1b2379b8874 48 sum -= buf[oldest_index];
bwang 147:c1b2379b8874 49 oldest_index++;
bwang 147:c1b2379b8874 50 if (oldest_index >= _length) oldest_index -= _length;
bwang 148:9bca96f7be5c 51
bwang 147:c1b2379b8874 52 newest_index++;
bwang 147:c1b2379b8874 53 if (newest_index >= _length) newest_index -= _length;
bwang 147:c1b2379b8874 54 buf[newest_index] = x;
bwang 148:9bca96f7be5c 55
bwang 147:c1b2379b8874 56 sum += x;
bwang 148:9bca96f7be5c 57
bwang 147:c1b2379b8874 58 if (!_use_median) return;
bwang 148:9bca96f7be5c 59
bwang 145:37ffa3ba3862 60 /*find sorted index of oldest element*/
bwang 145:37ffa3ba3862 61 int removed;
bwang 145:37ffa3ba3862 62 for (removed = 0; removed < _length; removed++) {
bwang 147:c1b2379b8874 63 if (sorted[removed] == oldest) break;
bwang 145:37ffa3ba3862 64 }
bwang 145:37ffa3ba3862 65
bwang 145:37ffa3ba3862 66 /*insert x*/
bwang 145:37ffa3ba3862 67 int i;
bwang 145:37ffa3ba3862 68 if (removed == _length - 1) {
bwang 145:37ffa3ba3862 69 i = _length - 1;
bwang 148:9bca96f7be5c 70 while (i > 0 && sorted[i - 1] > 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 148:9bca96f7be5c 75 } else if (removed == 0) {
bwang 145:37ffa3ba3862 76 i = 0;
bwang 148:9bca96f7be5c 77 while (i < _length - 1 && sorted[i + 1] < x) {
bwang 145:37ffa3ba3862 78 sorted[i] = sorted[i + 1];
bwang 145:37ffa3ba3862 79 i++;
bwang 145:37ffa3ba3862 80 }
bwang 145:37ffa3ba3862 81 sorted[i] = x;
bwang 148:9bca96f7be5c 82 } else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) {
bwang 145:37ffa3ba3862 83 sorted[removed] = x;
bwang 148:9bca96f7be5c 84 } else if (sorted[removed - 1] > x) {
bwang 145:37ffa3ba3862 85 i = removed;
bwang 148:9bca96f7be5c 86 while (i > 0 && sorted[i - 1] > x) {
bwang 145:37ffa3ba3862 87 sorted[i] = sorted[i - 1];
bwang 145:37ffa3ba3862 88 i--;
bwang 145:37ffa3ba3862 89 }
bwang 145:37ffa3ba3862 90 sorted[i] = x;
bwang 148:9bca96f7be5c 91 } else {
bwang 145:37ffa3ba3862 92 i = removed;
bwang 148:9bca96f7be5c 93 while (i < _length - 1 && sorted[i + 1] < x) {
bwang 145:37ffa3ba3862 94 sorted[i] = sorted[i + 1];
bwang 145:37ffa3ba3862 95 i++;
bwang 145:37ffa3ba3862 96 }
bwang 145:37ffa3ba3862 97 sorted[i] = x;
bwang 145:37ffa3ba3862 98 }
bwang 145:37ffa3ba3862 99 }
bwang 145:37ffa3ba3862 100 }
bwang 145:37ffa3ba3862 101
bwang 145:37ffa3ba3862 102 float CircularBuffer::mean() {
bwang 145:37ffa3ba3862 103 return sum / num;
bwang 145:37ffa3ba3862 104 }
bwang 145:37ffa3ba3862 105
bwang 145:37ffa3ba3862 106 float CircularBuffer::median() {
bwang 145:37ffa3ba3862 107 if (num < _length) {
bwang 145:37ffa3ba3862 108 if (num % 2 == 1) {
bwang 145:37ffa3ba3862 109 return sorted[(num - 1) / 2];
bwang 148:9bca96f7be5c 110 } else {
bwang 145:37ffa3ba3862 111 return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 112 }
bwang 145:37ffa3ba3862 113 }
bwang 145:37ffa3ba3862 114 else {
bwang 145:37ffa3ba3862 115 if (_length % 2 == 1) {
bwang 145:37ffa3ba3862 116 return sorted[(_length - 1) / 2];
bwang 148:9bca96f7be5c 117 } else {
bwang 145:37ffa3ba3862 118 return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 119 }
bwang 145:37ffa3ba3862 120 }
bwang 145:37ffa3ba3862 121 }