#Best for spike/impulse rejection (e.g. sensor glitches). Uses insertion sort on the buffer. 

FUNCTION_BLOCK FB_MedianFilter
VAR_INPUT
    xEnable     : BOOL;
    rInput      : REAL;
    iWindowSize : INT := 7;    // Odd number recommended, max 15
END_VAR
VAR_OUTPUT
    rOutput : REAL;
END_VAR
VAR
    arBuffer : ARRAY[0..14] OF REAL;
    arSorted : ARRAY[0..14] OF REAL;
    iIndex   : INT := 0;
    iCount   : INT := 0;
    iWin     : INT;
    i, j     : INT;
    rTemp    : REAL;
END_VAR

IF NOT xEnable THEN
    iIndex := 0; iCount := 0; rOutput := rInput;
    RETURN;
END_IF;

iWin := MIN(MAX(iWindowSize, 1), 15);

arBuffer[iIndex] := rInput;
iIndex := (iIndex + 1) MOD iWin;
IF iCount < iWin THEN iCount := iCount + 1; END_IF;

// Copy to sort buffer
FOR i := 0 TO iCount - 1 DO
    arSorted[i] := arBuffer[i];
END_FOR;

// Insertion sort
FOR i := 1 TO iCount - 1 DO
    rTemp := arSorted[i];
    j := i - 1;
    WHILE (j >= 0) AND (arSorted[j] > rTemp) DO
        arSorted[j + 1] := arSorted[j];
        j := j - 1;
    END_WHILE;
    arSorted[j + 1] := rTemp;
END_FOR;

// Pick middle element
rOutput := arSorted[iCount / 2];