Subversion Repositories FlightCtrl

Rev

Rev 143 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 143 Rev 149
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 
 
 57 
 
58 
unsigned char h,m,s;
 58 
unsigned char h,m,s;
59 
volatile unsigned char Timeout = 0;
 59 
volatile unsigned char Timeout = 0;
60 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 60 
volatile int MesswertNick,MesswertRoll,MesswertGier;
61 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 61 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
62 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 62 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
63 
volatile float NeutralAccZ = 0;
 63 
volatile float NeutralAccZ = 0;
64 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 64 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
65 
volatile long IntegralNick = 0,IntegralNick2 = 0;
 65 
volatile long IntegralNick = 0,IntegralNick2 = 0;
66 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
 66 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
67 
volatile long Integral_Gier = 0;
 67 
volatile long Integral_Gier = 0;
68 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 68 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
69 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 69 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
70 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 70 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
71 
volatile long Mess_Integral_Hoch = 0;
 71 
volatile long Mess_Integral_Hoch = 0;
72 
volatile int  KompassValue = 0;
 72 
volatile int  KompassValue = 0;
73 
volatile int  KompassStartwert = 0;
 73 
volatile int  KompassStartwert = 0;
74 
volatile int  KompassRichtung = 0;
 74 
volatile int  KompassRichtung = 0;
75 
unsigned char MAX_GAS,MIN_GAS;
 75 
unsigned char MAX_GAS,MIN_GAS;
76 
unsigned char Notlandung = 0;
 76 
unsigned char Notlandung = 0;
77 
unsigned char HoehenReglerAktiv = 0;
 77 
unsigned char HoehenReglerAktiv = 0;
78 
static int SignalSchlecht = 0;
 78 
static int SignalSchlecht = 0;
79 
 //Salvo 2.9.2007 Ersatzkompass
 79 
 //Salvo 2.9.2007 Ersatzkompass
80 
volatile long GyroKomp_Int,GyroKomp_Int2;
 80 
volatile long GyroKomp_Int,GyroKomp_Int2;
81 
volatile int GyroKomp_Inc_Grad; // Gyroincrements / Grad
 81 
volatile int GyroKomp_Inc_Grad; // Gyroincrements / Grad
82 
volatile int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
 82 
volatile int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
83 
// Salvo End
 83 
// Salvo End
84 
float GyroFaktor;
 84 
float GyroFaktor;
85 
float IntegralFaktor;
 85 
float IntegralFaktor;
86 
 
 86 
 
87 
volatile int  DiffNick,DiffRoll;
 87 
volatile int  DiffNick,DiffRoll;
88 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 88 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
89 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 89 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
90 
unsigned char MotorWert[5];
 90 
unsigned char MotorWert[5];
91 
volatile unsigned char SenderOkay = 0;
 91 
volatile unsigned char SenderOkay = 0;
92 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 92 
int StickNick = 0,StickRoll = 0,StickGier = 0;
93 
char MotorenEin = 0;
 93 
char MotorenEin = 0;
94 
int HoehenWert = 0;
 94 
int HoehenWert = 0;
95 
int SollHoehe = 0;
 95 
int SollHoehe = 0;
96 
int w,v;
 96 
int w,v;
97 
 
 97 
 
98 
float Kp =  FAKTOR_P;
 98 
float Kp =  FAKTOR_P;
99 
float Ki =  FAKTOR_I;
 99 
float Ki =  FAKTOR_I;
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 = 50;            // Wert : 10-250
 106 
unsigned char Parameter_Gyro_P = 50;            // 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 
struct mk_param_struct EE_Parameter;
 115 
struct mk_param_struct EE_Parameter;
116 
 
 116 
 
117 
void Piep(unsigned char Anzahl)
 117 
void Piep(unsigned char Anzahl)
118 
{
 118 
{
119 
 while(Anzahl--)
 119 
 while(Anzahl--)
120 
 {
 120 
 {
121 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 121 
  if(MotorenEin) return; //auf keinen Fall im Flug!
122 
  beeptime = 100;
 122 
  beeptime = 100;
123 
  Delay_ms(250);
 123 
  Delay_ms(250);
124 
 }
 124 
 }
125 
}
 125 
}
126 
 
 126 
 
127 
//############################################################################
 127 
//############################################################################
128 
//  Nullwerte ermitteln
 128 
//  Nullwerte ermitteln
129 
void SetNeutral(void)
 129 
void SetNeutral(void)
130 
//############################################################################
 130 
//############################################################################
131 
{
 131 
{
132 
    unsigned int timer;
 132 
    unsigned int timer;
133 
        NeutralAccX = 0;
 133 
        NeutralAccX = 0;
134 
        NeutralAccY = 0;
 134 
        NeutralAccY = 0;
135 
        NeutralAccZ = 0;
 135 
        NeutralAccZ = 0;
136 
    AdNeutralNick = 0; 
 136 
    AdNeutralNick = 0; 
137 
        AdNeutralRoll = 0;     
 137 
        AdNeutralRoll = 0;     
138 
        AdNeutralGier = 0;
 138 
        AdNeutralGier = 0;
139 
    CalibrierMittelwert();     
 139 
    CalibrierMittelwert();     
140 
    timer = SetDelay(5);    
 140 
    timer = SetDelay(5);    
141 
        while (!CheckDelay(timer));
 141 
        while (!CheckDelay(timer));
142 
        CalibrierMittelwert();
 142 
        CalibrierMittelwert();
143 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 143 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
144 
     {    
 144 
     {    
145 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 145 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
146 
     }
 146 
     }
147 
    AdNeutralNick= abs(MesswertNick);  
 147 
    AdNeutralNick= abs(MesswertNick);  
148 
        AdNeutralRoll= abs(MesswertRoll);      
 148 
        AdNeutralRoll= abs(MesswertRoll);      
149 
        AdNeutralGier= abs(MesswertGier);
 149 
        AdNeutralGier= abs(MesswertGier);
150 
// Salvo 1.9.2007 ACC mit festen neutralwerten *****
 150 
// Salvo 1.9.2007 ACC mit festen neutralwerten *****
151 
        if (ACC_NEUTRAL_FIXED > 0)
 151 
        if (ACC_NEUTRAL_FIXED > 0)
152 
        {
 152 
        {
153 
                NeutralAccX     = ACC_X_NEUTRAL;
 153 
                NeutralAccX     = ACC_X_NEUTRAL;
154 
                NeutralAccY     = ACC_Y_NEUTRAL;
 154 
                NeutralAccY     = ACC_Y_NEUTRAL;
155 
        }
 155 
        }
156 
        else
 156 
        else
157 
        { // Automatisch bei Offsetabgleich ermitteln
 157 
        { // Automatisch bei Offsetabgleich ermitteln
158 
        NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 158 
        NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
159 
                NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 159 
                NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
160 
        }
 160 
        }
161 
 // Salvo End  
161 
 // Salvo End  
162 
        NeutralAccZ = Aktuell_az;      
 162 
        NeutralAccZ = Aktuell_az;      
163 
        Mess_IntegralNick = 0; 
 163 
        Mess_IntegralNick = 0; 
164 
    Mess_IntegralNick2 = 0;
 164 
    Mess_IntegralNick2 = 0;
165 
    Mess_IntegralRoll = 0;     
 165 
    Mess_IntegralRoll = 0;     
166 
    Mess_IntegralRoll2 = 0;
 166 
    Mess_IntegralRoll2 = 0;
167 
    Mess_Integral_Gier = 0;    
 167 
    Mess_Integral_Gier = 0;    
168 
    MesswertNick = 0;
 168 
    MesswertNick = 0;
169 
    MesswertRoll = 0;
 169 
    MesswertRoll = 0;
170 
    MesswertGier = 0;
 170 
    MesswertGier = 0;
171 
    StartLuftdruck = Luftdruck;
 171 
    StartLuftdruck = Luftdruck;
172 
    HoeheD = 0;
 172 
    HoeheD = 0;
173 
    Mess_Integral_Hoch = 0;
 173 
    Mess_Integral_Hoch = 0;
174 
    KompassStartwert = KompassValue;
 174 
    KompassStartwert = KompassValue;
175 
    GPS_Neutral();
 175 
    GPS_Neutral();
176 
    beeptime = 50;  
 176 
    beeptime = 50;  
177 
//Salvo 2.9.2007 Ersatzkompass
 177 
//Salvo 2.9.2007 Ersatzkompass
178 
        GyroKomp_Int = 0;
 178 
        GyroKomp_Int = 0;
179 
        GyroKomp_Int2 = 0;
 179 
        GyroKomp_Int2 = 0;
180 
        GyroKomp_Inc_Grad =     GYROKOMP_INC_GRAD_DEFAULT;
 180 
        GyroKomp_Inc_Grad =     GYROKOMP_INC_GRAD_DEFAULT;
181 
// Salvo End
 181 
// Salvo End
182 
}
 182 
}
183 
 
 183 
 
184 
//############################################################################
 184 
//############################################################################
185 
// Bildet den Mittelwert aus den Messwerten
 185 
// Bildet den Mittelwert aus den Messwerten
186 
void Mittelwert(void)
 186 
void Mittelwert(void)
187 
//############################################################################
 187 
//############################################################################
188 
{      
 188 
{      
189 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 189 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
190 
    ANALOG_OFF;
 190 
    ANALOG_OFF;
191 
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 191 
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
192 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 192 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
193 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 193 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
194 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
 194 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
195 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
 195 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
196 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
 196 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
197 
    AccumulateNick = 0;   MessanzahlNick = 0;
 197 
    AccumulateNick = 0;   MessanzahlNick = 0;
198 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 198 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
199 
    AccumulateGier = 0;   MessanzahlGier = 0;
 199 
    AccumulateGier = 0;   MessanzahlGier = 0;
200 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 200 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
201 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 201 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
202 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 202 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
203 
    Integral_Gier  = Mess_Integral_Gier;
 203 
    Integral_Gier  = Mess_Integral_Gier;
204 
//    Integral_Gier2 = Mess_Integral_Gier2;
 204 
//    Integral_Gier2 = Mess_Integral_Gier2;
205 
    IntegralNick = Mess_IntegralNick;
 205 
    IntegralNick = Mess_IntegralNick;
206 
    IntegralRoll = Mess_IntegralRoll;
 206 
    IntegralRoll = Mess_IntegralRoll;
207 
    IntegralNick2 = Mess_IntegralNick2;
 207 
    IntegralNick2 = Mess_IntegralNick2;
208 
    IntegralRoll2 = Mess_IntegralRoll2;
 208 
    IntegralRoll2 = Mess_IntegralRoll2;
209 
    // ADC einschalten
 209 
    // ADC einschalten
210 
    ANALOG_ON; 
 210 
    ANALOG_ON; 
211 
 
 211 
 
212 
//------------------------------------------------------------------------------
 212 
//------------------------------------------------------------------------------
213 
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
 213 
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
214 
    else                                         
 214 
    else                                         
215 
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 215 
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
216 
 
 216 
 
217 
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
 217 
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
218 
    else                                         
 218 
    else                                         
219 
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 219 
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
220 
//------------------------------------------------------------------------------
 220 
//------------------------------------------------------------------------------
221 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 221 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
222 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 222 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
223 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 223 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
224 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 224 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
225 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 225 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
226 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 226 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
227 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 227 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
228 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 228 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
229 
}
 229 
}
230 
 
 230 
 
231 
//############################################################################
 231 
//############################################################################
232 
// Messwerte beim Ermitteln der Nullage
 232 
// Messwerte beim Ermitteln der Nullage
233 
void CalibrierMittelwert(void)
 233 
void CalibrierMittelwert(void)
234 
//############################################################################
 234 
//############################################################################
235 
{                
 235 
{                
236 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 236 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
237 
        ANALOG_OFF;
 237 
        ANALOG_OFF;
238 
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 238 
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
239 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 239 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
240 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 240 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
241 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
 241 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
242 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
 242 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
243 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
 243 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
244 
    AccumulateNick = 0;   MessanzahlNick = 0;
 244 
    AccumulateNick = 0;   MessanzahlNick = 0;
245 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 245 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
246 
    AccumulateGier = 0;   MessanzahlGier = 0;
 246 
    AccumulateGier = 0;   MessanzahlGier = 0;
247 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 247 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
248 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 248 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
249 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 249 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
250 
    // ADC einschalten
 250 
    // ADC einschalten
251 
    ANALOG_ON; 
 251 
    ANALOG_ON; 
252 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 252 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
253 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 253 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
254 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 254 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
255 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 255 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
256 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 256 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
257 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 257 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
258 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 258 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
259 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 259 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
260 
}
 260 
}
261 
 
 261 
 
262 
//############################################################################
 262 
//############################################################################
263 
// Senden der Motorwerte per I2C-Bus
 263 
// Senden der Motorwerte per I2C-Bus
264 
void SendMotorData(void)
 264 
void SendMotorData(void)
265 
//############################################################################
 265 
//############################################################################
266 
{
 266 
{
267 
    if(MOTOR_OFF || !MotorenEin)
 267 
    if(MOTOR_OFF || !MotorenEin)
268 
        {
 268 
        {
269 
        Motor_Hinten = 0;
 269 
        Motor_Hinten = 0;
270 
        Motor_Vorne = 0;
 270 
        Motor_Vorne = 0;
271 
        Motor_Rechts = 0;
 271 
        Motor_Rechts = 0;
272 
        Motor_Links = 0;
 272 
        Motor_Links = 0;
273 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 273 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
274 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 274 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
275 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 275 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
276 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 276 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
277 
        }
 277 
        }
278 
 
 278 
 
279 
    DebugOut.Analog[12] = Motor_Vorne;
 279 
    DebugOut.Analog[12] = Motor_Vorne;
280 
    DebugOut.Analog[13] = Motor_Hinten;
 280 
    DebugOut.Analog[13] = Motor_Hinten;
281 
    DebugOut.Analog[14] = Motor_Links;
 281 
    DebugOut.Analog[14] = Motor_Links;
282 
    DebugOut.Analog[15] = Motor_Rechts;  
 282 
    DebugOut.Analog[15] = Motor_Rechts;  
283 
 
 283 
 
284 
    //Start I2C Interrupt Mode
 284 
    //Start I2C Interrupt Mode
285 
    twi_state = 0;
 285 
    twi_state = 0;
286 
    motor = 0;
 286 
    motor = 0;
287 
    i2c_start();
 287 
    i2c_start();
288 
}
 288 
}
289 
 
 289 
 
290 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 290 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
291 
// + Konstanten
291 
// + Konstanten
292 
// + 0-250 -> normale Werte
 292 
// + 0-250 -> normale Werte
293 
// + 251 -> Poti1
 293 
// + 251 -> Poti1
294 
// + 252 -> Poti2
 294 
// + 252 -> Poti2
295 
// + 253 -> Poti3
 295 
// + 253 -> Poti3
296 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 296 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
297 
void DefaultKonstanten1(void)
 297 
void DefaultKonstanten1(void)
298 
{
 298 
{
299 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 299 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
300 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 300 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
301 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 301 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
302 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 302 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
303 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 303 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
304 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 304 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
305 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 305 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
306 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
 306 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
307 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
 307 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
308 
 EE_Parameter.Hoehe_MinGas = 30;
 308 
 EE_Parameter.Hoehe_MinGas = 30;
309 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
 309 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
310 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
 310 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
311 
 EE_Parameter.Luftdruck_D  = 70;      // Wert : 0-250
 311 
 EE_Parameter.Luftdruck_D  = 70;      // Wert : 0-250
312 
 EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
 312 
 EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
313 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
 313 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
314 
 EE_Parameter.Stick_P = 2; //2           // Wert : 1-6
 314 
 EE_Parameter.Stick_P = 2; //2           // Wert : 1-6
315 
 EE_Parameter.Stick_D = 4; //8           // Wert : 0-64
 315 
 EE_Parameter.Stick_D = 4; //8           // Wert : 0-64
316 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
 316 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
317 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 317 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
318 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
 318 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
319 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
 319 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
320 
 EE_Parameter.KompassWirkung = 64;    // Wert : 0-250
 320 
 EE_Parameter.KompassWirkung = 64;    // Wert : 0-250
321 
 EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
 321 
 EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
322 
 EE_Parameter.Gyro_I = 150;               // Wert : 0-250
 322 
 EE_Parameter.Gyro_I = 150;               // Wert : 0-250
323 
 EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250
 323 
 EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250
324 
 EE_Parameter.NotGas = 90;                // Wert : 0-250     // Gaswert bei Empangsverlust
 324 
 EE_Parameter.NotGas = 90;                // Wert : 0-250     // Gaswert bei Empangsverlust
325 
 EE_Parameter.NotGasZeit = 50;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 325 
 EE_Parameter.NotGasZeit = 50;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
326 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 326 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
327 
 EE_Parameter.I_Faktor = 5;
 327 
 EE_Parameter.I_Faktor = 5;
328 
 EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
 328 
 EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
329 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
 329 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
330 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
 330 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
331 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
 331 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
332 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 332 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
333 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 333 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
334 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 334 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
335 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
 335 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
336 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 336 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
337 
 EE_Parameter.ServoNickRefresh = 5;
 337 
 EE_Parameter.ServoNickRefresh = 5;
338 
 memcpy(EE_Parameter.Name, "Normal\0", 12);  
 338 
 memcpy(EE_Parameter.Name, "Normal\0", 12);  
339 
}
 339 
}
340 
 
 340 
 
341 
void DefaultKonstanten2(void)
 341 
void DefaultKonstanten2(void)
342 
{
 342 
{
343 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 343 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
344 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 344 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
345 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 345 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
346 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 346 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
347 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 347 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
348 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 348 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
349 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 349 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
350 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
 350 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
351 
 EE_Parameter.Hoehe_MinGas = 30;
 351 
 EE_Parameter.Hoehe_MinGas = 30;
352 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
 352 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
353 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
 353 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
354 
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
 354 
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
355 
 EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
 355 
 EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
356 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
 356 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
357 
 EE_Parameter.Stick_P = 2; //2           // Wert : 1-6
 357 
 EE_Parameter.Stick_P = 2; //2           // Wert : 1-6
358 
 EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
 358 
 EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
359 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
 359 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
360 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 360 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
361 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
 361 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
362 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
 362 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
363 
 EE_Parameter.KompassWirkung = 64;    // Wert : 0-250
 363 
 EE_Parameter.KompassWirkung = 64;    // Wert : 0-250
364 
 EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
 364 
 EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
365 
 EE_Parameter.Gyro_I = 175;           // Wert : 0-250
 365 
 EE_Parameter.Gyro_I = 175;           // Wert : 0-250
366 
 EE_Parameter.UnterspannungsWarnung = 100;  // Wert : 0-250
 366 
 EE_Parameter.UnterspannungsWarnung = 100;  // Wert : 0-250
367 
 EE_Parameter.NotGas = 90;                 // Wert : 0-250     // Gaswert bei Empangsverlust
 367 
 EE_Parameter.NotGas = 90;                 // Wert : 0-250     // Gaswert bei Empangsverlust
368 
 EE_Parameter.NotGasZeit = 50;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 368 
 EE_Parameter.NotGasZeit = 50;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
369 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 369 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
370 
 EE_Parameter.I_Faktor = 5;
 370 
 EE_Parameter.I_Faktor = 5;
371 
 EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
 371 
 EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
372 
 EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
 372 
 EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
373 
 EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
 373 
 EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
374 
 EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
 374 
 EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
375 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
 375 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
376 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
 376 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
377 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 377 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
378 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 378 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
379 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 379 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
380 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
 380 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
381 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 381 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
382 
 EE_Parameter.ServoNickRefresh = 5;
 382 
 EE_Parameter.ServoNickRefresh = 5;
383 
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
 383 
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
384 
}
 384 
}
385 
 
 385 
 
386 
 
 386 
 
387 
//############################################################################
 387 
//############################################################################
388 
// Trägt ggf. das Poti als Parameter ein
 388 
// Trägt ggf. das Poti als Parameter ein
389 
void ParameterZuordnung(void)
 389 
void ParameterZuordnung(void)
390 
//############################################################################
 390 
//############################################################################
391 
{
 391 
{
392 
 
 392 
 
393 
 #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;}
 393 
 #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;}
394 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 394 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
395 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 395 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
396 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 396 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
397 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 397 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
398 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 398 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
399 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 399 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
400 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 400 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
401 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 401 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
402 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 402 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
403 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 403 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
404 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 404 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
405 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 405 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
406 
 
 406 
 
407 
   unsigned char ServoNickComp;          // Wert : 0-250     // Einfluss Gyro/Servo
 407 
   unsigned char ServoNickComp;          // Wert : 0-250     // Einfluss Gyro/Servo
408 
   unsigned char ServoNickCompInvert;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 408 
   unsigned char ServoNickCompInvert;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
409 
   unsigned char ServoNickMin;           // Wert : 0-250     // Anschlag
 409 
   unsigned char ServoNickMin;           // Wert : 0-250     // Anschlag
410 
   unsigned char ServoNickMax;           // Wert : 0-250     // Anschlag
 410 
   unsigned char ServoNickMax;           // Wert : 0-250     // Anschlag
411 
 
 411 
 
412 
 
 412 
 
413 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 413 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
414 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 414 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
415 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 415 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
416 
 
 416 
 
417 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 417 
 Ki = (float) Parameter_I_Faktor * 0.0001;
418 
 MAX_GAS = EE_Parameter.Gas_Max;
 418 
 MAX_GAS = EE_Parameter.Gas_Max;
419 
 MIN_GAS = EE_Parameter.Gas_Min;
 419 
 MIN_GAS = EE_Parameter.Gas_Min;
420 
}
 420 
}
421 
 
 421 
 
422 
 
 422 
 
423 
//############################################################################
 423 
//############################################################################
424 
//
 424 
//
425 
void MotorRegler(void)
 425 
void MotorRegler(void)
426 
//############################################################################
 426 
//############################################################################
427 
{
 427 
{
428 
         int motorwert,pd_ergebnis,h,tmp_int;
 428 
         int motorwert,pd_ergebnis,h,tmp_int;
429 
         int GierMischanteil,GasMischanteil;
 429 
         int GierMischanteil,GasMischanteil;
430 
     static long SummeNick=0,SummeRoll=0;
 430 
     static long SummeNick=0,SummeRoll=0;
431 
     static long sollGier = 0,tmp_long,tmp_long2;
 431 
     static long sollGier = 0,tmp_long,tmp_long2;
432 
     static int IntegralFehlerNick = 0;
 432 
     static int IntegralFehlerNick = 0;
433 
     static int IntegralFehlerRoll = 0;
 433 
     static int IntegralFehlerRoll = 0;
434 
         static unsigned int RcLostTimer;
 434 
         static unsigned int RcLostTimer;
435 
         static unsigned char delay_neutral = 0;
 435 
         static unsigned char delay_neutral = 0;
436 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 436 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
437 
         static unsigned int  modell_fliegt = 0;
 437 
         static unsigned int  modell_fliegt = 0;
438 
     static int hoehenregler = 0;
 438 
     static int hoehenregler = 0;
439 
     static char TimerWerteausgabe = 0;
 439 
     static char TimerWerteausgabe = 0;
440 
     static char NeueKompassRichtungMerken = 0;
 440 
     static char NeueKompassRichtungMerken = 0;
441 
        Mittelwert();
 441 
        Mittelwert();
442 
//******PROVISORISCH***************
 442 
//******PROVISORISCH***************
443 
        Get_GPS_data();
 443 
        Get_GPS_data();
444 
        if (gps_act_position.status >  0)    
 444 
        if (gps_act_position.status >  0)    
445 
        {
 445 
        {
446 
                ROT_ON;
 446 
                ROT_ON;
447 
                gps_act_position.status         = 0;
 447 
                gps_act_position.status         = 0;
448 
//              ROT_ON;
 - 
 
449 
        }
 448 
        }
450 
 
 449 
 
451 
//******PROVISORISCH***************
 450 
//******PROVISORISCH***************
452 
    GRN_ON;
 451 
    GRN_ON;
453 
 
 452 
 
454 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
453 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
455 
// Gaswert ermitteln
 454 
// Gaswert ermitteln
456 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
455 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
457 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 456 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
458 
    if(GasMischanteil < 0) GasMischanteil = 0;
 457 
    if(GasMischanteil < 0) GasMischanteil = 0;
459 
   
 458 
   
460 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
459 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
461 
// Emfang schlecht
 460 
// Emfang schlecht
462 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
461 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
463 
   if(SenderOkay < 100)
 462 
   if(SenderOkay < 100)
464 
        {
 463 
        {
465 
        if(!PcZugriff)  beeptime = 500;
 464 
        if(!PcZugriff)  beeptime = 500;
466 
        if(RcLostTimer) RcLostTimer--;
 465 
        if(RcLostTimer) RcLostTimer--;
467 
        else
 466 
        else
468 
         {
 467 
         {
469 
          MotorenEin = 0;
 468 
          MotorenEin = 0;
470 
          Notlandung = 0;
 469 
          Notlandung = 0;
471 
         }
 470 
         }
472 
        ROT_ON;
 471 
        ROT_ON;
473 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 472 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
474 
            {
 473 
            {
475 
            GasMischanteil = EE_Parameter.NotGas;
 474 
            GasMischanteil = EE_Parameter.NotGas;
476 
            Notlandung = 1;
 475 
            Notlandung = 1;
477 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 476 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
478 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 477 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
479 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 478 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
480 
/*          Poti1 = 65;
 479 
/*          Poti1 = 65;
481 
            Poti2 = 48;
 480 
            Poti2 = 48;
482 
            Poti3 = 0;
 481 
            Poti3 = 0;
483 
*/          }
 482 
*/          }
484 
         else MotorenEin = 0;
 483 
         else MotorenEin = 0;
485 
        }
 484 
        }
486 
        else
 485 
        else
487 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
486 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
488 
// Emfang gut
 487 
// Emfang gut
489 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
488 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
490 
        if(SenderOkay > 140)
 489 
        if(SenderOkay > 140)
491 
            {
 490 
            {
492 
            Notlandung = 0;
 491 
            Notlandung = 0;
493 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 492 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
494 
            if(GasMischanteil > 40)
 493 
            if(GasMischanteil > 40)
495 
                {
 494 
                {
496 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 495 
                if(modell_fliegt < 0xffff) modell_fliegt++;
497 
                }
 496 
                }
498 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 497 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
499 
                {
 498 
                {
500 
                SummeNick = 0;
 499 
                SummeNick = 0;
501 
                SummeRoll = 0;
 500 
                SummeRoll = 0;
502 
                Mess_Integral_Gier = 0;
 501 
                Mess_Integral_Gier = 0;
503 
                Mess_Integral_Gier2 = 0;
 502 
                Mess_Integral_Gier2 = 0;
504 
                }
 503 
                }
505 
            if((GasMischanteil > 200) && MotorenEin == 0)
 504 
            if((GasMischanteil > 200) && MotorenEin == 0)
506 
                {
 505 
                {
507 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
506 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
508 
// auf Nullwerte kalibrieren
 507 
// auf Nullwerte kalibrieren
509 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
508 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
510 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 509 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
511 
                    {
 510 
                    {
512 
                    unsigned char setting;
 511 
                    unsigned char setting;
513 
                    if(++delay_neutral > 200)  // nicht sofort
 512 
                    if(++delay_neutral > 200)  // nicht sofort
514 
                        {
 513 
                        {
515 
                        GRN_OFF;
 514 
                        GRN_OFF;
516 
                        SetNeutral();
 515 
                        SetNeutral();
517 
                        MotorenEin = 0;
 516 
                        MotorenEin = 0;
518 
                        delay_neutral = 0;
 517 
                        delay_neutral = 0;
519 
                        modell_fliegt = 0;
 518 
                        modell_fliegt = 0;
520 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 519 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
521 
                        {
 520 
                        {
522 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 521 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
523 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 522 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
524 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 523 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
525 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 524 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
526 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 525 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
527 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 526 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
528 
                        }
 527 
                        }
529 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 528 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
530 
                        Piep(GetActiveParamSetNumber());
 529 
                        Piep(GetActiveParamSetNumber());
531 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 530 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
532 
                          {
 531 
                          {
533 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 532 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
534 
                          }  
 533 
                          }  
535 
                        }
 534 
                        }
536 
                    }
 535 
                    }
537 
                 else delay_neutral = 0;
 536 
                 else delay_neutral = 0;
538 
                }
 537 
                }
539 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
538 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
540 
// Gas ist unten
 539 
// Gas ist unten
541 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
540 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
542 
            if(GasMischanteil < 35)
 541 
            if(GasMischanteil < 35)
543 
                {
 542 
                {
544 
                // Starten
 543 
                // Starten
545 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 544 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
546 
                    {
 545 
                    {
547 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
546 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
548 
// Einschalten
 547 
// Einschalten
549 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
548 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
550 
                    if(++delay_einschalten > 200)
 549 
                    if(++delay_einschalten > 200)
551 
                        {
 550 
                        {
552 
                        delay_einschalten = 200;
 551 
                        delay_einschalten = 200;
553 
                        modell_fliegt = 1;
 552 
                        modell_fliegt = 1;
554 
                        MotorenEin = 1;
 553 
                        MotorenEin = 1;
555 
                        sollGier = 0;
 554 
                        sollGier = 0;
556 
                        Mess_Integral_Gier = 0;
 555 
                        Mess_Integral_Gier = 0;
557 
                        Mess_Integral_Gier2 = 0;
 556 
                        Mess_Integral_Gier2 = 0;
558 
                        Mess_IntegralNick = 0;
 557 
                        Mess_IntegralNick = 0;
559 
                        Mess_IntegralRoll = 0;
 558 
                        Mess_IntegralRoll = 0;
560 
                        Mess_IntegralNick2 = IntegralNick;
 559 
                        Mess_IntegralNick2 = IntegralNick;
561 
                        Mess_IntegralRoll2 = IntegralRoll;
 560 
                        Mess_IntegralRoll2 = IntegralRoll;
562 
                        SummeNick = 0;
 561 
                        SummeNick = 0;
563 
                        SummeRoll = 0;
 562 
                        SummeRoll = 0;
564 
                        }          
 563 
                        }          
565 
                    }  
 564 
                    }  
566 
                    else delay_einschalten = 0;
 565 
                    else delay_einschalten = 0;
567 
                //Auf Neutralwerte setzen
 566 
                //Auf Neutralwerte setzen
568 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
567 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
569 
// Auschalten
 568 
// Auschalten
570 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
569 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
571 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 570 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
572 
                    {
 571 
                    {
573 
                    if(++delay_ausschalten > 200)  // nicht sofort
 572 
                    if(++delay_ausschalten > 200)  // nicht sofort
574 
                        {
 573 
                        {
575 
                        MotorenEin = 0;
 574 
                        MotorenEin = 0;
576 
                        delay_ausschalten = 200;
 575 
                        delay_ausschalten = 200;
577 
                        modell_fliegt = 0;
 576 
                        modell_fliegt = 0;
578 
                        }
 577 
                        }
579 
                    }
 578 
                    }
580 
                else delay_ausschalten = 0;
 579 
                else delay_ausschalten = 0;
581 
                }
 580 
                }
582 
            }
 581 
            }
583 
 
 582 
 
584 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
583 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
585 
// neue Werte von der Funke
 584 
// neue Werte von der Funke
586 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
585 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
587 
 if(!NewPpmData-- || Notlandung)  
 586 
 if(!NewPpmData-- || Notlandung)  
588 
  {
 587 
  {
589 
    ParameterZuordnung();
 588 
    ParameterZuordnung();
590 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 589 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
591 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 590 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
592 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 591 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
593 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 592 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
594 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 593 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
595 
 
 594 
 
596 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 595 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
597 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 596 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
598 
 
 597 
 
599 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 598 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
600 
    if(GyroFaktor < 0) GyroFaktor = 0;
 599 
    if(GyroFaktor < 0) GyroFaktor = 0;
601 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 600 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
602 
  }
 601 
  }
603 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
602 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
604 
// Bei Empfangsausfall im Flug
603 
// Bei Empfangsausfall im Flug
605 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
604 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
606 
   if(Notlandung)
 605 
   if(Notlandung)
607 
    {
 606 
    {
608 
     StickGier = 0;
 607 
     StickGier = 0;
609 
     StickNick = 0;
 608 
     StickNick = 0;
610 
     StickRoll = 0;
 609 
     StickRoll = 0;
611 
     GyroFaktor  = 0.1;
 610 
     GyroFaktor  = 0.1;
612 
     IntegralFaktor = 0.005;
 611 
     IntegralFaktor = 0.005;
613 
    }  
 612 
    }  
614 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
613 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
615 
// Gyro-Drift kompensieren
 614 
// Gyro-Drift kompensieren
616 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
615 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
617 
#define DRIFT_FAKTOR 3
 616 
#define DRIFT_FAKTOR 3
618 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
 617 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
619 
                {
 618 
                {
620 
// Salvo 8.9.2007 Ersatzkompass *******
 619 
// Salvo 8.9.2007 Ersatzkompass *******
621 
                    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
620 
                    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
622 
                        if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 )
 621 
                        if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 )
623 
                        {
 622 
                        {
624 
//                              GyroKomp_Int = GyroKomp_Int- (long)(GYROKOMP_INC_GRAD_DEFAULT *360);
 623 
//                              GyroKomp_Int = GyroKomp_Int- (long)(GYROKOMP_INC_GRAD_DEFAULT *360);
625 
                                GyroKomp_Int = 0;
 624 
                                GyroKomp_Int = 0;
626 
                        }
 625 
                        }
627 
                        ANALOG_ON;      // ADC einschalten
 626 
                        ANALOG_ON;      // ADC einschalten
628 
                        ROT_OFF;
 627 
                        ROT_OFF;
629 
// Salvo End
 628 
// Salvo End
630 
 
 629 
 
631 
               IntegralFehlerNick = IntegralNick2 - IntegralNick;
 630 
               IntegralFehlerNick = IntegralNick2 - IntegralNick;
632 
           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
 631 
           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
633 
           ZaehlMessungen = 0;
 632 
           ZaehlMessungen = 0;
634 
// Salvo 1.9.2007 *************************
 633 
// Salvo 1.9.2007 *************************
635 
// Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen
 634 
// Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen
636 
                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 635 
                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
637 
                        w = (abs(Mittelwert_AccNick));
 636 
                        w = (abs(Mittelwert_AccNick));
638 
                        v = (abs(Mittelwert_AccRoll));
 637 
                        v = (abs(Mittelwert_AccRoll));
639 
                ANALOG_ON;      // ADC einschalten
 638 
                ANALOG_ON;      // ADC einschalten
640 
                        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 639 
                        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
641 
                        {              
 640 
                        {              
642 
                 if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
 641 
                 if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
643 
                 if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
 642 
                 if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
644 
                 if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
 643 
                 if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
645 
                 if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
 644 
                 if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
646 
                        }
 645 
                        }
647 
                        else if ((w  < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 646 
                        else if ((w  < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
648 
                        {
 647 
                        {
649 
                 if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
 648 
                 if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
650 
                 if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
 649 
                 if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
651 
                 if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
 650 
                 if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
652 
                 if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
 651 
                 if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
653 
                        }
 652 
                        }
654 
                        else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 653 
                        else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
655 
                        {
 654 
                        {
656 
                 if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
 655 
                 if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
657 
                 if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
 656 
                 if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
658 
                 if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
 657 
                 if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
659 
                 if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
 658 
                 if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
660 
                        }
 659 
                        }
661 
// Salvo End
 660 
// Salvo End
662 
 
 661 
 
663 
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist ***********************
 662 
// Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist ***********************
664 
// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert
 663 
// Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert
665 
// Ohne Driftkompensation ist die Gierachse wesentlich stabiler
 664 
// Ohne Driftkompensation ist die Gierachse wesentlich stabiler
666 
                        if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
 665 
                        if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht))
667 
                        {
 666 
                        {
668 
                 if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;        
 667 
                 if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;        
669 
                 if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;  
 668 
                 if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;  
670 
                        }  
 669 
                        }  
671 
                        else
 670 
                        else
672 
                        {
 671 
                        {
673 
                         Mess_Integral_Gier2 = 0;      
 672 
                         Mess_Integral_Gier2 = 0;      
674 
                        }
 673 
                        }
675 
// Salvo End ***********************
 674 
// Salvo End ***********************
676 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 675 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
677 
                Mess_IntegralNick2 = IntegralNick;
 676 
                Mess_IntegralNick2 = IntegralNick;
678 
                Mess_IntegralRoll2 = IntegralRoll;
 677 
                Mess_IntegralRoll2 = IntegralRoll;
679 
                Mess_Integral_Gier2 = Integral_Gier;
 678 
                Mess_Integral_Gier2 = Integral_Gier;
680 
                ANALOG_ON;      // ADC einschalten
 679 
                ANALOG_ON;      // ADC einschalten
681 
        }
 680 
        }
682 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
681 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
683 
// Integrale auf ACC-Signal abgleichen
 682 
// Integrale auf ACC-Signal abgleichen
684 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
683 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
685 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
 684 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
686 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
 685 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
687 
#define AUSGLEICH 500
 686 
#define AUSGLEICH 500
688 
        if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 687 
        if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
689 
        if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 688 
        if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
690 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 689 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
691 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 690 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
692 
 
 691 
 
693 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 692 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
694 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
 693 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
695 
        w = (abs(Mittelwert_AccNick));
 694 
        w = (abs(Mittelwert_AccNick));
696 
        v = (abs(Mittelwert_AccRoll));
 695 
        v = (abs(Mittelwert_AccRoll));
697 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 696 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
698 
        {
 697 
        {
699 
        Mess_IntegralNick -= tmp_long;
 698 
        Mess_IntegralNick -= tmp_long;
700 
        Mess_IntegralRoll -= tmp_long2;
 699 
        Mess_IntegralRoll -= tmp_long2;
701 
        }
 700 
        }
702 
        else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
 701 
        else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
703 
        {
 702 
        {
704 
        Mess_IntegralNick -= tmp_long/2; //Vorher 8
 703 
        Mess_IntegralNick -= tmp_long/2; //Vorher 8
705 
        Mess_IntegralRoll -= tmp_long2/2;
 704 
        Mess_IntegralRoll -= tmp_long2/2;
706 
        }
 705 
        }
707 
        else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
 706 
        else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
708 
        {
 707 
        {
709 
        Mess_IntegralNick -= tmp_long/4;
 708 
        Mess_IntegralNick -= tmp_long/4;
710 
        Mess_IntegralRoll -= tmp_long2/4;
 709 
        Mess_IntegralRoll -= tmp_long2/4;
711 
        }
 710 
        }
712 
        else
 711 
        else
713 
        {
 712 
        {
714 
        Mess_IntegralNick -= tmp_long/8;
 713 
        Mess_IntegralNick -= tmp_long/8;
715 
        Mess_IntegralRoll -= tmp_long2/8;              
 714 
        Mess_IntegralRoll -= tmp_long2/8;              
716 
        }
 715 
        }
717 
// Salvo End ***********************
 716 
// Salvo End ***********************
718 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
717 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
719 
//  Gieren
 718 
//  Gieren
720 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
719 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
721 
    sollGier = StickGier;
 720 
    sollGier = StickGier;
722 
    if(abs(StickGier) > 35)
 721 
    if(abs(StickGier) > 35)
723 
     {
 722 
     {
724 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 723 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
725 
     }
 724 
     }
726 
    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
 725 
    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
727 
    Mess_Integral_Gier -= tmp_int;  
 726 
    Mess_Integral_Gier -= tmp_int;  
728 
    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
 727 
    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
729 
    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
 728 
    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
730 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
 729 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
731 
    Mess_Integral_Gier2  -= tmp_int;  
 730 
    Mess_Integral_Gier2  -= tmp_int;  
732 
// Salvo End *************************
 731 
// Salvo End *************************
733 
 ANALOG_ON;     // ADC einschalten
 732 
 ANALOG_ON;     // ADC einschalten
734 
 
 733 
 
735 
// Salvo Ersatzkompass  8.9.2007 **********************
 734 
// Salvo Ersatzkompass  8.9.2007 **********************
736 
        if (Kompass_Neuer_Wert > 0)
 735 
        if (Kompass_Neuer_Wert > 0)
737 
        {
 736 
        {
738 
           w = (abs(Mittelwert_AccNick));
 737 
           w = (abs(Mittelwert_AccNick));
739 
           v = (abs(Mittelwert_AccRoll));
 738 
           v = (abs(Mittelwert_AccRoll));
740 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
 739 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
741 
           {
 740 
           {
742 
                if  ((abs(KompassValue - Kompass_Value_Old)) < 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
 741 
                if  ((abs(KompassValue - Kompass_Value_Old)) < 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
743 
                 {
 742 
                 {
744 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern     
743 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern     
745 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
 744 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
746 
                  w = KompassValue - GyroKomp_Int;
 745 
                  w = KompassValue - GyroKomp_Int;
747 
                  if ((w > 0) && (w < 180))
 746 
                  if ((w > 0) && (w < 180))
748 
                  {
 747 
                  {
749 
                   ++GyroKomp_Int;
 748 
                   ++GyroKomp_Int;
750 
                  }
 749 
                  }
751 
                  else if ((w > 0) && (w >= 180))
 750 
                  else if ((w > 0) && (w >= 180))
752 
                  {
 751 
                  {
753 
                   --GyroKomp_Int;
 752 
                   --GyroKomp_Int;
754 
                  }
 753 
                  }
755 
                  else if ((w < 0) && (w >= -180))
 754 
                  else if ((w < 0) && (w >= -180))
756 
                  {
 755 
                  {
757 
                   --GyroKomp_Int;
 756 
                   --GyroKomp_Int;
758 
                  }
 757 
                  }
759 
                  else if ((w < 0) && (w < -180))
 758 
                  else if ((w < 0) && (w < -180))
760 
                  {
 759 
                  {
761 
                   ++GyroKomp_Int;
 760 
                   ++GyroKomp_Int;
762 
                  }
 761 
                  }
763 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
 762 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
764 
 
 763 
 
765 
                   GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
 764 
                   GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
766 
                  ANALOG_ON;    // ADC einschalten
 765 
                  ANALOG_ON;    // ADC einschalten
767 
                 }
 766 
                 }
768 
           }
 767 
           }
769 
           Kompass_Neuer_Wert = 0;
 768 
           Kompass_Neuer_Wert = 0;
770 
        }
 769 
        }
771 
 
 770 
 
772 
// Salvo End *************************
 771 
// Salvo End *************************
773 
 
 772 
 
774 
 
 773 
 
775 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
774 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
776 
//  Kompass
 775 
//  Kompass
777 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
776 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
778 
 //KompassValue = 12;
 777 
 //KompassValue = 12;
779 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
 778 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
780 
     {
 779 
     {
781 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
 780 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
782 
       v = abs(IntegralRoll /512);
 781 
       v = abs(IntegralRoll /512);
783 
       if(v > w) w = v; // grösste Neigung ermitteln
 782 
       if(v > w) w = v; // grösste Neigung ermitteln
784 
       if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht)    
 783 
       if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht)    
785 
        {
 784 
        {
786 
         KompassStartwert = KompassValue;
 785 
         KompassStartwert = KompassValue;
787 
         NeueKompassRichtungMerken = 0;
 786 
         NeueKompassRichtungMerken = 0;
788 
        }
 787 
        }
789 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 788 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
790 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 789 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
791 
       if(w > 0)
 790 
       if(w > 0)
792 
        {
 791 
        {
793 
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 792 
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
794 
 
 793 
 
795 
// Salvo 30.8.2007 Winkelbegrenzung **********************
 794 
// Salvo 30.8.2007 Winkelbegrenzung **********************
796 
         if ((!SignalSchlecht) )
 795 
         if ((!SignalSchlecht) )
797 
                 {
 796 
                 {
798 
                        if (abs(KompassRichtung) < 135 )
 797 
                        if (abs(KompassRichtung) < 135 )
799 
                        {
 798 
                        {
800 
                                Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 799 
                                Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
801 
                        }
 800 
                        }
802 
                 }
 801 
                 }
803 
 // Salvo End *************************
 802 
 // Salvo End *************************
804 
 
 803 
 
805 
          ANALOG_ON;  // ADC einschalten
 804 
          ANALOG_ON;  // ADC einschalten
806 
          if(SignalSchlecht) SignalSchlecht--;
 805 
          if(SignalSchlecht) SignalSchlecht--;
807 
        }  
 806 
        }  
808 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 807 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
809 
     }
 808 
     }
810 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
809 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
811 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
810 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
812 
 
 811 
 
813 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
812 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
814 
//  Debugwerte zuordnen
 813 
//  Debugwerte zuordnen
815 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
814 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
816 
DebugOut.Sekunden++;
 815 
DebugOut.Sekunden++;
817 
  if(!TimerWerteausgabe--)
 816 
  if(!TimerWerteausgabe--)
818 
   {
 817 
   {
819 
    TimerWerteausgabe = 49;
 818 
    TimerWerteausgabe = 49;
820 
//    DebugOut.Analog[0] = MesswertNick;
 819 
//    DebugOut.Analog[0] = MesswertNick;
821 
//    DebugOut.Analog[1] = MesswertRoll;
 820 
//    DebugOut.Analog[1] = MesswertRoll;
822 
//    DebugOut.Analog[2] = MesswertGier;
 821 
//    DebugOut.Analog[2] = MesswertGier;
823 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 822 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
824 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 823 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
825 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 824 
    DebugOut.Analog[2] = Mittelwert_AccNick;
826 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 825 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
827 
    DebugOut.Analog[4] = MesswertGier;
 826 
    DebugOut.Analog[4] = MesswertGier;
828 
    DebugOut.Analog[5] = HoehenWert;
 827 
    DebugOut.Analog[5] = HoehenWert;
829 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 828 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
830 
    DebugOut.Analog[7] = GasMischanteil;
 829 
    DebugOut.Analog[7] = GasMischanteil;
831 
    DebugOut.Analog[8] = KompassValue;
 830 
    DebugOut.Analog[8] = KompassValue;
832 
// ******provisorisch
 831 
// ******provisorisch
833 
    DebugOut.Analog[9]  = cnt1;
 832 
    DebugOut.Analog[9]  = cnt1;
834 
//    DebugOut.Analog[10] = cnt1;
 833 
//    DebugOut.Analog[10] = cnt1;
835 
//      DebugOut.Analog[11] = cnt2;
 834 
//      DebugOut.Analog[11] = cnt2;
836 
    DebugOut.Analog[10]  = (gps_act_position.utm_east/10) % 10000;     
 835 
    DebugOut.Analog[10]  = (gps_act_position.utm_east/10) % 10000;     
837 
    DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
 836 
    DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
838 
// ******provisorisch
 837 
// ******provisorisch
839 
               
 838 
               
840 
 /*  
 839 
 /*  
841 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 840 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
842 
    DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT;
 841 
    DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT;
843 
        DebugOut.Analog[11] = GyroKomp_Inc_Grad;
 842 
        DebugOut.Analog[11] = GyroKomp_Inc_Grad;
844 
        DebugOut.Analog[12] = GyroKomp_Value;
 843 
        DebugOut.Analog[12] = GyroKomp_Value;
845 
*/
 844 
*/
846 
//    DebugOut.Analog[9] = SollHoehe;
 845 
//    DebugOut.Analog[9] = SollHoehe;
847 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 846 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
848 
//    DebugOut.Analog[11] = KompassStartwert;
 847 
//    DebugOut.Analog[11] = KompassStartwert;
849 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 848 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
850 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 849 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
851 
//    DebugOut.Analog[9] = KompassRichtung;    
 850 
//    DebugOut.Analog[9] = KompassRichtung;    
852 
//    DebugOut.Analog[10] = GasMischanteil;
 851 
//    DebugOut.Analog[10] = GasMischanteil;
853 
//    DebugOut.Analog[3] = HoeheD * 32;
 852 
//    DebugOut.Analog[3] = HoeheD * 32;
854 
//    DebugOut.Analog[4] = hoehenregler;
 853 
//    DebugOut.Analog[4] = hoehenregler;
855 
  }
 854 
  }
856 
 
 855 
 
857 
 
 856 
 
858 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
857 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
859 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 858 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
860 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
859 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
861 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 860 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
862 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 861 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
863 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 862 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
864 
 
 863 
 
865 
    // Maximalwerte abfangen
 864 
    // Maximalwerte abfangen
866 
    #define MAX_SENSOR  2048
 865 
    #define MAX_SENSOR  2048
867 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 866 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
868 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 867 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
869 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 868 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
870 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 869 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
871 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 870 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
872 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 871 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
873 
 
 872 
 
874 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
873 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
875 
// Höhenregelung
 874 
// Höhenregelung
876 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 875 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
877 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
876 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
878 
//OCR0B = 180 - (Poti1 + 120) / 4;
 877 
//OCR0B = 180 - (Poti1 + 120) / 4;
879 
//DruckOffsetSetting = OCR0B;
 878 
//DruckOffsetSetting = OCR0B;
880 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 879 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
881 
  {
 880 
  {
882 
    int tmp_int;
 881 
    int tmp_int;
883 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 882 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
884 
    {
 883 
    {
885 
     if(Parameter_MaxHoehe < 50)
 884 
     if(Parameter_MaxHoehe < 50)
886 
      {
 885 
      {
887 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 886 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
888 
       HoehenReglerAktiv = 0;
 887 
       HoehenReglerAktiv = 0;
889 
      }
 888 
      }
890 
      else  
 889 
      else  
891 
        HoehenReglerAktiv = 1;
 890 
        HoehenReglerAktiv = 1;
892 
    }
 891 
    }
893 
    else
 892 
    else
894 
    {
 893 
    {
895 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
 894 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
896 
     HoehenReglerAktiv = 1;
 895 
     HoehenReglerAktiv = 1;
897 
    }
 896 
    }
898 
 
 897 
 
899 
    if(Notlandung) SollHoehe = 0;
 898 
    if(Notlandung) SollHoehe = 0;
900 
    h = HoehenWert;
 899 
    h = HoehenWert;
901 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 900 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
902 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 901 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
903 
      h = GasMischanteil - h;         // vom Gas abziehen
 902 
      h = GasMischanteil - h;         // vom Gas abziehen
904 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 903 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
905 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 904 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
906 
      if(tmp_int > 50) tmp_int = 50;
 905 
      if(tmp_int > 50) tmp_int = 50;
907 
      else if(tmp_int < -50) tmp_int = -50;
 906 
      else if(tmp_int < -50) tmp_int = -50;
908 
      h -= tmp_int;
 907 
      h -= tmp_int;
909 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 908 
      hoehenregler = (hoehenregler*15 + h) / 16;      
910 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 909 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
911 
       {
 910 
       {
912 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 911 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
913 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 912 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
914 
       }  
 913 
       }  
915 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 914 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
916 
      GasMischanteil = hoehenregler;
 915 
      GasMischanteil = hoehenregler;
917 
     }
 916 
     }
918 
  }
 917 
  }
919 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
918 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
920 
// + Mischer und PI-Regler
 919 
// + Mischer und PI-Regler
921 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
920 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
922 
 
 921 
 
923 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
922 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
924 
// Gier-Anteil
 923 
// Gier-Anteil
925 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
924 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
926 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 925 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
927 
    if(GierMischanteil > 100)  GierMischanteil = 100;
 926 
    if(GierMischanteil > 100)  GierMischanteil = 100;
928 
    if(GierMischanteil < -100) GierMischanteil = -100;
 927 
    if(GierMischanteil < -100) GierMischanteil = -100;
929 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
928 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
930 
// Nick-Achse
 929 
// Nick-Achse
931 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
930 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
932 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
 931 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
933 
    SummeNick += DiffNick;                                   // I-Anteil
 932 
    SummeNick += DiffNick;                                   // I-Anteil
934 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 933 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
935 
    if(SummeNick >  16000) SummeNick =  16000;
 934 
    if(SummeNick >  16000) SummeNick =  16000;
936 
    if(SummeNick < -16000) SummeNick = -16000;
 935 
    if(SummeNick < -16000) SummeNick = -16000;
937 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
936 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
938 
    // Motor Vorn
 937 
    // Motor Vorn
939 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 938 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
940 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 939 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
941 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 940 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
942 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 941 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
943 
        Motor_Vorne = motorwert;           
 942 
        Motor_Vorne = motorwert;           
944 
    // Motor Heck
 943 
    // Motor Heck
945 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 944 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
946 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 945 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
947 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 946 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
948 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 947 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
949 
        Motor_Hinten = motorwert;              
 948 
        Motor_Hinten = motorwert;              
950 
 
 949 
 
951 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
950 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
952 
// Roll-Achse
 951 
// Roll-Achse
953 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
952 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
954 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
 953 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
955 
        SummeRoll += DiffRoll;                                                              // I-Anteil
 954 
        SummeRoll += DiffRoll;                                                              // I-Anteil
956 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 955 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
957 
    if(SummeRoll >  16000) SummeRoll =  16000;
 956 
    if(SummeRoll >  16000) SummeRoll =  16000;
958 
    if(SummeRoll < -16000) SummeRoll = -16000;
 957 
    if(SummeRoll < -16000) SummeRoll = -16000;
959 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 958 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
960 
    // Motor Links
 959 
    // Motor Links
961 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 960 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
962 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 961 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
963 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 962 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
964 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 963 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
965 
        Motor_Links = motorwert;               
 964 
        Motor_Links = motorwert;               
966 
    // Motor Rechts
 965 
    // Motor Rechts
967 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 966 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
968 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 967 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
969 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 968 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
970 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 969 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
971 
        Motor_Rechts = motorwert;
 970 
        Motor_Rechts = motorwert;
972 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 971 
   // +++++++++++++++++++++++++++++++++++++++++++++++
973 
 
 972 
 
974 
}
 973 
}
975 
 
 974 
 
976 
 
 975