//

***********************************************************************************
**
// ** MEDIAN FILTER
**
// ** v1.0 August 2018 C. Laffoon
**
// ** Median filter function for use in reducing effects of random noise spikes
**
// ** To increase/decrease number of filter elements, change constant
FILTER_ELEMENTS **
// ** and also edit size of arrays to match.
**
//
***********************************************************************************
**
FUNCTION_BLOCK FB201

CONST
FILTER_ELEMENTS := 11;
END_CONST

VAR_INPUT
IN_VALUE : INT;
END_VAR

VAR_OUTPUT
OUT_VALUE : INT;
END_VAR

VAR_TEMP
// Temporary Variables
BFR_VAL : ARRAY[0..10] OF INT;
BFR_AGE : ARRAY[0..10] OF INT;
BFR_IDX : INT;
VAL_IDX : INT;
TMP_IDX : INT;
OLDEST_DELETED : BOOL;
OLDEST_AGE : INT;
OLDEST_IDX : INT;
INCOMING_VALUE_WRITTEN : BOOL;
MEDIAN_IDX : INT;
END_VAR

VAR
// Static Variables
VALUE : ARRAY[0..10] OF INT;
AGE : ARRAY[0..10] OF INT;
END_VAR

LABEL
LAB1, LAB2;
END_LABEL

// ***********************************************************
// ** Statement Section **
// ***********************************************************
BEGIN
 BFR_IDX := 0; // Initialize Index
VAL_IDX := 0; // Initialize Index
OLDEST_DELETED := FALSE;
OLDEST_AGE := -1;
INCOMING_VALUE_WRITTEN := FALSE;
MEDIAN_IDX := (FILTER_ELEMENTS / 2);

// Find oldest element (this will be removed from updated list)

FOR TMP_IDX := 0 TO FILTER_ELEMENTS-1 DO
IF AGE[TMP_IDX] > OLDEST_AGE THEN
OLDEST_AGE := AGE[TMP_IDX];
OLDEST_IDX := TMP_IDX;
END_IF;
END_FOR;

WHILE BFR_IDX < FILTER_ELEMENTS DO

// If this is oldest element, then just skip over it so that it falls out
of list.
IF VAL_IDX = OLDEST_IDX THEN
VAL_IDX := VAL_IDX + 1;
END_IF;

// If new value is smaller than current index value, then write incoming
value to buffer
IF INCOMING_VALUE_WRITTEN = FALSE AND IN_VALUE <= VALUE[VAL_IDX] OR VAL_IDX
> (FILTER_ELEMENTS-1) THEN
// Write incoming value and age to buffer
BFR_VAL[BFR_IDX] := IN_VALUE;
BFR_AGE[BFR_IDX] := 0;
BFR_IDX := BFR_IDX + 1;
INCOMING_VALUE_WRITTEN := TRUE;
END_IF;

// Write current value and incremented age to buffer

IF BFR_IDX < FILTER_ELEMENTS THEN
BFR_VAL[BFR_IDX] := VALUE[VAL_IDX];
BFR_AGE[BFR_IDX] := AGE[VAL_IDX] + 1;
BFR_IDX := BFR_IDX + 1;
VAL_IDX := VAL_IDX + 1;
END_IF;
END_WHILE;

// Write buffer to values/ages

FOR TMP_IDX := 0 TO (FILTER_ELEMENTS-1) DO
VALUE[TMP_IDX] := BFR_VAL[TMP_IDX];
AGE[TMP_IDX] := BFR_AGE[TMP_IDX];
END_FOR;

// You've been waiting for it! Here it is...

// Return median of dataset (middle element)
OUT_VALUE := VALUE[MEDIAN_IDX];

END_FUNCTION_BLOCK