124 lines
3.3 KiB
C
124 lines
3.3 KiB
C
/*
|
|
* MedianFilter.c
|
|
*
|
|
* Created on: May 19, 2018
|
|
* Author: alexandru.bogdan
|
|
* Editor: Carlos Derseher
|
|
*
|
|
* original source code:
|
|
* https://github.com/accabog/MedianFilter
|
|
*/
|
|
|
|
/**
|
|
* This Module expects odd numbers of buffer lengths!!!
|
|
*/
|
|
|
|
#include <stdint.h>
|
|
|
|
#include "MedianFilter.h"
|
|
|
|
/**
|
|
*
|
|
*/
|
|
int MEDIANFILTER_Init(sMedianFilter_t *medianFilter)
|
|
{
|
|
if (medianFilter && medianFilter->medianBuffer &&
|
|
(medianFilter->numNodes % 2) && (medianFilter->numNodes > 1))
|
|
{
|
|
//initialize buffer nodes
|
|
for(unsigned int i = 0; i < medianFilter->numNodes; i++)
|
|
{
|
|
medianFilter->medianBuffer[i].value = 0;
|
|
medianFilter->medianBuffer[i].nextAge = &medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes];
|
|
medianFilter->medianBuffer[i].nextValue = &medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes];
|
|
medianFilter->medianBuffer[(i + 1) % medianFilter->numNodes].prevValue = &medianFilter->medianBuffer[i];
|
|
}
|
|
//initialize heads
|
|
medianFilter->ageHead = medianFilter->medianBuffer;
|
|
medianFilter->valueHead = medianFilter->medianBuffer;
|
|
medianFilter->medianHead = &medianFilter->medianBuffer[medianFilter->numNodes / 2];
|
|
|
|
medianFilter->bufferCnt = 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
int64_t MEDIANFILTER_Insert(sMedianFilter_t *medianFilter, int64_t sample)
|
|
{
|
|
unsigned int i;
|
|
sMedianNode_t *newNode, *it;
|
|
|
|
if(medianFilter->ageHead == medianFilter->valueHead)
|
|
{ //if oldest node is also the smallest node, increment value head
|
|
medianFilter->valueHead = medianFilter->valueHead->nextValue;
|
|
}
|
|
|
|
if((medianFilter->ageHead == medianFilter->medianHead) ||
|
|
(medianFilter->ageHead->value > medianFilter->medianHead->value))
|
|
{ //prepare for median correction
|
|
medianFilter->medianHead = medianFilter->medianHead->prevValue;
|
|
}
|
|
|
|
//replace age head with new sample
|
|
newNode = medianFilter->ageHead;
|
|
newNode->value = sample;
|
|
|
|
//remove age head from list
|
|
medianFilter->ageHead->nextValue->prevValue = medianFilter->ageHead->prevValue;
|
|
medianFilter->ageHead->prevValue->nextValue = medianFilter->ageHead->nextValue;
|
|
//increment age head
|
|
medianFilter->ageHead = medianFilter->ageHead->nextAge;
|
|
|
|
//find new node position
|
|
it = medianFilter->valueHead; //set iterator as value head
|
|
for(i = 0; i < medianFilter->numNodes - 1; i++)
|
|
{
|
|
if(sample < it->value)
|
|
{
|
|
if(i == 0)
|
|
{ //replace value head if new node is the smallest
|
|
medianFilter->valueHead = newNode;
|
|
}
|
|
break;
|
|
}
|
|
it = it->nextValue;
|
|
}
|
|
|
|
//insert new node in list
|
|
it->prevValue->nextValue = newNode;
|
|
newNode->prevValue = it->prevValue;
|
|
it->prevValue = newNode;
|
|
newNode->nextValue = it;
|
|
|
|
if (medianFilter->bufferCnt < medianFilter->numNodes) {
|
|
medianFilter->bufferCnt++;
|
|
}
|
|
|
|
//adjust median node
|
|
if(i >= (medianFilter->numNodes / 2))
|
|
{
|
|
medianFilter->medianHead = medianFilter->medianHead->nextValue;
|
|
}
|
|
|
|
return medianFilter->medianHead->value;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
uint8_t MEDIANFILTER_isFull(sMedianFilter_t *medianFilter) {
|
|
if (medianFilter->bufferCnt < medianFilter->numNodes) {
|
|
return 0;
|
|
}
|
|
else {
|
|
return 1;
|
|
}
|
|
}
|
|
|