153,7 → 153,12 |
@author Michael Walter |
**************************************************************************** */ |
void GetMeasurements(void) |
{ |
{ |
static int LongTermAccumulatedRoll = 0; |
static int LongTermAccumulatedNick = 0; |
static int LongTermAccumulatedRoll_Cnt = 0; |
static int LongTermAccumulatedNick_Cnt = 0; |
|
ANALOG_OFF; |
AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt; |
AverageNick = AccumulatedNick / AccumulatedNick_cnt; |
172,8 → 177,52 |
} |
AirPressureCnt++; |
|
if(modell_fliegt < 0x250) |
if (LongTermAccumulatedRoll_Cnt < 500) |
{ |
if (abs(LongTermAccumulatedRoll) < 10000) |
{ |
LongTermAccumulatedRoll += AverageRoll; |
} |
LongTermAccumulatedRoll_Cnt++; |
|
if(abs(LongTermAccumulatedNick) < 10000) |
{ |
LongTermAccumulatedNick += AverageNick; |
} |
LongTermAccumulatedNick_Cnt++; |
} |
else |
{ |
static float fPreviousPsi =0.0F; |
static float fPreviousTheta =0.0F; |
|
//DebugOut.Analog[8] = (int) (((status.Phi - fPreviousPsi) / (0.00001F * fCycleTime)) / LongTermAccumulatedRoll_Cnt ); |
//DebugOut.Analog[9] = (int) ((LongTermAccumulatedRoll / LongTermAccumulatedRoll_Cnt)); |
|
AdNeutralRoll += (int) ((LongTermAccumulatedRoll + (status.Phi - fPreviousPsi) / (0.00001F * fCycleTime)) / LongTermAccumulatedRoll_Cnt ); |
AdNeutralNick += (int) ((LongTermAccumulatedNick + (status.Theta - fPreviousTheta) / (0.00001F * fCycleTime)) / LongTermAccumulatedNick_Cnt ); |
|
fPreviousPsi = status.Phi; |
fPreviousTheta = status.Theta; |
|
//AdNeutralRoll += (MAX(-20, MIN(20,LongTermAccumulatedRoll / LongTermAccumulatedRoll_Cnt))); |
//AdNeutralNick += (MAX(-20, MIN(20,LongTermAccumulatedNick / LongTermAccumulatedNick_Cnt))); |
|
LongTermAccumulatedRoll_Cnt = 0; |
LongTermAccumulatedNick_Cnt = 0; |
LongTermAccumulatedRoll = 0; |
LongTermAccumulatedNick = 0; |
} |
|
#if 0 |
DebugOut.Analog[3] = fSumNick; |
DebugOut.Analog[4] = fSumRoll; |
DebugOut.Analog[5] = fSumGier; |
#endif |
|
|
if((modell_fliegt < 0x250) && 0) |
{ |
//if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
AdNeutralNick = 0.998F * AdNeutralNick + 0.002F * AdWertNick_Raw; |
AdNeutralRoll = 0.998F * AdNeutralRoll + 0.002F * AdWertRoll_Raw; |
182,7 → 231,7 |
AdNeutralGier = 0.998F * AdNeutralGier + 0.002F * AdWertGier_Raw; |
} |
} |
else if(modell_fliegt < 0x2000) |
else if(modell_fliegt < 0x2000&& 0) |
{ |
//if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw; |