Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
2 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
||
3 | { |
||
4 | tmpl3 = (MesswertRoll * winkel_nick) / 2048L; |
||
5 | tmpl3 *= Parameter_AchsKopplung2; //65 |
||
6 | tmpl3 /= 4096L; |
||
7 | tmpl4 = (MesswertNick * winkel_roll) / 2048L; |
||
8 | tmpl4 *= Parameter_AchsKopplung2; //65 |
||
9 | tmpl4 /= 4096L; |
||
10 | KopplungsteilNickRoll = tmpl3; |
||
11 | KopplungsteilRollNick = tmpl4; |
||
12 | tmpl4 -= tmpl3; |
||
13 | ErsatzKompass += tmpl4; |
||
14 | if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen |
||
15 | |||
16 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
||
17 | tmpl *= Parameter_AchsKopplung1; // 90 |
||
18 | tmpl /= 4096L; |
||
19 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
||
20 | tmpl2 *= Parameter_AchsKopplung1; |
||
21 | tmpl2 /= 4096L; |
||
22 | if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
||
23 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
||
24 | } |
||
25 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
||
26 | |||
27 | TrimRoll = tmpl - tmpl2 / 100L; |
||
28 | TrimNick = -tmpl2 + tmpl / 100L; |
||
29 | |||
30 | |||
31 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
||
32 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
||
33 | Mess_IntegralRoll += MesswertRoll + TrimRoll - LageKorrekturRoll; |
||
34 | if(Mess_IntegralRoll > Umschlag180Roll) |
||
35 | { |
||
36 | Mess_IntegralRoll = -(Umschlag180Roll - 25000L); |
||
37 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
||
38 | } |
||
39 | if(Mess_IntegralRoll <-Umschlag180Roll) |
||
40 | { |
||
41 | Mess_IntegralRoll = (Umschlag180Roll - 25000L); |
||
42 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
||
43 | } |
||
44 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
||
45 | Mess_IntegralNick2 += MesswertNick + TrimNick; |
||
46 | Mess_IntegralNick += MesswertNick + TrimNick - LageKorrekturNick; |
||
47 | if(Mess_IntegralNick > Umschlag180Nick) |
||
48 | { |
||
49 | Mess_IntegralNick = -(Umschlag180Nick - 25000L); |
||
50 | Mess_IntegralNick2 = Mess_IntegralNick; |
||
51 | } |
||
52 | if(Mess_IntegralNick <-Umschlag180Nick) |
||
53 | { |
||
54 | Mess_IntegralNick = (Umschlag180Nick - 25000L); |
||
55 | Mess_IntegralNick2 = Mess_IntegralNick; |
||
56 | } |
||
57 | |||
58 | |||
59 | .. |
||
60 | |||
61 | if(RohMesswertRoll > 0) TrimRoll += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
||
62 | else TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L; |
||
63 | if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
||
64 | else TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L; |
||
65 | |||
66 | |||
67 | #define TRIM_MAX 200 |
||
68 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
||
69 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
||
70 | |||
1645 | - | 71 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING); |
72 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING); |
||
73 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING)); |
||
1612 | dongfang | 74 | |
75 | messwertNick er nu P-part.... |
||
76 |