add firmware

utils/filt.cpp

@@ -0,0 +1,67 @@
+#include <math.h>
+
+#include "filt.h"
+
+float FilterGPS::Limit(float Value, float Min, float Max)
+{
+	if (Value > Max) return Max;
+	if (Value < Min) return Min;
+	return Value;
+}
+
+float FilterGPS::Minimum(float Value1, float Value2)
+{
+	if (Value1 < Value2) return Value1;
+	return Value2;
+}
+
+void FilterGPS::Update(bool valid, float gps, float acc)
+{
+	const float ga = 9.80665f;
+
+	Delay.Acc[Delay.Index] = acc;
+	Delay.Pos[Delay.Index] = Position.Value;
+	Delay.Spd[Delay.Index++] = Speed.Value;
+	if (Delay.Index >= Delay.Size) Delay.Index = 0;
+
+	float prev = Delay.Acc[Delay.Index];
+	float shift = gps - Delay.Pos[Delay.Index];
+
+	float coef_a = 1;
+	if (Position.EnAcc) coef_a = Position.Acc / fabsf(prev);
+	float coef_p = 1;
+	if (Position.EnGPS) coef_p = powf(fabsf(shift * Position.Length), Position.Width);
+
+	float coef1 = Minimum(coef_a, coef_p);
+	coef1 = Limit(coef1, Position.Min, Position.Max);
+
+	Speed.Count++;
+	if (valid)
+	{
+		Speed.ValueGPS = (gps - Speed.Last_GPS) * Freq / Speed.Count;
+		Speed.Last_GPS = gps;
+		Speed.ValueAccGPS= (Speed.ValueGPS- Speed.Speed_GPS) * Freq / Speed.Count;
+		Speed.Speed_GPS = Speed.ValueGPS;
+		Speed.Count = 0;
+	}
+
+	for (int a = 0; a < Delay.Size; a++) Delay.Pos[a] += shift * coef1;
+	Position.Value += shift * coef1 + (Speed.Value / Freq) * (1 - coef1);
+
+	// SPEED
+
+	shift = Speed.ValueGPS - Delay.Spd[Delay.Index];
+
+	coef_a = 1;
+	if (Speed.EnAcc) coef_a = Speed.Acc / fabsf(prev);
+	float coef_g = 1;
+	if (Speed.EnAcc) coef_g = Speed.AccGPS / fabsf(Speed.ValueAccGPS);
+	float coef_s = 1;
+	if (Speed.EnGPS) coef_s = powf(fabsf(shift * Speed.Length), Speed.Width);
+
+	float coef2 = Minimum(coef_a, Minimum(coef_s, coef_g));
+	coef2 = Limit(coef2, Speed.Min, Speed.Max);
+
+	for (int a = 0; a < Delay.Size; a++) Delay.Spd[a] += shift * coef2;
+	Speed.Value += shift * coef2 + (acc * ga / Freq) * (1 - coef2);
+}