93 lines
2.1 KiB
C
93 lines
2.1 KiB
C
|
// -----------------------------------------------------------------------------
|
||
|
|
// Median Filter
|
||
|
|
// Copyright (C) 2017-2018 by Xose Pérez <xose dot perez at gmail dot com>
|
||
|
|
// -----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
#if SENSOR_SUPPORT
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include "BaseFilter.h"
|
||
|
|
|
||
|
|
class MedianFilter : public BaseFilter {
|
||
|
|
|
||
|
|
public:
|
||
|
|
|
||
|
|
~MedianFilter() {
|
||
|
|
if (_data) delete _data;
|
||
|
|
}
|
||
|
|
|
||
|
|
void add(double value) {
|
||
|
|
if (_pointer <= _size) {
|
||
|
|
_data[_pointer] = value;
|
||
|
|
_pointer++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
unsigned char count() {
|
||
|
|
return _pointer;
|
||
|
|
}
|
||
|
|
|
||
|
|
void reset() {
|
||
|
|
if (_pointer > 0) {
|
||
|
|
_data[0] = _data[_pointer-1];
|
||
|
|
_pointer = 1;
|
||
|
|
} else {
|
||
|
|
_pointer = 0;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
double result() {
|
||
|
|
|
||
|
|
double sum = 0;
|
||
|
|
|
||
|
|
if (_pointer > 2) {
|
||
|
|
|
||
|
|
for (unsigned char i = 1; i <= _pointer - 2; i++) {
|
||
|
|
|
||
|
|
// For each position,
|
||
|
|
// we find the median with the previous and next value
|
||
|
|
// and use that for the sum
|
||
|
|
|
||
|
|
double previous = _data[i-1];
|
||
|
|
double current = _data[i];
|
||
|
|
double next = _data[i+1];
|
||
|
|
|
||
|
|
if (previous > current) std::swap(previous, current);
|
||
|
|
if (current > next) std::swap(current, next);
|
||
|
|
if (previous > current) std::swap(previous, current);
|
||
|
|
|
||
|
|
sum += current;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
sum /= (_pointer - 2);
|
||
|
|
|
||
|
|
} else if (_pointer > 0) {
|
||
|
|
|
||
|
|
sum = _data[0];
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
return sum;
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void resize(unsigned char size) {
|
||
|
|
if (_size == size) return;
|
||
|
|
_size = size;
|
||
|
|
if (_data) delete _data;
|
||
|
|
_data = new double[_size+1];
|
||
|
|
for (unsigned char i=0; i<=_size; i++) _data[i] = 0;
|
||
|
|
_pointer = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
protected:
|
||
|
|
|
||
|
|
unsigned char _pointer = 0;
|
||
|
|
double * _data = NULL;
|
||
|
|
|
||
|
|
};
|
||
|
|
|
||
|
|
#endif // SENSOR_SUPPORT
|