Subversion Repositories FlightCtrl

Rev

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

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