Bayley Wang
/
foc-ed_in_the_bot_compact
robot
Filter/fCircularBuffer.cpp
- Committer:
- bwang
- Date:
- 2018-11-13
- Revision:
- 252:38644631ed97
- Parent:
- 224:90172915d0fb
File content as of revision 252:38644631ed97:
#include "mbed.h" #include "math.h" #include "Filter.h" fCircularBuffer::fCircularBuffer(int length, bool use_median) { _length = length; _use_median = use_median; oldest_index = 0; newest_index = -1; num = 0; sum = 0.0f; buf = (float*)malloc(_length * sizeof(float)); sorted = (float*)malloc(_length * sizeof(float)); for (int i = 0; i < _length; i++) { buf[i] = 0.0f; sorted[i] = 0.0f; } } float &fCircularBuffer::at(int index) { int actual = oldest_index + index; if (actual >= _length) actual -= _length; return buf[actual]; } void fCircularBuffer::add(float x) { if (num < _length) { newest_index++; buf[newest_index] = x; sum += x; num++; if (!_use_median || _length < 4) return; /*insert x into sorted array*/ int i = num - 1; while (i > 0 && sorted[i - 1] > x) { sorted[i] = sorted[i - 1]; i--; } sorted[i] = x; } else { /*update circular buffer*/ float oldest = buf[oldest_index]; sum -= buf[oldest_index]; oldest_index++; if (oldest_index >= _length) oldest_index -= _length; newest_index++; if (newest_index >= _length) newest_index -= _length; buf[newest_index] = x; sum += x; if (!_use_median || _length < 4) return; /*find sorted index of oldest element*/ int removed; for (removed = 0; removed < _length; removed++) { if (sorted[removed] == oldest) break; } /*insert x*/ int i; if (removed == _length - 1) { i = _length - 1; while (i > 0 && sorted[i - 1] > x) { sorted[i] = sorted[i - 1]; i--; } sorted[i] = x; } else if (removed == 0) { i = 0; while (i < _length - 1 && sorted[i + 1] < x) { sorted[i] = sorted[i + 1]; i++; } sorted[i] = x; } else if (sorted[removed - 1] <= x && sorted[removed + 1] >= x) { sorted[removed] = x; } else if (sorted[removed - 1] > x) { i = removed; while (i > 0 && sorted[i - 1] > x) { sorted[i] = sorted[i - 1]; i--; } sorted[i] = x; } else { i = removed; while (i < _length - 1 && sorted[i + 1] < x) { sorted[i] = sorted[i + 1]; i++; } sorted[i] = x; } } } float fCircularBuffer::mean() { return sum / num; } float median3(float *buf) { if (buf[0] > buf[1]) { if (buf[1] > buf[2]) { return buf[1]; } else if (buf[0] > buf[2]) { return buf[2]; } else { return buf[0]; } } else { if (buf[0] > buf[2]) { return buf[0]; } else if (buf[1] > buf[2]) { return buf[2]; } else { return buf[1]; } } } float fCircularBuffer::median() { if (_length == 1) { return buf[0]; } if (_length == 2) { if (num < 2) return buf[0]; return (buf[0] + buf[1]) / 2.0f; } if (_length == 3) { if (num < 2) return buf[0]; if (num == 2) return (buf[0] + buf[1]) / 2.0f; return median3(buf); } if (num < _length) { if (num % 2 == 1) { return sorted[(num - 1) / 2]; } else { return (sorted[num / 2] + sorted[num / 2 - 1]) / 2.0f; } } else { if (_length % 2 == 1) { return sorted[(_length - 1) / 2]; } else { return (sorted[_length / 2] + sorted[_length / 2 - 1]) / 2.0f; } } }