50 lines
1.1 KiB
Plaintext
50 lines
1.1 KiB
Plaintext
|
#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];
|