Rev 1224 | Rev 1232 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1224 | Rev 1225 | ||
|---|---|---|---|
| Line 327... | Line 327... | ||
| 327 | tmpl *= Parameter_AchsKopplung1; // 90 |
327 | tmpl *= Parameter_AchsKopplung1; // 90 |
| 328 | tmpl /= 4096L; |
328 | tmpl /= 4096L; |
| 329 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
329 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
| 330 | tmpl2 *= Parameter_AchsKopplung1; |
330 | tmpl2 *= Parameter_AchsKopplung1; |
| 331 | tmpl2 /= 4096L; |
331 | tmpl2 /= 4096L; |
| 332 | if(labs(tmpl) > 128 || labs(tmpl2) > 128 /* || abs(KopplungsteilNickRoll) > 128 || abs(KopplungsteilRollNick) > 128)*/) TrichterFlug = 1; |
332 | if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
| 333 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
333 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
| 334 | } |
334 | } |
| 335 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
335 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
| Line 336... | Line 336... | ||
| 336 | 336 | ||
| Line 985... | Line 985... | ||
| 985 | /* |
985 | /* |
| 986 | DebugOut.Analog[29] = ausgleichRoll; |
986 | DebugOut.Analog[29] = ausgleichRoll; |
| 987 | DebugOut.Analog[30] = LageKorrekturRoll * 10; |
987 | DebugOut.Analog[30] = LageKorrekturRoll * 10; |
| 988 | */ |
988 | */ |
| Line 989... | Line 989... | ||
| 989 | 989 | ||
| - | 990 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
|
| 990 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL * 4) |
991 | #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
| 991 | #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) |
992 | #define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16 |
| 992 | #define BEWEGUNGS_LIMIT 20000 |
993 | #define BEWEGUNGS_LIMIT 20000 |
| 993 | // Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
994 | // Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 995 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
|
| 994 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
996 | if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4; |
| 995 | if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
997 | if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
| 996 | { |
998 | { |
| 997 | if(IntegralFehlerNick > FEHLER_LIMIT2) |
999 | if(IntegralFehlerNick > FEHLER_LIMIT2) |
| 998 | { |
1000 | { |
| Line 1027... | Line 1029... | ||
| 1027 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
1029 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
| 1028 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
1030 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
| Line 1029... | Line 1031... | ||
| 1029 | 1031 | ||
| 1030 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
1032 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1031 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
- | |
| - | 1033 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
|
| 1032 | 1034 | if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4; |
|
| 1033 | ausgleichRoll = 0; |
1035 | ausgleichRoll = 0; |
| 1034 | if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
1036 | if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8)) |
| 1035 | { |
1037 | { |
| 1036 | if(IntegralFehlerRoll > FEHLER_LIMIT2) |
1038 | if(IntegralFehlerRoll > FEHLER_LIMIT2) |
| Line 1267... | Line 1269... | ||
| 1267 | { |
1269 | { |
| 1268 | if(Parameter_MaxHoehe < 50) |
1270 | if(Parameter_MaxHoehe < 50) |
| 1269 | { |
1271 | { |
| 1270 | if(!delay--) |
1272 | if(!delay--) |
| 1271 | { |
1273 | { |
| 1272 | if((MessLuftdruck > 1000) && ((int)DruckOffsetSetting - ((int)ExpandBaro-10) < 256)) |
1274 | if((MessLuftdruck > 1000) && OCR0A < 254) |
| 1273 | { |
1275 | { |
| - | 1276 | if(OCR0A < 244) |
|
| - | 1277 | { |
|
| 1274 | ExpandBaro -= 10; |
1278 | ExpandBaro -= 10; |
| 1275 | OCR0A = DruckOffsetSetting - ExpandBaro; |
1279 | OCR0A = DruckOffsetSetting - ExpandBaro; |
| - | 1280 | } |
|
| - | 1281 | else OCR0A = 254; |
|
| 1276 | beeptime = 300; |
1282 | beeptime = 300; |
| 1277 | delay = 250; |
1283 | delay = 250; |
| 1278 | } |
1284 | } |
| 1279 | else |
1285 | else |
| 1280 | if((MessLuftdruck < 100) && ((int)DruckOffsetSetting + 10 < ExpandBaro)) |
1286 | if((MessLuftdruck < 100) && OCR0A > 1) |
| 1281 | { |
1287 | { |
| - | 1288 | if(OCR0A > 10) |
|
| - | 1289 | { |
|
| 1282 | ExpandBaro += 10; |
1290 | ExpandBaro += 10; |
| 1283 | OCR0A = DruckOffsetSetting - ExpandBaro; |
1291 | OCR0A = DruckOffsetSetting - ExpandBaro; |
| - | 1292 | } |
|
| - | 1293 | else OCR0A = 1; |
|
| 1284 | beeptime = 300; |
1294 | beeptime = 300; |
| 1285 | delay = 250; |
1295 | delay = 250; |
| 1286 | } |
1296 | } |
| 1287 | else |
1297 | else |
| 1288 | { |
1298 | { |