Rev 987 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 987 | Rev 1040 | ||
|---|---|---|---|
| Line 151... | Line 151... | ||
| 151 | @pre - |
151 | @pre - |
| 152 | @post - |
152 | @post - |
| 153 | @author Michael Walter |
153 | @author Michael Walter |
| 154 | **************************************************************************** */ |
154 | **************************************************************************** */ |
| 155 | void GetMeasurements(void) |
155 | void GetMeasurements(void) |
| 156 | { |
156 | { |
| - | 157 | static int LongTermAccumulatedRoll = 0; |
|
| - | 158 | static int LongTermAccumulatedNick = 0; |
|
| - | 159 | static int LongTermAccumulatedRoll_Cnt = 0; |
|
| - | 160 | static int LongTermAccumulatedNick_Cnt = 0; |
|
| - | 161 | ||
| 157 | ANALOG_OFF; |
162 | ANALOG_OFF; |
| 158 | AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt; |
163 | AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt; |
| 159 | AverageNick = AccumulatedNick / AccumulatedNick_cnt; |
164 | AverageNick = AccumulatedNick / AccumulatedNick_cnt; |
| 160 | AverageGier = AccumulatedGier / AccumulatedGier_cnt; |
165 | AverageGier = AccumulatedGier / AccumulatedGier_cnt; |
| Line 170... | Line 175... | ||
| 170 | DeltaAltitude = CurrentAltitude - LastAltitude; |
175 | DeltaAltitude = CurrentAltitude - LastAltitude; |
| 171 | LastAltitude = CurrentAltitude; |
176 | LastAltitude = CurrentAltitude; |
| 172 | } |
177 | } |
| 173 | AirPressureCnt++; |
178 | AirPressureCnt++; |
| Line -... | Line 179... | ||
| - | 179 | ||
| - | 180 | if (LongTermAccumulatedRoll_Cnt < 500) |
|
| - | 181 | { |
|
| - | 182 | if (abs(LongTermAccumulatedRoll) < 10000) |
|
| - | 183 | { |
|
| - | 184 | LongTermAccumulatedRoll += AverageRoll; |
|
| - | 185 | } |
|
| - | 186 | LongTermAccumulatedRoll_Cnt++; |
|
| - | 187 | ||
| - | 188 | if(abs(LongTermAccumulatedNick) < 10000) |
|
| - | 189 | { |
|
| - | 190 | LongTermAccumulatedNick += AverageNick; |
|
| - | 191 | } |
|
| - | 192 | LongTermAccumulatedNick_Cnt++; |
|
| - | 193 | } |
|
| - | 194 | else |
|
| - | 195 | { |
|
| - | 196 | static float fPreviousPsi =0.0F; |
|
| - | 197 | static float fPreviousTheta =0.0F; |
|
| - | 198 | ||
| - | 199 | //DebugOut.Analog[8] = (int) (((status.Phi - fPreviousPsi) / (0.00001F * fCycleTime)) / LongTermAccumulatedRoll_Cnt ); |
|
| - | 200 | //DebugOut.Analog[9] = (int) ((LongTermAccumulatedRoll / LongTermAccumulatedRoll_Cnt)); |
|
| - | 201 | ||
| - | 202 | AdNeutralRoll += (int) ((LongTermAccumulatedRoll + (status.Phi - fPreviousPsi) / (0.00001F * fCycleTime)) / LongTermAccumulatedRoll_Cnt ); |
|
| - | 203 | AdNeutralNick += (int) ((LongTermAccumulatedNick + (status.Theta - fPreviousTheta) / (0.00001F * fCycleTime)) / LongTermAccumulatedNick_Cnt ); |
|
| - | 204 | ||
| - | 205 | fPreviousPsi = status.Phi; |
|
| - | 206 | fPreviousTheta = status.Theta; |
|
| - | 207 | ||
| - | 208 | //AdNeutralRoll += (MAX(-20, MIN(20,LongTermAccumulatedRoll / LongTermAccumulatedRoll_Cnt))); |
|
| - | 209 | //AdNeutralNick += (MAX(-20, MIN(20,LongTermAccumulatedNick / LongTermAccumulatedNick_Cnt))); |
|
| - | 210 | ||
| - | 211 | LongTermAccumulatedRoll_Cnt = 0; |
|
| - | 212 | LongTermAccumulatedNick_Cnt = 0; |
|
| - | 213 | LongTermAccumulatedRoll = 0; |
|
| - | 214 | LongTermAccumulatedNick = 0; |
|
| - | 215 | } |
|
| - | 216 | ||
| - | 217 | #if 0 |
|
| - | 218 | DebugOut.Analog[3] = fSumNick; |
|
| - | 219 | DebugOut.Analog[4] = fSumRoll; |
|
| - | 220 | DebugOut.Analog[5] = fSumGier; |
|
| - | 221 | #endif |
|
| - | 222 | ||
| 174 | 223 | ||
| 175 | if(modell_fliegt < 0x250) |
224 | if((modell_fliegt < 0x250) && 0) |
| 176 | { |
225 | { |
| 177 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
226 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
| 178 | AdNeutralNick = 0.998F * AdNeutralNick + 0.002F * AdWertNick_Raw; |
227 | AdNeutralNick = 0.998F * AdNeutralNick + 0.002F * AdWertNick_Raw; |
| 179 | AdNeutralRoll = 0.998F * AdNeutralRoll + 0.002F * AdWertRoll_Raw; |
228 | AdNeutralRoll = 0.998F * AdNeutralRoll + 0.002F * AdWertRoll_Raw; |
| 180 | if (abs(StickGier) < 15 || MotorenEin == 0) |
229 | if (abs(StickGier) < 15 || MotorenEin == 0) |
| 181 | { |
230 | { |
| 182 | AdNeutralGier = 0.998F * AdNeutralGier + 0.002F * AdWertGier_Raw; |
231 | AdNeutralGier = 0.998F * AdNeutralGier + 0.002F * AdWertGier_Raw; |
| 183 | } |
232 | } |
| 184 | } |
233 | } |
| 185 | else if(modell_fliegt < 0x2000) |
234 | else if(modell_fliegt < 0x2000&& 0) |
| 186 | { |
235 | { |
| 187 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
236 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
| 188 | AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw; |
237 | AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw; |
| 189 | AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw; |
238 | AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw; |