Rev 1224 | Rev 1232 | Go to most recent revision | Show entire file | Regard 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) |
- | 1275 | { |
|
- | 1276 | if(OCR0A < 244) |
|
1273 | { |
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) |
- | 1287 | { |
|
- | 1288 | if(OCR0A > 10) |
|
1281 | { |
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 | { |