68 lines
1.8 KiB
C++
68 lines
1.8 KiB
C++
#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);
|
|
}
|