added median filter code

Median Filter.txt

@@ -0,0 +1,50 @@
+#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];