add firmware

utils/filt.h

@@ -0,0 +1,56 @@
+#pragma once
+class FilterGPS
+{
+  public:
+
+    float Freq=100;
+
+    struct
+    {
+      float Value=0;
+      //---
+      float Min=0.0001, Max=0.1;
+      //---
+      bool EnAcc=true;
+      float Acc=0.02;
+      //---
+      bool EnGPS=true;
+      float Width=2, Length=1;
+      //---
+    }Position;
+
+    struct
+    {
+      float ValueAccGPS=0;
+      float ValueGPS=0;
+      float Value=0;
+      //---
+      float Min=0.0001, Max=0.1;
+      //---
+      bool EnAcc=true;
+      float Acc=0.02;
+      float AccGPS=0.02;
+      //---
+      bool EnGPS=true;
+      float Width=2, Length=1;
+      //---
+      float Last_GPS=0, Speed_GPS=0;
+      long Count=0;
+    }Speed;
+
+    struct
+    {
+      float Pos[50];
+      float Acc[50];
+      float Spd[50];
+      long Size=30, Index=0;
+    }Delay;
+
+    void Update(bool valid, float gps, float acc);
+  
+  private:
+    
+    float Limit(float Value, float Min, float Max);
+    float Minimum(float Value1, float Value2);
+
+};