static uint8_t _getAdcValue(uint8_t pin) {
uint8_t ret = 0;
ADMUX &= ~(_BV(MUX0) | _BV(MUX1));
ADMUX |= pin;
adcBufferWritten = 0;
ADCSRA |= _BV(ADIE);
set_sleep_mode(SLEEP_MODE_IDLE);
sleep_enable();
while (adcBufferWritten != ADC_BUFFER_SIZE) {
while (TCNT0 != TIMER_TOP) {}
sleep_cpu();
adcBufferWritten++;
};
sleep_disable();
#if defined(ADC_MEDIAN_INSTEAD_OF_AVERAGE)
ret = _getMedian(adcBuffer, adcBufferWritten);
#else
ret = _average(adcBuffer, adcBufferWritten);
#endif
return ret;
}
bool TemporalMedianFilter::_filter(const T *in, T *out)
{
uint s = _size.width*_size.height;
T *cur, *hist;
if(_current.size() != s*sizeof(T))
{
_current.resize(s*sizeof(T));
memset(_current.data(), 0, s*sizeof(T));
}
cur = (T *)_current.data();
if(_history.size() < _order)
{
Vector<ByteType> h;
h.resize(s*sizeof(T));
memcpy(h.data(), in, s*sizeof(T));
_history.push_back(std::move(h));
memcpy(cur, in, s*sizeof(T));
memcpy(out, in, s*sizeof(T));
}
else
{
_history.pop_front();
Vector<ByteType> h;
h.resize(s*sizeof(T));
memcpy(h.data(), in, s*sizeof(T));
_history.push_back(std::move(h));
for(auto i = 0; i < s; i++)
{
T v;
_getMedian(i, v);
if(v > 0 && fabs(((float)v - cur[i])/cur[i]) > _deadband)
out[i] = cur[i] = v;
else
out[i] = cur[i];
}
}
return true;
}