add firmware

utils/med2.cpp

@@ -0,0 +1,58 @@
+#include <string.h>
+
+#include "med2.h"
+
+void MED2_Init(MED2_Data& Data)
+{
+  for (unsigned char a = 0; a < Data.Size; a++) { Data.Index[a] = a; Data.Buff[a] = 0; }
+  Data.Last = 0;
+}
+
+long MED2_Update(long Value, MED2_Data& Data)
+{
+  unsigned char* index=Data.Index;
+  unsigned char size=Data.Size;
+  unsigned char& last=Data.Last;
+  long* buff=Data.Buff;
+  //---
+  buff[last] = Value;
+  //---
+  unsigned char id;
+  for (id = 0; id < size; id++) if (index[id] == last) break;
+  //---
+  bool f = (!id || (Value > buff[index[id - 1]]));
+
+  if (f)
+  {
+    for (unsigned char a = id; a < size - 1; a++)
+    {
+      if (buff[index[a + 1]] >= Value) break;
+      unsigned char tmp = index[a];
+      index[a] = index[a + 1];
+      index[a + 1] = tmp;
+    }
+  }
+  else
+  {
+    for (unsigned char a = id; a > 0; a--)
+    {
+      if (buff[index[a - 1]] <= Value) break;
+      unsigned char tmp = index[a - 1];
+      index[a - 1] = index[a];
+      index[a] = tmp;
+    }
+  }
+
+  last++;
+  if (last >= size) last = 0;
+  
+  if(Data.Smooth>1)
+  {
+    long ret=0;
+    long s = Data.Smooth/2;
+    for(long a=-s; a<s; a++) ret+=buff[index[size/2+a]];
+    return ret/Data.Smooth;
+  }
+  
+  return buff[index[size/2]];
+}