| 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; |