Subversion Repositories FlightCtrl

Rev

Rev 395 | Rev 419 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 395 Rev 401
1 
/*#######################################################################################
 1 
/*#######################################################################################
2 
Flight Control
 2 
Flight Control
3 
#######################################################################################*/
 3 
#######################################################################################*/
4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 
// + Copyright (c) 04.2007 Holger Buss
 5 
// + Copyright (c) 04.2007 Holger Buss
6 
// + Nur für den privaten Gebrauch
 6 
// + Nur für den privaten Gebrauch
7 
// + www.MikroKopter.com
 7 
// + www.MikroKopter.com
8 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 8 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
9 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
9 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
10 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
10 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
11 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
11 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
12 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
12 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
13 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 13 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
14 
// + Verkauf von Luftbildaufnahmen, usw.
 14 
// + Verkauf von Luftbildaufnahmen, usw.
15 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 15 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
16 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
17 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 17 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
18 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 18 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 19 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
20 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 20 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
21 
// + eindeutig als Ursprung verlinkt werden
 21 
// + eindeutig als Ursprung verlinkt werden
22 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 22 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 23 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
24 
// + Benutzung auf eigene Gefahr
 24 
// + Benutzung auf eigene Gefahr
25 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 25 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
26 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 26 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27 
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
27 
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
28 
// + mit unserer Zustimmung zulässig
 28 
// + mit unserer Zustimmung zulässig
29 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 30 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
32 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
33 
// + this list of conditions and the following disclaimer.
 33 
// + this list of conditions and the following disclaimer.
34 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 34 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
35 
// +     from this software without specific prior written permission.
 35 
// +     from this software without specific prior written permission.
36 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
36 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
37 
// +     for non-commercial use (directly or indirectly)
 37 
// +     for non-commercial use (directly or indirectly)
38 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
38 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
39 
// +     with our written permission
 39 
// +     with our written permission
40 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
40 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
41 
// +     clearly linked as origin
41 
// +     clearly linked as origin
42 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 42 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
43 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 43 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
44 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 44 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
45 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 45 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
46 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 46 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
47 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 47 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
48 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 48 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
49 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 49 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
51 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 51 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
52 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 52 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
53 
// +  POSSIBILITY OF SUCH DAMAGE.
53 
// +  POSSIBILITY OF SUCH DAMAGE.
54 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 54 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
55 
 
 55 
 
56 
#include "main.h"
 56 
#include "main.h"
57 
#include "eeprom.c"
 57 
#include "eeprom.c"
58 
 
 58 
 
59 
unsigned char h,m,s;
 59 
unsigned char h,m,s;
60 
volatile unsigned int I2CTimeout = 100;
 60 
volatile unsigned int I2CTimeout = 100;
61 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 61 
volatile int MesswertNick,MesswertRoll,MesswertGier;
62 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 62 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
63 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 63 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
64 
volatile float NeutralAccZ = 0;
 64 
volatile float NeutralAccZ = 0;
65 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 65 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
66 
volatile long IntegralNick = 0,IntegralNick2 = 0;
 66 
volatile long IntegralNick = 0,IntegralNick2 = 0;
67 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
 67 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
68 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 68 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
69 
volatile long Integral_Gier = 0;
 69 
volatile long Integral_Gier = 0;
70 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 70 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
71 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 71 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
72 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 72 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
73 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 73 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
74 
volatile long Mess_Integral_Hoch = 0;
 74 
volatile long Mess_Integral_Hoch = 0;
75 
volatile int  KompassValue = 0;
 75 
volatile int  KompassValue = 0;
76 
volatile int  KompassStartwert = 0;
 76 
volatile int  KompassStartwert = 0;
77 
volatile int  KompassRichtung = 0;
 77 
volatile int  KompassRichtung = 0;
78 
unsigned char MAX_GAS,MIN_GAS;
 78 
unsigned char MAX_GAS,MIN_GAS;
79 
unsigned char Notlandung = 0;
 79 
unsigned char Notlandung = 0;
80 
unsigned char HoehenReglerAktiv = 0;
 80 
unsigned char HoehenReglerAktiv = 0;
81 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 81 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
82 
 
 82 
 
83 
float GyroFaktor;
 83 
float GyroFaktor;
84 
float IntegralFaktor;
 84 
float IntegralFaktor;
85 
 
 85 
 
86 
volatile int  DiffNick,DiffRoll;
 86 
volatile int  DiffNick,DiffRoll;
87 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 87 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
88 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 88 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
89 
unsigned char MotorWert[5];
 89 
unsigned char MotorWert[5];
90 
volatile unsigned char SenderOkay = 0;
 90 
volatile unsigned char SenderOkay = 0;
91 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 91 
int StickNick = 0,StickRoll = 0,StickGier = 0;
92 
char MotorenEin = 0;
 92 
char MotorenEin = 0;
93 
int HoehenWert = 0;
 93 
int HoehenWert = 0;
94 
int SollHoehe = 0;
 94 
int SollHoehe = 0;
95 
int I_LageRoll = 0,I_LageNick = 0;
 95 
int I_LageRoll = 0,I_LageNick = 0;
96 
float Kp =  FAKTOR_P;
 96 
float Kp =  FAKTOR_P;
97 
float Ki =  FAKTOR_I;
 97 
float Ki =  FAKTOR_I;
98 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
 98 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
99 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 99 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
100 
 
 100 
 
101 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 101 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
102 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 102 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
103 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 103 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
104 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 104 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
105 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
 105 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
106 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 106 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
107 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 107 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
108 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 108 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
109 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 109 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
110 
unsigned char Parameter_UserParam1 = 0;
 110 
unsigned char Parameter_UserParam1 = 0;
111 
unsigned char Parameter_UserParam2 = 0;
 111 
unsigned char Parameter_UserParam2 = 0;
112 
unsigned char Parameter_UserParam3 = 0;
 112 
unsigned char Parameter_UserParam3 = 0;
113 
unsigned char Parameter_UserParam4 = 0;
 113 
unsigned char Parameter_UserParam4 = 0;
114 
unsigned char Parameter_ServoNickControl = 100;
 114 
unsigned char Parameter_ServoNickControl = 100;
115 
unsigned char Parameter_LoopGasLimit = 70;
 115 
unsigned char Parameter_LoopGasLimit = 70;
116 
unsigned char Parameter_AchsKopplung1 = 0;
 116 
unsigned char Parameter_AchsKopplung1 = 0;
117 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 117 
unsigned char Parameter_AchsGegenKopplung1 = 0;
118 
struct mk_param_struct EE_Parameter;
 118 
struct mk_param_struct EE_Parameter;
119 
 
 119 
 
120 
void Piep(unsigned char Anzahl)
 120 
void Piep(unsigned char Anzahl)
121 
{
 121 
{
122 
 while(Anzahl--)
 122 
 while(Anzahl--)
123 
 {
 123 
 {
124 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 124 
  if(MotorenEin) return; //auf keinen Fall im Flug!
125 
  beeptime = 100;
 125 
  beeptime = 100;
126 
  Delay_ms(250);
 126 
  Delay_ms(250);
127 
 }
 127 
 }
128 
}
 128 
}
129 
 
 129 
 
130 
//############################################################################
 130 
//############################################################################
131 
//  Nullwerte ermitteln
 131 
//  Nullwerte ermitteln
132 
void SetNeutral(void)
 132 
void SetNeutral(void)
133 
//############################################################################
 133 
//############################################################################
134 
{
 134 
{
135 
        NeutralAccX = 0;
 135 
        NeutralAccX = 0;
136 
        NeutralAccY = 0;
 136 
        NeutralAccY = 0;
137 
        NeutralAccZ = 0;
 137 
        NeutralAccZ = 0;
138 
    AdNeutralNick = 0; 
 138 
    AdNeutralNick = 0; 
139 
        AdNeutralRoll = 0;     
 139 
        AdNeutralRoll = 0;     
140 
        AdNeutralGier = 0;
 140 
        AdNeutralGier = 0;
141 
    Parameter_AchsKopplung1 = 0;
 141 
    Parameter_AchsKopplung1 = 0;
142 
    Parameter_AchsGegenKopplung1 = 0;
 142 
    Parameter_AchsGegenKopplung1 = 0;
143 
    CalibrierMittelwert();     
 143 
    CalibrierMittelwert();     
144 
    Delay_ms_Mess(100);
 144 
    Delay_ms_Mess(100);
145 
        CalibrierMittelwert();
 145 
        CalibrierMittelwert();
146 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 146 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
147 
     {    
 147 
     {    
148 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 148 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
149 
     }
 149 
     }
150 
 
 150 
 
151 
     AdNeutralNick= AdWertNick;
 151 
     AdNeutralNick= AdWertNick;
152 
         AdNeutralRoll= AdWertRoll;    
 152 
         AdNeutralRoll= AdWertRoll;    
153 
         AdNeutralGier= AdWertGier;
 153 
         AdNeutralGier= AdWertGier;
154 
 
 - 
 
- 
 
 154 
     StartNeutralRoll = AdNeutralRoll;
- 
 
 155 
     StartNeutralNick = AdNeutralNick;
155 
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 156 
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
156 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 157 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
157 
        NeutralAccZ = Aktuell_az;
 158 
        NeutralAccZ = Aktuell_az;
158 
   
 159 
   
159 
        Mess_IntegralNick = 0; 
 160 
        Mess_IntegralNick = 0; 
160 
    Mess_IntegralNick2 = 0;
 161 
    Mess_IntegralNick2 = 0;
161 
    Mess_IntegralRoll = 0;     
 162 
    Mess_IntegralRoll = 0;     
162 
    Mess_IntegralRoll2 = 0;
 163 
    Mess_IntegralRoll2 = 0;
163 
    Mess_Integral_Gier = 0;    
 164 
    Mess_Integral_Gier = 0;    
164 
    MesswertNick = 0;
 165 
    MesswertNick = 0;
165 
    MesswertRoll = 0;
 166 
    MesswertRoll = 0;
166 
    MesswertGier = 0;
 167 
    MesswertGier = 0;
167 
    StartLuftdruck = Luftdruck;
 168 
    StartLuftdruck = Luftdruck;
168 
    HoeheD = 0;
 169 
    HoeheD = 0;
169 
    Mess_Integral_Hoch = 0;
 170 
    Mess_Integral_Hoch = 0;
170 
    KompassStartwert = KompassValue;
 171 
    KompassStartwert = KompassValue;
171 
    GPS_Neutral();
 172 
    GPS_Neutral();
172 
    beeptime = 50;  
 173 
    beeptime = 50;  
173 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 174 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
174 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
 175 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
175 
}
 176 
}
176 
 
 177 
 
177 
//############################################################################
 178 
//############################################################################
178 
// Bearbeitet die Messwerte
 179 
// Bearbeitet die Messwerte
179 
void Mittelwert(void)
 180 
void Mittelwert(void)
180 
//############################################################################
 181 
//############################################################################
181 
{      
 182 
{      
182 
    static signed long tmpl,tmpl2;     
 183 
    static signed long tmpl,tmpl2;     
183 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 184 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
184 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 185 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
185 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 186 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
186 
    if(PlatinenVersion != 10)
 - 
 
187 
     {
 - 
 
188 
      MesswertGier *= 2;
 - 
 
189 
          MesswertRoll *= 2;
 - 
 
190 
      MesswertNick *= 2;
 - 
 
191 
     }
 187 
 
192 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
 188 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
193 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 189 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
194 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
 190 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
195 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
 191 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
196 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
 192 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
197 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
 193 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
198 
    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
 194 
    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
199 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
 195 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
200 
            Mess_Integral_Gier +=  MesswertGier;
 196 
            Mess_Integral_Gier +=  MesswertGier;
201 
            Mess_Integral_Gier2 += MesswertGier;
 197 
            Mess_Integral_Gier2 += MesswertGier;
202 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 198 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
203 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
 199 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
204 
         {
 200 
         {
205 
            tmpl = Mess_IntegralNick / 4096L;
 201 
            tmpl = Mess_IntegralNick / 4096L;
206 
            tmpl *= MesswertGier;
 202 
            tmpl *= MesswertGier;
207 
            tmpl *= Parameter_AchsKopplung1;  //125
 203 
            tmpl *= Parameter_AchsKopplung1;  //125
208 
            tmpl /= 2048L;
 204 
            tmpl /= 2048L;
209 
            tmpl2 = Mess_IntegralRoll / 4096L;
 205 
            tmpl2 = Mess_IntegralRoll / 4096L;
210 
            tmpl2 *= MesswertGier;
 206 
            tmpl2 *= MesswertGier;
211 
            tmpl2 *= Parameter_AchsKopplung1;
 207 
            tmpl2 *= Parameter_AchsKopplung1;
212 
            tmpl2 /= 2048L;
 208 
            tmpl2 /= 2048L;
213 
         }
 209 
         }
214 
      else  tmpl = tmpl2 = 0;
 210 
      else  tmpl = tmpl2 = 0;
215 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
 211 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
216 
            MesswertRoll += tmpl;
 212 
            MesswertRoll += tmpl;
217 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
 213 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
218 
            Mess_IntegralRoll2 += MesswertRoll;
 214 
            Mess_IntegralRoll2 += MesswertRoll;
219 
            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;
 215 
            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;
220 
            if(Mess_IntegralRoll > Umschlag180Roll)
 216 
            if(Mess_IntegralRoll > Umschlag180Roll)
221 
            {
 217 
            {
222 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
 218 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
223 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 219 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
224 
            }
 220 
            }
225 
            if(Mess_IntegralRoll <-Umschlag180Roll)
 221 
            if(Mess_IntegralRoll <-Umschlag180Roll)
226 
            {
 222 
            {
227 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
 223 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
228 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 224 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
229 
            }  
 225 
            }  
230 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
 226 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
231 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
 227 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
- 
 
 228 
            if(PlatinenVersion == 10)
- 
 
 229 
                         {
232 
            if(AdWertRoll > 1010) MesswertRoll = +1000;
 230 
              if(AdWertRoll > 1010) MesswertRoll = +1000;
233 
            if(AdWertRoll > 1017) MesswertRoll = +2000;
 231 
              if(AdWertRoll > 1017) MesswertRoll = +2000;
- 
 
 232 
                         }
- 
 
 233 
                         else
- 
 
 234 
                         {
- 
 
 235 
              if(AdWertRoll > 2020) MesswertRoll = +1000;
- 
 
 236 
              if(AdWertRoll > 2034) MesswertRoll = +2000;
- 
 
 237 
                         }
234 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
 238 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
235 
            MesswertNick -= tmpl2;
 239 
            MesswertNick -= tmpl2;
236 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
 240 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
237 
            Mess_IntegralNick2 += MesswertNick;
 241 
            Mess_IntegralNick2 += MesswertNick;
238 
            Mess_IntegralNick  += MesswertNick - I_LageNick;
 242 
            Mess_IntegralNick  += MesswertNick - I_LageNick;
239 
            if(Mess_IntegralNick > Umschlag180Nick)
 243 
            if(Mess_IntegralNick > Umschlag180Nick)
240 
            {
 244 
            {
241 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
 245 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
242 
             Mess_IntegralNick2 = Mess_IntegralNick;
 246 
             Mess_IntegralNick2 = Mess_IntegralNick;
243 
            }
 247 
            }
244 
            if(Mess_IntegralNick <-Umschlag180Nick)
 248 
            if(Mess_IntegralNick <-Umschlag180Nick)
245 
            {
 249 
            {
246 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
 250 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
247 
             Mess_IntegralNick2 = Mess_IntegralNick;
 251 
             Mess_IntegralNick2 = Mess_IntegralNick;
248 
            }
 252 
            }
249 
            if(AdWertNick < 15)   MesswertNick = -1000;
 253 
            if(AdWertNick < 15)   MesswertNick = -1000;
250 
            if(AdWertNick <  7)   MesswertNick = -2000;
 254 
            if(AdWertNick <  7)   MesswertNick = -2000;
- 
 
 255 
            if(PlatinenVersion == 10)
- 
 
 256 
                         {
251 
            if(AdWertNick > 1010) MesswertNick = +1000;
 257 
              if(AdWertNick > 1010) MesswertNick = +1000;
252 
            if(AdWertNick > 1017) MesswertNick = +2000;
 258 
              if(AdWertNick > 1017) MesswertNick = +2000;
- 
 
 259 
                         }
- 
 
 260 
                         else
- 
 
 261 
                         {
- 
 
 262 
              if(AdWertNick > 2020) MesswertNick = +1000;
- 
 
 263 
              if(AdWertNick > 2034) MesswertNick = +2000;
- 
 
 264 
                         }
253 
//++++++++++++++++++++++++++++++++++++++++++++++++
 265 
//++++++++++++++++++++++++++++++++++++++++++++++++
254 
// ADC einschalten
 266 
// ADC einschalten
255 
    ANALOG_ON; 
 267 
    ANALOG_ON; 
256 
//++++++++++++++++++++++++++++++++++++++++++++++++
 268 
//++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 269 
DebugOut.Analog[11] = MesswertRoll;
257 
 
 270 
 
258 
    Integral_Gier  = Mess_Integral_Gier;
 271 
    Integral_Gier  = Mess_Integral_Gier;
259 
    IntegralNick = Mess_IntegralNick;
 272 
    IntegralNick = Mess_IntegralNick;
260 
    IntegralRoll = Mess_IntegralRoll;
 273 
    IntegralRoll = Mess_IntegralRoll;
261 
    IntegralNick2 = Mess_IntegralNick2;
 274 
    IntegralNick2 = Mess_IntegralNick2;
262 
    IntegralRoll2 = Mess_IntegralRoll2;
 275 
    IntegralRoll2 = Mess_IntegralRoll2;
263 
 
 276 
 
264 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
 277 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
265 
  {
 278 
  {
266 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
 279 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
267 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 280 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
268 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
 281 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
269 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 282 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
270 
  }
 283 
  }
271 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 284 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
272 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 285 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
273 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 286 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
274 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 287 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
275 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 288 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
276 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 289 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
277 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 290 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
278 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 291 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
279 
}
 292 
}
280 
 
 293 
 
281 
//############################################################################
 294 
//############################################################################
282 
// Messwerte beim Ermitteln der Nullage
 295 
// Messwerte beim Ermitteln der Nullage
283 
void CalibrierMittelwert(void)
 296 
void CalibrierMittelwert(void)
284 
//############################################################################
 297 
//############################################################################
285 
{                
 298 
{                
286 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 299 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
287 
        ANALOG_OFF;
 300 
        ANALOG_OFF;
288 
        MesswertNick = AdWertNick;
 301 
        MesswertNick = AdWertNick;
289 
        MesswertRoll = AdWertRoll;
 302 
        MesswertRoll = AdWertRoll;
290 
        MesswertGier = AdWertGier;
 303 
        MesswertGier = AdWertGier;
291 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
 304 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
292 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
 305 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
293 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
 306 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
294 
   // ADC einschalten
 307 
   // ADC einschalten
295 
    ANALOG_ON; 
 308 
    ANALOG_ON; 
296 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 309 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
297 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 310 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
298 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 311 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
299 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 312 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
300 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 313 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
301 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 314 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
302 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 315 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
303 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 316 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
304 
 
 317 
 
305 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 318 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
306 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 319 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
307 
}
 320 
}
308 
 
 321 
 
309 
//############################################################################
 322 
//############################################################################
310 
// Senden der Motorwerte per I2C-Bus
 323 
// Senden der Motorwerte per I2C-Bus
311 
void SendMotorData(void)
 324 
void SendMotorData(void)
312 
//############################################################################
 325 
//############################################################################
313 
{
 326 
{
314 
    if(MOTOR_OFF || !MotorenEin)
 327 
    if(MOTOR_OFF || !MotorenEin)
315 
        {
 328 
        {
316 
        Motor_Hinten = 0;
 329 
        Motor_Hinten = 0;
317 
        Motor_Vorne = 0;
 330 
        Motor_Vorne = 0;
318 
        Motor_Rechts = 0;
 331 
        Motor_Rechts = 0;
319 
        Motor_Links = 0;
 332 
        Motor_Links = 0;
320 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 333 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
321 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 334 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
322 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 335 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
323 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 336 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
324 
        }
 337 
        }
325 
 
 338 
 
326 
    DebugOut.Analog[12] = Motor_Vorne;
 339 
    DebugOut.Analog[12] = Motor_Vorne;
327 
    DebugOut.Analog[13] = Motor_Hinten;
 340 
    DebugOut.Analog[13] = Motor_Hinten;
328 
    DebugOut.Analog[14] = Motor_Links;
 341 
    DebugOut.Analog[14] = Motor_Links;
329 
    DebugOut.Analog[15] = Motor_Rechts;  
 342 
    DebugOut.Analog[15] = Motor_Rechts;  
330 
 
 343 
 
331 
    //Start I2C Interrupt Mode
 344 
    //Start I2C Interrupt Mode
332 
    twi_state = 0;
 345 
    twi_state = 0;
333 
    motor = 0;
 346 
    motor = 0;
334 
    i2c_start();
 347 
    i2c_start();
335 
}
 348 
}
336 
 
 349 
 
337 
 
 350 
 
338 
 
 351 
 
339 
//############################################################################
 352 
//############################################################################
340 
// Trägt ggf. das Poti als Parameter ein
 353 
// Trägt ggf. das Poti als Parameter ein
341 
void ParameterZuordnung(void)
 354 
void ParameterZuordnung(void)
342 
//############################################################################
 355 
//############################################################################
343 
{
 356 
{
344 
 
 357 
 
345 
 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
 358 
 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
346 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 359 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
347 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 360 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
348 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 361 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
349 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 362 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
350 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 363 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
351 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 364 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
352 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 365 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
353 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 366 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
354 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 367 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
355 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 368 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
356 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 369 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
357 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 370 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
358 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 371 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
359 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 372 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
360 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
 373 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
361 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 374 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
362 
 
 375 
 
363 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 376 
 Ki = (float) Parameter_I_Faktor * 0.0001;
364 
 MAX_GAS = EE_Parameter.Gas_Max;
 377 
 MAX_GAS = EE_Parameter.Gas_Max;
365 
 MIN_GAS = EE_Parameter.Gas_Min;
 378 
 MIN_GAS = EE_Parameter.Gas_Min;
366 
}
 379 
}
367 
 
 380 
 
368 
 
 381 
 
369 
//############################################################################
 382 
//############################################################################
370 
//
 383 
//
371 
void MotorRegler(void)
 384 
void MotorRegler(void)
372 
//############################################################################
 385 
//############################################################################
373 
{
 386 
{
374 
         int motorwert,pd_ergebnis,h,tmp_int;
 387 
         int motorwert,pd_ergebnis,h,tmp_int;
375 
         int GierMischanteil,GasMischanteil;
 388 
         int GierMischanteil,GasMischanteil;
376 
     static long SummeNick=0,SummeRoll=0;
 389 
     static long SummeNick=0,SummeRoll=0;
377 
     static long sollGier = 0,tmp_long,tmp_long2;
 390 
     static long sollGier = 0,tmp_long,tmp_long2;
378 
     static long IntegralFehlerNick = 0;
 391 
     static long IntegralFehlerNick = 0;
379 
     static long IntegralFehlerRoll = 0;
 392 
     static long IntegralFehlerRoll = 0;
380 
         static unsigned int RcLostTimer;
 393 
         static unsigned int RcLostTimer;
381 
         static unsigned char delay_neutral = 0;
 394 
         static unsigned char delay_neutral = 0;
382 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 395 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
383 
         static unsigned int  modell_fliegt = 0;
 396 
         static unsigned int  modell_fliegt = 0;
384 
     static int hoehenregler = 0;
 397 
     static int hoehenregler = 0;
385 
     static char TimerWerteausgabe = 0;
 398 
     static char TimerWerteausgabe = 0;
386 
     static char NeueKompassRichtungMerken = 0;
 399 
     static char NeueKompassRichtungMerken = 0;
387 
     static long ausgleichNick, ausgleichRoll;
 400 
     static long ausgleichNick, ausgleichRoll;
388 
     
 401 
     
389 
        Mittelwert();
 402 
        Mittelwert();
390 
 
 403 
 
391 
    GRN_ON;
 404 
    GRN_ON;
392 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
405 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
393 
// Gaswert ermitteln
 406 
// Gaswert ermitteln
394 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
407 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
395 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 408 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
396 
    if(GasMischanteil < 0) GasMischanteil = 0;
 409 
    if(GasMischanteil < 0) GasMischanteil = 0;
397 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
410 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
398 
// Emfang schlecht
 411 
// Emfang schlecht
399 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
412 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
400 
   if(SenderOkay < 100)
 413 
   if(SenderOkay < 100)
401 
        {
 414 
        {
402 
        if(!PcZugriff)
 415 
        if(!PcZugriff)
403 
         {
 416 
         {
404 
           if(BeepMuster == 0xffff)
 417 
           if(BeepMuster == 0xffff)
405 
            {
 418 
            {
406 
             beeptime = 15000;
 419 
             beeptime = 15000;
407 
             BeepMuster = 0x0c00;
 420 
             BeepMuster = 0x0c00;
408 
            }
 421 
            }
409 
         }
 422 
         }
410 
        if(RcLostTimer) RcLostTimer--;
 423 
        if(RcLostTimer) RcLostTimer--;
411 
        else
 424 
        else
412 
         {
 425 
         {
413 
          MotorenEin = 0;
 426 
          MotorenEin = 0;
414 
          Notlandung = 0;
 427 
          Notlandung = 0;
415 
         }
 428 
         }
416 
        ROT_ON;
 429 
        ROT_ON;
417 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 430 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
418 
            {
 431 
            {
419 
            GasMischanteil = EE_Parameter.NotGas;
 432 
            GasMischanteil = EE_Parameter.NotGas;
420 
            Notlandung = 1;
 433 
            Notlandung = 1;
421 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 434 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
422 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 435 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
423 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 436 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
424 
            }
 437 
            }
425 
         else MotorenEin = 0;
 438 
         else MotorenEin = 0;
426 
        }
 439 
        }
427 
        else
 440 
        else
428 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
441 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
429 
// Emfang gut
 442 
// Emfang gut
430 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
443 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
431 
        if(SenderOkay > 140)
 444 
        if(SenderOkay > 140)
432 
            {
 445 
            {
433 
            Notlandung = 0;
 446 
            Notlandung = 0;
434 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 447 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
435 
            if(GasMischanteil > 40)
 448 
            if(GasMischanteil > 40)
436 
                {
 449 
                {
437 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 450 
                if(modell_fliegt < 0xffff) modell_fliegt++;
438 
                }
 451 
                }
439 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 452 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
440 
                {
 453 
                {
441 
                SummeNick = 0;
 454 
                SummeNick = 0;
442 
                SummeRoll = 0;
 455 
                SummeRoll = 0;
443 
                Mess_Integral_Gier = 0;
 456 
                Mess_Integral_Gier = 0;
444 
                Mess_Integral_Gier2 = 0;
 457 
                Mess_Integral_Gier2 = 0;
445 
                }
 458 
                }
446 
            if((GasMischanteil > 200) && MotorenEin == 0)
 459 
            if((GasMischanteil > 200) && MotorenEin == 0)
447 
                {
 460 
                {
448 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
461 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
449 
// auf Nullwerte kalibrieren
 462 
// auf Nullwerte kalibrieren
450 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
463 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
451 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 464 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
452 
                    {
 465 
                    {
453 
                    unsigned char setting = 2;
 466 
                    unsigned char setting = 2;
454 
                    if(++delay_neutral > 200)  // nicht sofort
 467 
                    if(++delay_neutral > 200)  // nicht sofort
455 
                        {
 468 
                        {
456 
                        GRN_OFF;
 469 
                        GRN_OFF;
457 
                        MotorenEin = 0;
 470 
                        MotorenEin = 0;
458 
                        delay_neutral = 0;
 471 
                        delay_neutral = 0;
459 
                        modell_fliegt = 0;
 472 
                        modell_fliegt = 0;
460 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 473 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
461 
                        {
 474 
                        {
462 
                         unsigned char setting=1;
 475 
                         unsigned char setting=1;
463 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 476 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
464 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 477 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
465 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 478 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
466 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 479 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
467 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 480 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
468 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 481 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
469 
                        }
 482 
                        }
470 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 483 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
471 
                          {
 484 
                          {
472 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 485 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
473 
                          }  
 486 
                          }  
474 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 487 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
475 
                        SetNeutral();
 488 
                        SetNeutral();
476 
                        Piep(GetActiveParamSetNumber());
 489 
                        Piep(GetActiveParamSetNumber());
477 
                        }
 490 
                        }
478 
                    }
 491 
                    }
479 
                 else delay_neutral = 0;
 492 
                 else delay_neutral = 0;
480 
                }
 493 
                }
481 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
494 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
482 
// Gas ist unten
 495 
// Gas ist unten
483 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
496 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
484 
            if(GasMischanteil < 35)
 497 
            if(GasMischanteil < 35)
485 
                {
 498 
                {
486 
                // Starten
 499 
                // Starten
487 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 500 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
488 
                    {
 501 
                    {
489 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
502 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
490 
// Einschalten
 503 
// Einschalten
491 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
504 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
492 
                    if(++delay_einschalten > 200)
 505 
                    if(++delay_einschalten > 200)
493 
                        {
 506 
                        {
494 
                        delay_einschalten = 200;
 507 
                        delay_einschalten = 200;
495 
                        modell_fliegt = 1;
 508 
                        modell_fliegt = 1;
496 
                        MotorenEin = 1;
 509 
                        MotorenEin = 1;
497 
                        sollGier = 0;
 510 
                        sollGier = 0;
498 
                        Mess_Integral_Gier = 0;
 511 
                        Mess_Integral_Gier = 0;
499 
                        Mess_Integral_Gier2 = 0;
 512 
                        Mess_Integral_Gier2 = 0;
500 
                        Mess_IntegralNick = 0;
 513 
                        Mess_IntegralNick = 0;
501 
                        Mess_IntegralRoll = 0;
 514 
                        Mess_IntegralRoll = 0;
502 
                        Mess_IntegralNick2 = IntegralNick;
 515 
                        Mess_IntegralNick2 = IntegralNick;
503 
                        Mess_IntegralRoll2 = IntegralRoll;
 516 
                        Mess_IntegralRoll2 = IntegralRoll;
504 
                        SummeNick = 0;
 517 
                        SummeNick = 0;
505 
                        SummeRoll = 0;
 518 
                        SummeRoll = 0;
506 
                        }          
 519 
                        }          
507 
                    }  
 520 
                    }  
508 
                    else delay_einschalten = 0;
 521 
                    else delay_einschalten = 0;
509 
                //Auf Neutralwerte setzen
 522 
                //Auf Neutralwerte setzen
510 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
523 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
511 
// Auschalten
 524 
// Auschalten
512 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
525 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
513 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 526 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
514 
                    {
 527 
                    {
515 
                    if(++delay_ausschalten > 200)  // nicht sofort
 528 
                    if(++delay_ausschalten > 200)  // nicht sofort
516 
                        {
 529 
                        {
517 
                        MotorenEin = 0;
 530 
                        MotorenEin = 0;
518 
                        delay_ausschalten = 200;
 531 
                        delay_ausschalten = 200;
519 
                        modell_fliegt = 0;
 532 
                        modell_fliegt = 0;
520 
                        }
 533 
                        }
521 
                    }
 534 
                    }
522 
                else delay_ausschalten = 0;
 535 
                else delay_ausschalten = 0;
523 
                }
 536 
                }
524 
            }
 537 
            }
525 
 
 538 
 
526 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
539 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
527 
// neue Werte von der Funke
 540 
// neue Werte von der Funke
528 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
541 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
529 
 if(!NewPpmData-- || Notlandung)  
 542 
 if(!NewPpmData-- || Notlandung)  
530 
  {
 543 
  {
531 
    ParameterZuordnung();
 544 
    ParameterZuordnung();
532 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 545 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
533 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 546 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
534 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 547 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
535 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 548 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
536 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 549 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
537 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 550 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
538 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 551 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
539 
 
 552 
 
540 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 553 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
541 
    if(GyroFaktor < 0) GyroFaktor = 0;
 554 
    if(GyroFaktor < 0) GyroFaktor = 0;
542 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 555 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
543 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
 556 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
544 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 557 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
545 
     {
 558 
     {
546 
      if(IntegralNick >  60000)  
 559 
      if(IntegralNick >  60000)  
547 
      {
 560 
      {
548 
       StickNick -=  8 * EE_Parameter.Stick_P;      
 561 
       StickNick -=  8 * EE_Parameter.Stick_P;      
549 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 562 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
550 
      }
 563 
      }
551 
      else
 564 
      else
552 
      if(IntegralNick < -60000)  
 565 
      if(IntegralNick < -60000)  
553 
      {
 566 
      {
554 
       StickNick += 8 * EE_Parameter.Stick_P;
 567 
       StickNick += 8 * EE_Parameter.Stick_P;
555 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 568 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
556 
      }
 569 
      }
557 
      if(IntegralRoll >  60000)  
 570 
      if(IntegralRoll >  60000)  
558 
      {
 571 
      {
559 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
 572 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
560 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 573 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
561 
      }
 574 
      }
562 
      else
 575 
      else
563 
      if(IntegralRoll < -60000)  
 576 
      if(IntegralRoll < -60000)  
564 
      {
 577 
      {
565 
       StickRoll += 8 * EE_Parameter.Stick_P;
 578 
       StickRoll += 8 * EE_Parameter.Stick_P;
566 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 579 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
567 
      }
 580 
      }
568 
     }
 581 
     }
569 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
582 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
570 
// Looping?
 583 
// Looping?
571 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
584 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
572 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
 585 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
573 
  else
 586 
  else
574 
   {
 587 
   {
575 
     {
 588 
     {
576 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
 589 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
577 
     }  
 590 
     }  
578 
   }
 591 
   }
579 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
 592 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
580 
   else
 593 
   else
581 
   {
 594 
   {
582 
   if(Looping_Rechts) // Hysterese
 595 
   if(Looping_Rechts) // Hysterese
583 
     {
 596 
     {
584 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
 597 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
585 
     }
 598 
     }
586 
   }
 599 
   }
587 
 
 600 
 
588 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
 601 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
589 
  else
 602 
  else
590 
   {
 603 
   {
591 
    if(Looping_Oben)  // Hysterese
 604 
    if(Looping_Oben)  // Hysterese
592 
     {
 605 
     {
593 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
 606 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
594 
     }  
 607 
     }  
595 
   }
 608 
   }
596 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
 609 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
597 
   else
 610 
   else
598 
   {
 611 
   {
599 
    if(Looping_Unten) // Hysterese
 612 
    if(Looping_Unten) // Hysterese
600 
     {
 613 
     {
601 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
 614 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
602 
     }
 615 
     }
603 
   }
 616 
   }
604 
 
 617 
 
605 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
 618 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
606 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
 619 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
607 
  } // Ende neue Funken-Werte
 620 
  } // Ende neue Funken-Werte
608 
 
 621 
 
609 
  if(Looping_Roll) beeptime = 100;
 622 
  if(Looping_Roll) beeptime = 100;
610 
  if(Looping_Roll || Looping_Nick)
 623 
  if(Looping_Roll || Looping_Nick)
611 
   {
 624 
   {
612 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 625 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
613 
   }
 626 
   }
614 
   
 627 
   
615 
 
 628 
 
616 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
629 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
617 
// Bei Empfangsausfall im Flug
630 
// Bei Empfangsausfall im Flug
618 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
631 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
619 
   if(Notlandung)
 632 
   if(Notlandung)
620 
    {
 633 
    {
621 
     StickGier = 0;
 634 
     StickGier = 0;
622 
     StickNick = 0;
 635 
     StickNick = 0;
623 
     StickRoll = 0;
 636 
     StickRoll = 0;
624 
     GyroFaktor  = 0.1;
 637 
     GyroFaktor  = 0.1;
625 
     IntegralFaktor = 0.005;
 638 
     IntegralFaktor = 0.005;
626 
     Looping_Roll = 0;
 639 
     Looping_Roll = 0;
627 
     Looping_Nick = 0;
 640 
     Looping_Nick = 0;
628 
    }  
 641 
    }  
629 
 
 642 
 
630 
 
 643 
 
631 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
644 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
632 
// Integrale auf ACC-Signal abgleichen
 645 
// Integrale auf ACC-Signal abgleichen
633 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
646 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
634 
#define ABGLEICH_ANZAHL 256L
 647 
#define ABGLEICH_ANZAHL 256L
635 
 
 648 
 
636 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
 649 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
637 
 MittelIntegralRoll  += IntegralRoll;
 650 
 MittelIntegralRoll  += IntegralRoll;
638 
 MittelIntegralNick2 += IntegralNick2;
 651 
 MittelIntegralNick2 += IntegralNick2;
639 
 MittelIntegralRoll2 += IntegralRoll2;
 652 
 MittelIntegralRoll2 += IntegralRoll2;
640 
 
 653 
 
641 
 if(Looping_Nick || Looping_Roll)
 654 
 if(Looping_Nick || Looping_Roll)
642 
  {
 655 
  {
643 
    IntegralAccNick = 0;
 656 
    IntegralAccNick = 0;
644 
    IntegralAccRoll = 0;
 657 
    IntegralAccRoll = 0;
645 
    MittelIntegralNick = 0;
 658 
    MittelIntegralNick = 0;
646 
    MittelIntegralRoll = 0;
 659 
    MittelIntegralRoll = 0;
647 
    MittelIntegralNick2 = 0;
 660 
    MittelIntegralNick2 = 0;
648 
    MittelIntegralRoll2 = 0;
 661 
    MittelIntegralRoll2 = 0;
649 
    Mess_IntegralNick2 = Mess_IntegralNick;
 662 
    Mess_IntegralNick2 = Mess_IntegralNick;
650 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
 663 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
651 
    ZaehlMessungen = 0;
 664 
    ZaehlMessungen = 0;
652 
    I_LageNick = 0;
 665 
    I_LageNick = 0;
653 
    I_LageRoll = 0;
 666 
    I_LageRoll = 0;
654 
  }
 667 
  }
655 
 
 668 
 
656 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 669 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
657 
 {
 670 
 {
658 
  static int cnt = 0;
 671 
  static int cnt = 0;
659 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
 672 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
660 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
 673 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
661 
  if(!Looping_Nick && !Looping_Roll)
 674 
  if(!Looping_Nick && !Looping_Roll)
662 
  {
 675 
  {
663 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
 676 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
664 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
 677 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
665 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
 678 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
666 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
 679 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
667 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 680 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
668 
 
 - 
 
669 
//DebugOut.Analog[6] = labs(MittelIntegralNick_Alt - MittelIntegralNick) / 10;
 681 
#define MAX_I 32
670 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
 682 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
671 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
 683 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
672 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 684 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
673 
    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
 685 
    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
674 
     {
 686 
     {
675 
       tmp_long = IntegralFehlerNick / 700L;
 687 
       tmp_long = IntegralFehlerNick / 700L;
676 
       if(tmp_long > 40) tmp_long = 40;
 688 
       if(tmp_long > MAX_I) tmp_long = MAX_I;
677 
       if(tmp_long <-40) tmp_long =-40;
 689 
       if(tmp_long <-MAX_I) tmp_long =-MAX_I;
678 
       I_LageNick = tmp_long;    
 690 
       I_LageNick = tmp_long;    
679 
     }
 691 
     }
680 
     else
 692 
     else
681 
     {
 693 
     {
682 
      I_LageNick /= 2;
 694 
      I_LageNick /= 2;
683 
     }
 695 
     }
684 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
696 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
685 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
 697 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
686 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 698 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
687 
    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
 699 
    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
688 
     {
 700 
     {
689 
      tmp_long2 = IntegralFehlerRoll / 700L;
 701 
      tmp_long2 = IntegralFehlerRoll / 700L;
690 
      if(tmp_long2 > 40) tmp_long2 = 40;
 702 
      if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
691 
      if(tmp_long2 <-40) tmp_long2 =-40;
 703 
      if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
692 
      I_LageRoll = tmp_long2;    
 704 
      I_LageRoll = tmp_long2;    
693 
     }
 705 
     }
694 
     else
 706 
     else
695 
     {
 707 
     {
696 
      I_LageRoll /=2;
 708 
      I_LageRoll /=2;
697 
     }
 709 
     }
698 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
710 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
699 
   MittelIntegralNick_Alt = MittelIntegralNick;      
 711 
   MittelIntegralNick_Alt = MittelIntegralNick;      
700 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
 712 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
701 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
713 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
702 
    Mess_IntegralNick -= ausgleichNick;
 714 
    Mess_IntegralNick -= ausgleichNick;
703 
    Mess_IntegralRoll -= ausgleichRoll;
 715 
    Mess_IntegralRoll -= ausgleichRoll;
704 
 
 716 
 
705 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
717 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
706 
// Gyro-Drift ermitteln
 718 
// Gyro-Drift ermitteln
707 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
719 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
708 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
 720 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
709 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
 721 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
710 
    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
 722 
    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
711 
    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
 723 
    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
712 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
 724 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
713 
 
 725 
 
714 
    IntegralFehlerNick = tmp_long;
 726 
    IntegralFehlerNick = tmp_long;
715 
    IntegralFehlerRoll = tmp_long2;
 727 
    IntegralFehlerRoll = tmp_long2;
716 
    Mess_IntegralNick2 -= IntegralFehlerNick;
 728 
    Mess_IntegralNick2 -= IntegralFehlerNick;
717 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
 729 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
718 
 
 - 
 
719 
    if(PlatinenVersion != 10)
 - 
 
720 
    {
 - 
 
721 
     tmp_long /= 2;
 - 
 
722 
     tmp_long2 /= 2;
 - 
 
723 
    }
 - 
 
724 
 
 730 
 
725 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 731 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
726 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 732 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
727 
 
 733 
 
728 
 
 734 
 
729 
DebugOut.Analog[30] = I_LageRoll;
 735 
DebugOut.Analog[30] = I_LageRoll * 10;
730 
DebugOut.Analog[17] = IntegralAccNick / 26;
 736 
DebugOut.Analog[17] = IntegralAccNick / 26;
731 
DebugOut.Analog[18] = IntegralAccRoll / 26;
 737 
DebugOut.Analog[18] = IntegralAccRoll / 26;
732 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 738 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
733 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 739 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
734 
DebugOut.Analog[21] = MittelIntegralNick / 26;
 740 
DebugOut.Analog[21] = MittelIntegralNick / 26;
735 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
 741 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
736 
DebugOut.Analog[28] = ausgleichNick;
 742 
DebugOut.Analog[28] = ausgleichNick;
737 
DebugOut.Analog[29] = ausgleichRoll;
 743 
DebugOut.Analog[29] = ausgleichRoll;
738 
 
 744 
 
739 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
 745 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
740 
#define FEHLER_LIMIT2  4096
 746 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4)
741 
 
 747 
 
742 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
 748 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
743 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 749 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
744 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
 750 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
745 
         {
 751 
         {
746 
           if(last_n_p)
 752 
           if(last_n_p)
747 
           {
 753 
           {
748 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 754 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
749 
            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 755 
            ausgleichNick = IntegralFehlerNick / 4;
750 
            if(ausgleichNick > 10000) ausgleichNick = 10000;
 756 
            if(ausgleichNick > 5000) ausgleichNick = 5000;
751 
            Mess_IntegralNick += ausgleichNick;
 757 
            Mess_IntegralNick -= ausgleichNick;
752 
           }
 758 
           }
753 
           else last_n_p = 1;
 759 
           else last_n_p = 1;
754 
         } else  last_n_p = 0;
 760 
         } else  last_n_p = 0;
755 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
 761 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
756 
         {
 762 
         {
757 
           if(last_n_n)
 763 
           if(last_n_n)
758 
            {
 764 
            {
759 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 765 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
760 
             ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 766 
             ausgleichNick = IntegralFehlerNick / 4;
761 
             if(ausgleichNick < -10000) ausgleichNick = -10000;
 767 
             if(ausgleichNick < -5000) ausgleichNick = -5000;
762 
             Mess_IntegralNick += ausgleichNick;
 768 
             Mess_IntegralNick -= ausgleichNick;
763 
            }
 769 
            }
764 
           else last_n_n = 1;
 770 
           else last_n_n = 1;
765 
         } else  last_n_n = 0;
 771 
         } else  last_n_n = 0;
766 
 
 772 
 
767 
        if(cnt > 2) cnt = 2;
 773 
        if(cnt > 3) cnt = 3;
768 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
 774 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
769 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 - 
 
770 
//        if(IntegralFehlerNick >  FEHLER_LIMIT)  { AdNeutralNick += (cnt + last_n_p/20);  if(last_n_p++ == 4) {last_n_p = 4;}} else last_n_p = 0;
 - 
 
- 
 
 775 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
771 
//        if(IntegralFehlerNick < -FEHLER_LIMIT)  { AdNeutralNick -= (cnt + last_n_n/20);  if(last_n_n++ == 4) {last_n_n = 4;}} else last_n_n = 0;
 776 
 
772 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 777 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 778 
        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
773 
        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
 779 
ausgleichRoll = 0;
774 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
 780 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
775 
         {
 781 
         {
776 
           if(last_r_p)
 782 
           if(last_r_p)
777 
           {
 783 
           {
778 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 784 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
779 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 785 
            ausgleichRoll = IntegralFehlerRoll / 4;
780 
            if(ausgleichRoll > 10000) ausgleichRoll = 10000;
 786 
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
781 
            Mess_IntegralRoll += ausgleichRoll;
 787 
            Mess_IntegralRoll -= ausgleichRoll;
782 
           }
 788 
           }
783 
           else last_r_p = 1;
 789 
           else last_r_p = 1;
784 
         } else  last_r_p = 0;
 790 
         } else  last_r_p = 0;
785 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
 791 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
786 
         {
 792 
         {
787 
           if(last_r_n)
 793 
           if(last_r_n)
788 
           {
 794 
           {
789 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 795 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
790 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 796 
            ausgleichRoll = IntegralFehlerRoll / 4;
791 
            if(ausgleichRoll < -10000) ausgleichRoll = -10000;
 797 
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
792 
            Mess_IntegralRoll += ausgleichRoll;
 798 
            Mess_IntegralRoll -= ausgleichRoll;
793 
           }
 799 
           }
794 
           else last_r_n = 1;
 800 
           else last_r_n = 1;
795 
         } else  last_r_n = 0;
 801 
         } else  last_r_n = 0;
- 
 
 802 
 
- 
 
 803 
DebugOut.Analog[27] = ausgleichRoll;
796 
 
 804 
        if(cnt > 3) cnt = 3;
797 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 805 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
798 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 - 
 
799 
        if(cnt > 2) cnt = 2;
 - 
 
800 
//        if(IntegralFehlerRoll >  FEHLER_LIMIT)  { AdNeutralRoll += (cnt + last_r_p/20);  if(last_r_p++ == 4) {beeptime =500;last_r_p = 4;}} else last_r_p = 0;
 - 
 
801 
//        if(IntegralFehlerRoll < -FEHLER_LIMIT)  { AdNeutralRoll -= (cnt + last_r_n/20);  if(last_r_n++ == 4) {beeptime = 60;last_r_n = 4;}} else last_r_n = 0;
 - 
 
802 
 
 - 
 
803 
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
 - 
 
804 
//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)      
 - 
 
805 
        cnt = 2;
 806 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
806 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - 550);
 807 
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
807 
  }
 808 
  }
808 
    IntegralAccNick = 0;
 809 
    IntegralAccNick = 0;
809 
    IntegralAccRoll = 0;
 810 
    IntegralAccRoll = 0;
810 
    IntegralAccZ = 0;
 811 
    IntegralAccZ = 0;
811 
    MittelIntegralNick = 0;
 812 
    MittelIntegralNick = 0;
812 
    MittelIntegralRoll = 0;
 813 
    MittelIntegralRoll = 0;
813 
    MittelIntegralNick2 = 0;
 814 
    MittelIntegralNick2 = 0;
814 
    MittelIntegralRoll2 = 0;
 815 
    MittelIntegralRoll2 = 0;
815 
    ZaehlMessungen = 0;
 816 
    ZaehlMessungen = 0;
816 
 }
 817 
 }
817 
 
 818 
 
818 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
819 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
819 
//  Gieren
 820 
//  Gieren
820 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
821 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
821 
    if(abs(StickGier) > 20) // war 35
822 
    if(abs(StickGier) > 20) // war 35
822 
     {
 823 
     {
823 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 824 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
824 
     }
 825 
     }
825 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 826 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
826 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 827 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
827 
    sollGier = tmp_int;
 828 
    sollGier = tmp_int;
828 
    Mess_Integral_Gier -= tmp_int;  
 829 
    Mess_Integral_Gier -= tmp_int;  
829 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
 830 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
830 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 831 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
831 
 
 832 
 
832 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
833 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
833 
//  Kompass
 834 
//  Kompass
834 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
835 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
835 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
 836 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
836 
     {
 837 
     {
837 
       int w,v;
 838 
       int w,v;
838 
       static int SignalSchlecht = 0;
 839 
       static int SignalSchlecht = 0;
839 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
 840 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
840 
       v = abs(IntegralRoll /512);
 841 
       v = abs(IntegralRoll /512);
841 
       if(v > w) w = v; // grösste Neigung ermitteln
 842 
       if(v > w) w = v; // grösste Neigung ermitteln
842 
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
 843 
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
843 
        {
 844 
        {
844 
         KompassStartwert = KompassValue;
 845 
         KompassStartwert = KompassValue;
845 
         NeueKompassRichtungMerken = 0;
 846 
         NeueKompassRichtungMerken = 0;
846 
        }
 847 
        }
847 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 848 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
848 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 849 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
849 
       if(w > 0)
 850 
       if(w > 0)
850 
        {
 851 
        {
851 
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 852 
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
852 
          if(SignalSchlecht) SignalSchlecht--;
 853 
          if(SignalSchlecht) SignalSchlecht--;
853 
        }  
 854 
        }  
854 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 855 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
855 
     }
 856 
     }
856 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
857 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
857 
 
 858 
 
858 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
859 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
859 
//  Debugwerte zuordnen
 860 
//  Debugwerte zuordnen
860 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
861 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
861 
  if(!TimerWerteausgabe--)
 862 
  if(!TimerWerteausgabe--)
862 
   {
 863 
   {
863 
    TimerWerteausgabe = 24;
 864 
    TimerWerteausgabe = 24;
864 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 865 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
865 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 866 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
866 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 867 
    DebugOut.Analog[2] = Mittelwert_AccNick;
867 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 868 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
868 
    DebugOut.Analog[4] = MesswertGier;
 869 
    DebugOut.Analog[4] = MesswertGier;
869 
    DebugOut.Analog[5] = HoehenWert;
 870 
    DebugOut.Analog[5] = HoehenWert;
870 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
 871 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
871 
    DebugOut.Analog[8] = KompassValue;
 872 
    DebugOut.Analog[8] = KompassValue;
872 
    DebugOut.Analog[9] = UBat;
 873 
    DebugOut.Analog[9] = UBat;
873 
    DebugOut.Analog[10] = SenderOkay;
 874 
    DebugOut.Analog[10] = SenderOkay;
874 
    DebugOut.Analog[16] = Mittelwert_AccHoch;
 875 
    DebugOut.Analog[16] = Mittelwert_AccHoch;
875 
 
 876 
 
876 
/*    DebugOut.Analog[16] = motor_rx[0];
 877 
/*    DebugOut.Analog[16] = motor_rx[0];
877 
    DebugOut.Analog[17] = motor_rx[1];
 878 
    DebugOut.Analog[17] = motor_rx[1];
878 
    DebugOut.Analog[18] = motor_rx[2];
 879 
    DebugOut.Analog[18] = motor_rx[2];
879 
    DebugOut.Analog[19] = motor_rx[3];
 880 
    DebugOut.Analog[19] = motor_rx[3];
880 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
 881 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
881 
    DebugOut.Analog[20] /= 14;
 882 
    DebugOut.Analog[20] /= 14;
882 
    DebugOut.Analog[21] = motor_rx[4];
 883 
    DebugOut.Analog[21] = motor_rx[4];
883 
    DebugOut.Analog[22] = motor_rx[5];
 884 
    DebugOut.Analog[22] = motor_rx[5];
884 
    DebugOut.Analog[23] = motor_rx[6];
 885 
    DebugOut.Analog[23] = motor_rx[6];
885 
    DebugOut.Analog[24] = motor_rx[7];
 886 
    DebugOut.Analog[24] = motor_rx[7];
886 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 887 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
887 
*/
 888 
*/
888 
//    DebugOut.Analog[9] = MesswertNick;
 889 
//    DebugOut.Analog[9] = MesswertNick;
889 
//    DebugOut.Analog[9] = SollHoehe;
 890 
//    DebugOut.Analog[9] = SollHoehe;
890 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 891 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
891 
//    DebugOut.Analog[11] = KompassStartwert;
 892 
//    DebugOut.Analog[11] = KompassStartwert;
892 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 893 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
893 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 894 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
894 
//    DebugOut.Analog[9] = KompassRichtung;    
 895 
//    DebugOut.Analog[9] = KompassRichtung;    
895 
//    DebugOut.Analog[10] = GasMischanteil;
 896 
//    DebugOut.Analog[10] = GasMischanteil;
896 
//    DebugOut.Analog[3] = HoeheD * 32;
 897 
//    DebugOut.Analog[3] = HoeheD * 32;
897 
//    DebugOut.Analog[4] = hoehenregler;
 898 
//    DebugOut.Analog[4] = hoehenregler;
898 
  }
 899 
  }
899 
 
 900 
 
900 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
901 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
901 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 902 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
902 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
903 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
903 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 904 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
904 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 905 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
905 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 906 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
906 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 907 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
907 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 908 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
908 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 909 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
909 
 
 910 
 
910 
    // Maximalwerte abfangen
 911 
    // Maximalwerte abfangen
911 
    #define MAX_SENSOR  2048
 912 
    #define MAX_SENSOR  2048
912 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 913 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
913 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 914 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
914 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 915 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
915 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 916 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
916 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 917 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
917 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 918 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
918 
 
 919 
 
919 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
920 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
920 
// Höhenregelung
 921 
// Höhenregelung
921 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 922 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
922 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
923 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
923 
//OCR0B = 180 - (Poti1 + 120) / 4;
 924 
//OCR0B = 180 - (Poti1 + 120) / 4;
924 
//DruckOffsetSetting = OCR0B;
 925 
//DruckOffsetSetting = OCR0B;
925 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 926 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
926 
  {
 927 
  {
927 
    int tmp_int;
 928 
    int tmp_int;
928 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 929 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
929 
    {
 930 
    {
930 
     if(Parameter_MaxHoehe < 50)
 931 
     if(Parameter_MaxHoehe < 50)
931 
      {
 932 
      {
932 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 933 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
933 
       HoehenReglerAktiv = 0;
 934 
       HoehenReglerAktiv = 0;
934 
      }
 935 
      }
935 
      else  
 936 
      else  
936 
        HoehenReglerAktiv = 1;
 937 
        HoehenReglerAktiv = 1;
937 
    }
 938 
    }
938 
    else
 939 
    else
939 
    {
 940 
    {
940 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
 941 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
941 
     HoehenReglerAktiv = 1;
 942 
     HoehenReglerAktiv = 1;
942 
    }
 943 
    }
943 
 
 944 
 
944 
    if(Notlandung) SollHoehe = 0;
 945 
    if(Notlandung) SollHoehe = 0;
945 
    h = HoehenWert;
 946 
    h = HoehenWert;
946 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 947 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
947 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 948 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
948 
      h = GasMischanteil - h;         // vom Gas abziehen
 949 
      h = GasMischanteil - h;         // vom Gas abziehen
949 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 950 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
950 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 951 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
951 
      if(tmp_int > 50) tmp_int = 50;
 952 
      if(tmp_int > 50) tmp_int = 50;
952 
      else if(tmp_int < -50) tmp_int = -50;
 953 
      else if(tmp_int < -50) tmp_int = -50;
953 
      h -= tmp_int;
 954 
      h -= tmp_int;
954 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 955 
      hoehenregler = (hoehenregler*15 + h) / 16;      
955 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 956 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
956 
       {
 957 
       {
957 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 958 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
958 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 959 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
959 
       }  
 960 
       }  
960 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 961 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
961 
      GasMischanteil = hoehenregler;
 962 
      GasMischanteil = hoehenregler;
962 
     }
 963 
     }
963 
  }
 964 
  }
964 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
965 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
965 
// + Mischer und PI-Regler
 966 
// + Mischer und PI-Regler
966 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
967 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
967 
  DebugOut.Analog[7] = GasMischanteil;
 968 
  DebugOut.Analog[7] = GasMischanteil;
968 
 
 969 
 
969 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
970 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
970 
// Gier-Anteil
 971 
// Gier-Anteil
971 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
972 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
972 
#define MUL_G  1.0
 973 
#define MUL_G  1.0
973 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 974 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
974 
//GierMischanteil = 0;
 975 
//GierMischanteil = 0;
975 
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
 976 
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
976 
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
 977 
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
977 
    if(GierMischanteil > 100)  GierMischanteil = 100;
 978 
    if(GierMischanteil > 100)  GierMischanteil = 100;
978 
    if(GierMischanteil < -100) GierMischanteil = -100;
 979 
    if(GierMischanteil < -100) GierMischanteil = -100;
979 
 
 980 
 
980 
    if(GasMischanteil < 20) GierMischanteil = 0;
 981 
    if(GasMischanteil < 20) GierMischanteil = 0;
981 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
982 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
982 
// Nick-Achse
 983 
// Nick-Achse
983 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
984 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
984 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
 985 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
985 
    SummeNick += DiffNick;                                   // I-Anteil
 986 
    SummeNick += DiffNick;                                   // I-Anteil
986 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 987 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
987 
    if(SummeNick >  16000) SummeNick =  16000;
 988 
    if(SummeNick >  16000) SummeNick =  16000;
988 
    if(SummeNick < -16000) SummeNick = -16000;
 989 
    if(SummeNick < -16000) SummeNick = -16000;
989 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
990 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
990 
    // Motor Vorn
 991 
    // Motor Vorn
991 
#define MUL  2
 992 
#define MUL  2
992 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 993 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
993 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 994 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
994 
 
 995 
 
995 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 996 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
996 
        if ((motorwert < 0)) motorwert = 0;
 997 
        if ((motorwert < 0)) motorwert = 0;
997 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 998 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
998 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 999 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
999 
        Motor_Vorne = motorwert;           
 1000 
        Motor_Vorne = motorwert;           
1000 
    // Motor Heck
 1001 
    // Motor Heck
1001 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 1002 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
1002 
        if ((motorwert < 0)) motorwert = 0;
 1003 
        if ((motorwert < 0)) motorwert = 0;
1003 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1004 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
1004 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1005 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1005 
        Motor_Hinten = motorwert;              
 1006 
        Motor_Hinten = motorwert;              
1006 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1007 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1007 
// Roll-Achse
 1008 
// Roll-Achse
1008 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1009 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1009 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
 1010 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
1010 
        SummeRoll += DiffRoll;                                                              // I-Anteil
 1011 
        SummeRoll += DiffRoll;                                                              // I-Anteil
1011 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 1012 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
1012 
    if(SummeRoll >  16000) SummeRoll =  16000;
 1013 
    if(SummeRoll >  16000) SummeRoll =  16000;
1013 
    if(SummeRoll < -16000) SummeRoll = -16000;
 1014 
    if(SummeRoll < -16000) SummeRoll = -16000;
1014 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 1015 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
1015 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 1016 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
1016 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 1017 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
1017 
    // Motor Links
 1018 
    // Motor Links
1018 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1019 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1019 
        if ((motorwert < 0)) motorwert = 0;
 1020 
        if ((motorwert < 0)) motorwert = 0;
1020 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1021 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1021 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1022 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1022 
        Motor_Links = motorwert;               
 1023 
        Motor_Links = motorwert;               
1023 
    // Motor Rechts
 1024 
    // Motor Rechts
1024 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1025 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
1025 
        if ((motorwert < 0)) motorwert = 0;
 1026 
        if ((motorwert < 0)) motorwert = 0;
1026 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1027 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1027 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1028 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1028 
        Motor_Rechts = motorwert;
 1029 
        Motor_Rechts = motorwert;
1029 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1030 
   // +++++++++++++++++++++++++++++++++++++++++++++++
1030 
}
 1031 
}
1031 
 
 1032 
 
1032 
 
 1033