Subversion Repositories FlightCtrl

Rev

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
 
71
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
72
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
73
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
74
 
75
messwertNick er nu P-part....
76