Bayley Wang / Mbed 2 deprecated foc-ed_in_the_bot_compact

robot

Dependencies:   FastPWM3 mbed

Filter/CircularBuffer.cpp@155:7c6005933d4c, 2017-05-04 (annotated)

Committer:
bwang
Date:
Thu May 04 17:35:10 2017 +0000
Revision:
155:7c6005933d4c
Parent:
149:c51c0258c923 
05/04/2017 13:34 - changed logic in PwmIn::handle_fall, which no longer resets timer on invalid edges

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 149:c51c0258c923 35 if (!_use_median || _length < 4) 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 149:c51c0258c923 58 if (!_use_median || _length < 4) 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 149:c51c0258c923 106 float median3(float *buf) {
bwang 149:c51c0258c923 107 if (buf[0] > buf[1]) {
bwang 149:c51c0258c923 108 if (buf[1] > buf[2]) {
bwang 149:c51c0258c923 109 return buf[1];
bwang 149:c51c0258c923 110 } else if (buf[0] > buf[2]) {
bwang 149:c51c0258c923 111 return buf[2];
bwang 149:c51c0258c923 112 } else {
bwang 149:c51c0258c923 113 return buf[0];
bwang 149:c51c0258c923 114 }
bwang 149:c51c0258c923 115 } else {
bwang 149:c51c0258c923 116 if (buf[0] > buf[2]) {
bwang 149:c51c0258c923 117 return buf[0];
bwang 149:c51c0258c923 118 } else if (buf[1] > buf[2]) {
bwang 149:c51c0258c923 119 return buf[2];
bwang 149:c51c0258c923 120 } else {
bwang 149:c51c0258c923 121 return buf[1];
bwang 149:c51c0258c923 122 }
bwang 149:c51c0258c923 123 }
bwang 149:c51c0258c923 124 }
bwang 145:37ffa3ba3862 125 float CircularBuffer::median() {
bwang 149:c51c0258c923 126 if (_length == 1) {
bwang 149:c51c0258c923 127 return buf[0];
bwang 149:c51c0258c923 128 }
bwang 149:c51c0258c923 129 if (_length == 2) {
bwang 149:c51c0258c923 130 if (num < 2) return buf[0];
bwang 149:c51c0258c923 131 return (buf[0] + buf[1]) / 2.0f;
bwang 149:c51c0258c923 132 }
bwang 149:c51c0258c923 133 if (_length == 3) {
bwang 149:c51c0258c923 134 if (num < 2) return buf[0];
bwang 149:c51c0258c923 135 if (num == 2) return (buf[0] + buf[1]) / 2.0f;
bwang 149:c51c0258c923 136 return median3(buf);
bwang 149:c51c0258c923 137 }
bwang 145:37ffa3ba3862 138 if (num < _length) {
bwang 145:37ffa3ba3862 139 if (num % 2 == 1) {
bwang 145:37ffa3ba3862 140 return sorted[(num - 1) / 2];
bwang 148:9bca96f7be5c 141 } else {
bwang 145:37ffa3ba3862 142 return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 143 }
bwang 145:37ffa3ba3862 144 }
bwang 145:37ffa3ba3862 145 else {
bwang 145:37ffa3ba3862 146 if (_length % 2 == 1) {
bwang 145:37ffa3ba3862 147 return sorted[(_length - 1) / 2];
bwang 148:9bca96f7be5c 148 } else {
bwang 145:37ffa3ba3862 149 return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f;
bwang 145:37ffa3ba3862 150 }
bwang 145:37ffa3ba3862 151 }
bwang 145:37ffa3ba3862 152 }