Subversion Repositories FlightCtrl

Rev

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

Rev 184 Rev 190
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 = 15;            // 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, derzeit P-Anteil GPS
332 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung, derzeit I-Anteil GPS
 332 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung, derzeit I-Anteil GPS
333 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung, derzeit P-Anteil GPS
 333 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung, derzeit D-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 = 15;            // 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 
        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
446 
        n = Get_Rel_Position();
 447 
        n = Get_Rel_Position();
447 
        if (n == 0)    
 448 
        if (n == 0)    
448 
        {
 449 
        {
449 
                ROT_ON; //led blitzen lassen
 450 
                ROT_ON; //led blitzen lassen
450 
        }
 451 
        }
451 
//******PROVISORISCH***************
 452 
//******PROVISORISCH***************
452 
    GRN_ON;
 453 
    GRN_ON;
453 
 
 454 
 
454 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
455 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
455 
// Gaswert ermitteln
 456 
// Gaswert ermitteln
456 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
457 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
457 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 458 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
458 
    if(GasMischanteil < 0) GasMischanteil = 0;
 459 
    if(GasMischanteil < 0) GasMischanteil = 0;
459 
   
 460 
   
460 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
461 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
461 
// Emfang schlecht
 462 
// Emfang schlecht
462 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
463 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
463 
   if(SenderOkay < 100)
 464 
   if(SenderOkay < 100)
464 
        {
 465 
        {
465 
        if(!PcZugriff)  beeptime = 500;
 466 
        if(!PcZugriff)  beeptime = 500;
466 
        if(RcLostTimer) RcLostTimer--;
 467 
        if(RcLostTimer) RcLostTimer--;
467 
        else
 468 
        else
468 
         {
 469 
         {
469 
          MotorenEin = 0;
 470 
          MotorenEin = 0;
470 
          Notlandung = 0;
 471 
          Notlandung = 0;
471 
         }
 472 
         }
472 
        ROT_ON;
 473 
        ROT_ON;
473 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 474 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
474 
            {
 475 
            {
475 
            GasMischanteil = EE_Parameter.NotGas;
 476 
            GasMischanteil = EE_Parameter.NotGas;
476 
            Notlandung = 1;
 477 
            Notlandung = 1;
477 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 478 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
478 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 479 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
479 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 480 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
480 
/*          Poti1 = 65;
 481 
/*          Poti1 = 65;
481 
            Poti2 = 48;
 482 
            Poti2 = 48;
482 
            Poti3 = 0;
 483 
            Poti3 = 0;
483 
*/          }
 484 
*/          }
484 
         else MotorenEin = 0;
 485 
         else MotorenEin = 0;
485 
        }
 486 
        }
486 
        else
 487 
        else
487 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
488 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
488 
// Emfang gut
 489 
// Emfang gut
489 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
490 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
490 
        if(SenderOkay > 140)
 491 
        if(SenderOkay > 140)
491 
            {
 492 
            {
492 
            Notlandung = 0;
 493 
            Notlandung = 0;
493 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 494 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
494 
            if(GasMischanteil > 40)
 495 
            if(GasMischanteil > 40)
495 
                {
 496 
                {
496 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 497 
                if(modell_fliegt < 0xffff) modell_fliegt++;
497 
                }
 498 
                }
498 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 499 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
499 
                {
 500 
                {
500 
                SummeNick = 0;
 501 
                SummeNick = 0;
501 
                SummeRoll = 0;
 502 
                SummeRoll = 0;
502 
                Mess_Integral_Gier = 0;
 503 
                Mess_Integral_Gier = 0;
503 
                Mess_Integral_Gier2 = 0;
 504 
                Mess_Integral_Gier2 = 0;
504 
                }
 505 
                }
505 
            if((GasMischanteil > 200) && MotorenEin == 0)
 506 
            if((GasMischanteil > 200) && MotorenEin == 0)
506 
                {
 507 
                {
507 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
508 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
508 
// auf Nullwerte kalibrieren
 509 
// auf Nullwerte kalibrieren
509 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
510 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
510 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 511 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
511 
                    {
 512 
                    {
512 
                    unsigned char setting;
 513 
                    unsigned char setting;
513 
                    if(++delay_neutral > 200)  // nicht sofort
 514 
                    if(++delay_neutral > 200)  // nicht sofort
514 
                      {
 515 
                      {
515 
                        GRN_OFF;
 516 
                        GRN_OFF;
516 
                        SetNeutral();
 517 
                        SetNeutral();
517 
                        MotorenEin = 0;
 518 
                        MotorenEin = 0;
518 
                        delay_neutral = 0;
 519 
                        delay_neutral = 0;
519 
                        modell_fliegt = 0;
 520 
                        modell_fliegt = 0;
520 
                        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)
521 
                        {
 522 
                        {
522 
                         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;
523 
                         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;
524 
                         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;
525 
                         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;
526 
                         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;
527 
                         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
528 
                        }
 529 
                        }
529 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 530 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
530 
                        Piep(GetActiveParamSetNumber());
 531 
                        Piep(GetActiveParamSetNumber());
531 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 532 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
532 
                        {
 533 
                        {
533 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 534 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
534 
                        }  
 535 
                        }  
535 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
 536 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
536 
                                                if (gps_home_position.status > 0 )
 537 
                                                if (gps_home_position.status > 0 )
537 
                                                {
 538 
                                                {
538 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
 539 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
539 
                                                        beeptime = 2000;
 540 
                                                        beeptime = 2000;
540 
                                                        Delay_ms(500);
 541 
                                                        Delay_ms(500);
541 
                                                }
 542 
                                                }
542 
                       }
 543 
                       }
543 
                    }
 544 
                    }
544 
                 else delay_neutral = 0;
 545 
                 else delay_neutral = 0;
545 
                }
 546 
                }
546 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
547 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
547 
// Gas ist unten
 548 
// Gas ist unten
548 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
549 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
549 
            if(GasMischanteil < 35)
 550 
            if(GasMischanteil < 35)
550 
                {
 551 
                {
551 
                // Starten
 552 
                // Starten
552 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 553 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
553 
                    {
 554 
                    {
554 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
555 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
555 
// Einschalten
 556 
// Einschalten
556 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
557 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
557 
                    if(++delay_einschalten > 200)
 558 
                    if(++delay_einschalten > 200)
558 
                        {
 559 
                        {
559 
                        delay_einschalten = 200;
 560 
                        delay_einschalten = 200;
560 
                        modell_fliegt = 1;
 561 
                        modell_fliegt = 1;
561 
                        MotorenEin = 1;
 562 
                        MotorenEin = 1;
562 
                        sollGier = 0;
 563 
                        sollGier = 0;
563 
                        Mess_Integral_Gier = 0;
 564 
                        Mess_Integral_Gier = 0;
564 
                        Mess_Integral_Gier2 = 0;
 565 
                        Mess_Integral_Gier2 = 0;
565 
                        Mess_IntegralNick = 0;
 566 
                        Mess_IntegralNick = 0;
566 
                        Mess_IntegralRoll = 0;
 567 
                        Mess_IntegralRoll = 0;
567 
                        Mess_IntegralNick2 = IntegralNick;
 568 
                        Mess_IntegralNick2 = IntegralNick;
568 
                        Mess_IntegralRoll2 = IntegralRoll;
 569 
                        Mess_IntegralRoll2 = IntegralRoll;
569 
                        SummeNick = 0;
 570 
                        SummeNick = 0;
570 
                        SummeRoll = 0;
 571 
                        SummeRoll = 0;
571 
                        }          
 572 
                        }          
572 
                    }  
 573 
                    }  
573 
                    else delay_einschalten = 0;
 574 
                    else delay_einschalten = 0;
574 
                //Auf Neutralwerte setzen
 575 
                //Auf Neutralwerte setzen
575 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
576 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
576 
// Auschalten
 577 
// Auschalten
577 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
578 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
578 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 579 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
579 
                    {
 580 
                    {
580 
                    if(++delay_ausschalten > 200)  // nicht sofort
 581 
                    if(++delay_ausschalten > 200)  // nicht sofort
581 
                        {
 582 
                        {
582 
                        MotorenEin = 0;
 583 
                        MotorenEin = 0;
583 
                        delay_ausschalten = 200;
 584 
                        delay_ausschalten = 200;
584 
                        modell_fliegt = 0;
 585 
                        modell_fliegt = 0;
585 
                        }
 586 
                        }
586 
                    }
 587 
                    }
587 
                else delay_ausschalten = 0;
 588 
                else delay_ausschalten = 0;
588 
                }
 589 
                }
589 
            }
 590 
            }
590 
 
 591 
 
591 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
592 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
592 
// neue Werte von der Funke
 593 
// neue Werte von der Funke
593 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
594 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
594 
 if(!NewPpmData-- || Notlandung)  
 595 
 if(!NewPpmData-- || Notlandung)  
595 
  {
 596 
  {
596 
    ParameterZuordnung();
 597 
    ParameterZuordnung();
597 
    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;
598 
    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;
599 
    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;
600 
    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;
601 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 602 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
602 
 
 603 
 
603 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 604 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
604 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 605 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
605 
 
 606 
 
606 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 607 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
607 
    if(GyroFaktor < 0) GyroFaktor = 0;
 608 
    if(GyroFaktor < 0) GyroFaktor = 0;
608 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 609 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
609 
  }
 610 
  }
610 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
611 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
611 
// Bei Empfangsausfall im Flug
612 
// Bei Empfangsausfall im Flug
612 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
613 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
613 
   if(Notlandung)
 614 
   if(Notlandung)
614 
    {
 615 
    {
615 
     StickGier = 0;
 616 
     StickGier = 0;
616 
     StickNick = 0;
 617 
     StickNick = 0;
617 
     StickRoll = 0;
 618 
     StickRoll = 0;
618 
     GyroFaktor  = 0.1;
 619 
     GyroFaktor  = 0.1;
619 
     IntegralFaktor = 0.005;
 620 
     IntegralFaktor = 0.005;
620 
    }  
 621 
    }  
621 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
622 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
622 
// Gyro-Drift kompensieren
 623 
// Gyro-Drift kompensieren
623 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
624 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
624 
#define DRIFT_FAKTOR 3
 625 
#define DRIFT_FAKTOR 3
625 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
 626 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
626 
                {
 627 
                {
627 
// Salvo 12.9.2007 Ersatzkompass *******
 628 
// Salvo 12.9.2007 Ersatzkompass *******
628 
                    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
629 
                        if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0;
 630 
                        if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0;
630 
                        ANALOG_ON;      // ADC einschalten
 631 
                        ANALOG_ON;      // ADC einschalten
631 
                        ROT_OFF;
 632 
                        ROT_OFF;
632 
// Salvo End
 633 
// Salvo End
633 
 
 634 
 
634 
               IntegralFehlerNick = IntegralNick2 - IntegralNick;
 635 
               IntegralFehlerNick = IntegralNick2 - IntegralNick;
635 
           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
 636 
           IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
636 
           ZaehlMessungen = 0;
 637 
           ZaehlMessungen = 0;
637 
// Salvo 1.9.2007 *************************
 638 
// Salvo 1.9.2007 *************************
638 
// 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
639 
                   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
640 
                        w = (abs(Mittelwert_AccNick));
 641 
                        w = (abs(Mittelwert_AccNick));
641 
                        v = (abs(Mittelwert_AccRoll));
 642 
                        v = (abs(Mittelwert_AccRoll));
642 
                ANALOG_ON;      // ADC einschalten
 643 
                ANALOG_ON;      // ADC einschalten
643 
                        // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007
 644 
                        // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007
644 
                        if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30))
 645 
                        if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30))
645 
                        {
 646 
                        {
646 
                                if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 647 
                                if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
647 
                                {              
 648 
                                {              
648 
                         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
 649 
                         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
649 
                         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
 650 
                         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
650 
                         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
 651 
                         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
651 
                         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
 652 
                         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
652 
                                }
 653 
                                }
653 
                                else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 654 
                                else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
654 
                                {
 655 
                                {
655 
                         if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
 656 
                         if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
656 
                         if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
 657 
                         if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
657 
                         if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
 658 
                         if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
658 
                         if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
 659 
                         if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
659 
                                }
 660 
                                }
660 
                                else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 661 
                                else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
661 
                                {
 662 
                                {
662 
                         if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
 663 
                         if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
663 
                         if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
 664 
                         if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
664 
                         if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
 665 
                         if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
665 
                         if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
 666 
                         if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
666 
                                }
 667 
                                }
667 
                        }
 668 
                        }
668 
// Salvo End
 669 
// Salvo End
669 
 
 670 
 
670 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 671 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
671 
                Mess_IntegralNick2 = IntegralNick;
 672 
                Mess_IntegralNick2 = IntegralNick;
672 
                Mess_IntegralRoll2 = IntegralRoll;
 673 
                Mess_IntegralRoll2 = IntegralRoll;
673 
                Mess_Integral_Gier2 = Integral_Gier;
 674 
                Mess_Integral_Gier2 = Integral_Gier;
674 
                ANALOG_ON;      // ADC einschalten
 675 
                ANALOG_ON;      // ADC einschalten
675 
        }
 676 
        }
676 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
677 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
677 
// Integrale auf ACC-Signal abgleichen
 678 
// Integrale auf ACC-Signal abgleichen
678 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
679 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
679 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
 680 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
680 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
 681 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
681 
#define AUSGLEICH 500
 682 
#define AUSGLEICH 500
682 
        if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 683 
        if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
683 
        if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 684 
        if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
684 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 685 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
685 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 686 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
686 
 
 687 
 
687 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 688 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
688 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
 689 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
689 
 
 690 
 
690 
        w = (abs(Mittelwert_AccNick));
 691 
        w = (abs(Mittelwert_AccNick));
691 
        v = (abs(Mittelwert_AccRoll));
 692 
        v = (abs(Mittelwert_AccRoll));
692 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 693 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
693 
        {
 694 
        {
694 
        Mess_IntegralNick -= tmp_long;
 695 
        Mess_IntegralNick -= tmp_long;
695 
        Mess_IntegralRoll -= tmp_long2;
 696 
        Mess_IntegralRoll -= tmp_long2;
696 
        }
 697 
        }
697 
        else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
 698 
        else if ((w  < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT))
698 
        {
 699 
        {
699 
        Mess_IntegralNick -= tmp_long/2; //Vorher 8
 700 
        Mess_IntegralNick -= tmp_long/2; //Vorher 8
700 
        Mess_IntegralRoll -= tmp_long2/2;
 701 
        Mess_IntegralRoll -= tmp_long2/2;
701 
        }
 702 
        }
702 
        else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
 703 
        else if ((w  < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT))
703 
        {
 704 
        {
704 
        Mess_IntegralNick -= tmp_long/4;
 705 
        Mess_IntegralNick -= tmp_long/4;
705 
        Mess_IntegralRoll -= tmp_long2/4;
 706 
        Mess_IntegralRoll -= tmp_long2/4;
706 
        }
 707 
        }
707 
        else
 708 
        else
708 
        {
 709 
        {
709 
        Mess_IntegralNick -= tmp_long/8;
 710 
        Mess_IntegralNick -= tmp_long/8;
710 
        Mess_IntegralRoll -= tmp_long2/8;              
 711 
        Mess_IntegralRoll -= tmp_long2/8;              
711 
        }
 712 
        }
712 
// Salvo End ***********************
 713 
// Salvo End ***********************
713 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
714 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
714 
//  Gieren
 715 
//  Gieren
715 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
716 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
716 
    sollGier = StickGier;
 717 
    sollGier = StickGier;
717 
    if(abs(StickGier) > 30)
 718 
    if(abs(StickGier) > 30)
718 
     {
 719 
     {
719 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 720 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
720 
     }
 721 
     }
721 
    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
 722 
    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
722 
    Mess_Integral_Gier -= tmp_int;  
 723 
    Mess_Integral_Gier -= tmp_int;  
723 
    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
 724 
    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
724 
    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
 725 
    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
725 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
 726 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
726 
    Mess_Integral_Gier2  -= tmp_int;  
 727 
    Mess_Integral_Gier2  -= tmp_int;  
727 
// Salvo End *************************
 728 
// Salvo End *************************
728 
 ANALOG_ON;     // ADC einschalten
 729 
 ANALOG_ON;     // ADC einschalten
729 
 
 730 
 
730 
// Salvo Ersatzkompass  26.9.2007 **********************
 731 
// Salvo Ersatzkompass  26.9.2007 **********************
731 
        if ((Kompass_Neuer_Wert > 0))
 732 
        if ((Kompass_Neuer_Wert > 0))
732 
        {
 733 
        {
733 
           Kompass_Neuer_Wert = 0;
 734 
           Kompass_Neuer_Wert = 0;
734 
           w = (abs(Mittelwert_AccNick));
 735 
           w = (abs(Mittelwert_AccNick));
735 
           v = (abs(Mittelwert_AccRoll));
 736 
           v = (abs(Mittelwert_AccRoll));
736 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
 737 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
737 
           {
 738 
           {
738 
                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
 739 
                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
739 
                 {
 740 
                 {
740 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 741 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
741 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
742 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
742 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
 743 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
743 
                  w = KompassValue - GyroKomp_Int;
 744 
                  w = KompassValue - GyroKomp_Int;
744 
                  if ((w > 0) && (w < 180))
 745 
                  if ((w > 0) && (w < 180))
745 
                  {
 746 
                  {
746 
                   ++GyroKomp_Int;
 747 
                   ++GyroKomp_Int;
747 
                  }
 748 
                  }
748 
                  else if ((w > 0) && (w >= 180))
 749 
                  else if ((w > 0) && (w >= 180))
749 
                  {
 750 
                  {
750 
                   --GyroKomp_Int;
 751 
                   --GyroKomp_Int;
751 
                  }
 752 
                  }
752 
                  else if ((w < 0) && (w >= -180))
 753 
                  else if ((w < 0) && (w >= -180))
753 
                  {
 754 
                  {
754 
                   --GyroKomp_Int;
 755 
                   --GyroKomp_Int;
755 
                  }
 756 
                  }
756 
                  else if ((w < 0) && (w < -180))
 757 
                  else if ((w < 0) && (w < -180))
757 
                  {
 758 
                  {
758 
                   ++GyroKomp_Int;
 759 
                   ++GyroKomp_Int;
759 
                  }
 760 
                  }
760 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
 761 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
761 
                  GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
 762 
                  GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
762 
                  ANALOG_ON;    // ADC einschalten
 763 
                  ANALOG_ON;    // ADC einschalten
763 
                 }
 764 
                 }
764 
           }
 765 
           }
765 
           else magkompass_ok = 0;
 766 
           else magkompass_ok = 0;
766 
        }
 767 
        }
767 
// Salvo End *************************
 768 
// Salvo End *************************
768 
 
 769 
 
769 
// Salvo 15.9.2007 GPS Hold Aktiveren mit dem Hoehenschalter aber nur wenn Knueppel in Ruhelage sind
 770 
// Salvo 15.9.2007 GPS Hold Aktiveren mit dem Hoehenschalter aber nur wenn Knueppel in Ruhelage sind
770 
 //if  ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF))
 771 
 //if  ((Parameter_MaxHoehe > 200) && ( (abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF))
771 
 if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF) && ( (abs(StickNick)) < GPS_STICK_HOLDOFF))
 772 
 if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && ((abs(StickRoll)) < GPS_STICK_HOLDOFF)
- 
 
 773 
        && ( (abs(StickNick)) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
772 
 {
 774 
 {
773 
        short int n;
 775 
        short int n;
774 
        n= GPS_CRTL(GPS_CMD_REQ_HOLD);
 776 
        n= GPS_CRTL(GPS_CMD_REQ_HOLD);
775 
 }
 777 
 }
776 
 else
 778 
 else
777 
 {
 779 
 {
778 
        n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
 780 
        n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
779 
 }
 781 
 }
780 
 
 782 
 
781 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
783 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
782 
//  Kompass
 784 
//  Kompass
783 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
785 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
784 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
 786 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
785 
     {
 787 
     {
786 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
 788 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
787 
       v = abs(IntegralRoll /512);
 789 
       v = abs(IntegralRoll /512);
788 
           if(v > w) w = v; // grösste Neigung ermitteln
 790 
           if(v > w) w = v; // grösste Neigung ermitteln
789 
 
 791 
 
790 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
 792 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
791 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
 793 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
792 
        {
 794 
        {
793 
                 KompassStartwert = KompassValue;
 795 
                 KompassStartwert = KompassValue;
794 
         NeueKompassRichtungMerken = 0;
 796 
         NeueKompassRichtungMerken = 0;
795 
        }
 797 
        }
796 
// Salvo 13.9.2007
 798 
// Salvo 13.9.2007
797 
       w=0;
 799 
       w=0;
798 
// Salvo End
 800 
// Salvo End
799 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 801 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
800 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 802 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
801 
       if(w > 0)
 803 
       if(w > 0)
802 
        {
 804 
        {
803 
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 805 
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
804 
 
 806 
 
805 
// Salvo Kompasssteuerung **********************       
807 
// Salvo Kompasssteuerung **********************       
806 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 808 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
807 
// Salvo End
 809 
// Salvo End
808 
          ANALOG_ON;  // ADC einschalten
 810 
          ANALOG_ON;  // ADC einschalten
809 
        }  
 811 
        }  
810 
     }
 812 
     }
811 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
813 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
812 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
814 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
813 
 
 815 
 
814 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
816 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
815 
//  Debugwerte zuordnen
 817 
//  Debugwerte zuordnen
816 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
818 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
817 
DebugOut.Sekunden++;
 819 
DebugOut.Sekunden++;
818 
  if(!TimerWerteausgabe--)
 820 
  if(!TimerWerteausgabe--)
819 
   {
 821 
   {
820 
    TimerWerteausgabe = 49;
 822 
    TimerWerteausgabe = 49;
821 
//    DebugOut.Analog[0] = MesswertNick;
 823 
//    DebugOut.Analog[0] = MesswertNick;
822 
//    DebugOut.Analog[1] = MesswertRoll;
 824 
//    DebugOut.Analog[1] = MesswertRoll;
823 
//    DebugOut.Analog[2] = MesswertGier;
 825 
//    DebugOut.Analog[2] = MesswertGier;
824 
//    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 826 
//    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
825 
//    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 827 
//    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
826 
//    DebugOut.Analog[2] = Mittelwert_AccNick;
 828 
//    DebugOut.Analog[2] = Mittelwert_AccNick;
827 
//    DebugOut.Analog[3] = Mittelwert_AccRoll;
 829 
//    DebugOut.Analog[3] = Mittelwert_AccRoll;
828 
    DebugOut.Analog[4] = MesswertGier;
 830 
    DebugOut.Analog[4] = MesswertGier;
829 
    DebugOut.Analog[5] = HoehenWert;
 831 
    DebugOut.Analog[5] = HoehenWert;
830 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 832 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
831 
/*
 833 
/*
832 
    DebugOut.Analog[7] = GasMischanteil;
 834 
    DebugOut.Analog[7] = GasMischanteil;
833 
    DebugOut.Analog[8] = KompassValue;
 835 
    DebugOut.Analog[8] = KompassValue;
834 
*/
 836 
*/
835 
    DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
 837 
    DebugOut.Analog[0] = GPS_hdng_rel_2trgt;
836 
    DebugOut.Analog[1] = GPS_dist_2trgt;
 838 
    DebugOut.Analog[1] = GPS_dist_2trgt;
837 
    DebugOut.Analog[2] = GPS_Nick;
 839 
    DebugOut.Analog[2] = GPS_Nick;
838 
    DebugOut.Analog[3] = GPS_Roll;
 840 
    DebugOut.Analog[3] = GPS_Roll;
839 
        DebugOut.Analog[7] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 841 
        DebugOut.Analog[7] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
840 
    DebugOut.Analog[8] = KompassValue;
 842 
    DebugOut.Analog[8] = KompassValue;
841 
/*
 843 
/*
842 
        DebugOut.Analog[7] =  gps_rel_act_position.utm_east;
 844 
        DebugOut.Analog[7] =  gps_rel_act_position.utm_east;
843 
        DebugOut.Analog[8] =  gps_rel_act_position.utm_north;
 845 
        DebugOut.Analog[8] =  gps_rel_act_position.utm_north;
844 
*/
 846 
*/
845 
        DebugOut.Analog[9] =  gps_reg_x;
 847 
        DebugOut.Analog[9] =  gps_reg_x;
846 
        DebugOut.Analog[10] = gps_reg_y;
 848 
        DebugOut.Analog[10] = gps_reg_y;
- 
 
 849 
//      DebugOut.Analog[11]     = gps_alive_cnt;
847 
        DebugOut.Analog[11] = GPS_hdng_abs_2trgt;
 850 
        DebugOut.Analog[11] = GPS_hdng_abs_2trgt;
848 
 
 851 
 
849 
 
 852 
 
850 
// ******provisorisch
 853 
// ******provisorisch
851 
//    DebugOut.Analog[9]  = cnt1;      
854 
//    DebugOut.Analog[9]  = cnt1;      
852 
//    DebugOut.Analog[10] = cnt1;
 855 
//    DebugOut.Analog[10] = cnt1;
853 
//      DebugOut.Analog[11] = cnt2;
 856 
//      DebugOut.Analog[11] = cnt2;
854 
//    DebugOut.Analog[10]  = (gps_act_position.utm_east/10) % 10000;   
857 
//    DebugOut.Analog[10]  = (gps_act_position.utm_east/10) % 10000;   
855 
//    DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
 858 
//    DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000;
856 
// ******provisorisch
 859 
// ******provisorisch
857 
               
 860 
               
858 
/*
 861 
/*
859 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 862 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
860 
    DebugOut.Analog[10] = magkompass_ok;
 863 
    DebugOut.Analog[10] = magkompass_ok;
861 
        DebugOut.Analog[11] = Mess_Integral_Gier2;
 864 
        DebugOut.Analog[11] = Mess_Integral_Gier2;
862 
*/
 865 
*/
863 
/*
 866 
/*
864 
        DebugOut.Analog[11] = GyroKomp_Inc_Grad;
 867 
        DebugOut.Analog[11] = GyroKomp_Inc_Grad;
865 
        DebugOut.Analog[12] = GyroKomp_Value;
 868 
        DebugOut.Analog[12] = GyroKomp_Value;
866 
*/
 869 
*/
867 
//    DebugOut.Analog[9] = SollHoehe;
 870 
//    DebugOut.Analog[9] = SollHoehe;
868 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 871 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
869 
//    DebugOut.Analog[11] = KompassStartwert;
 872 
//    DebugOut.Analog[11] = KompassStartwert;
870 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 873 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
871 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 874 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
872 
//    DebugOut.Analog[9] = KompassRichtung;    
 875 
//    DebugOut.Analog[9] = KompassRichtung;    
873 
//    DebugOut.Analog[10] = GasMischanteil;
 876 
//    DebugOut.Analog[10] = GasMischanteil;
874 
//    DebugOut.Analog[3] = HoeheD * 32;
 877 
//    DebugOut.Analog[3] = HoeheD * 32;
875 
//    DebugOut.Analog[4] = hoehenregler;
 878 
//    DebugOut.Analog[4] = hoehenregler;
876 
  }
 879 
  }
877 
 
 880 
 
878 
 
 881 
 
879 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
882 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
880 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 883 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
881 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
884 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
882 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 885 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
883 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 886 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
884 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 887 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
885 
 
 888 
 
886 
    // Maximalwerte abfangen
 889 
    // Maximalwerte abfangen
887 
    #define MAX_SENSOR  2048
 890 
    #define MAX_SENSOR  2048
888 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 891 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
889 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 892 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
890 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 893 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
891 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 894 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
892 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 895 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
893 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 896 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
894 
 
 897 
 
895 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
898 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
896 
// Höhenregelung
 899 
// Höhenregelung
897 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 900 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
898 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
901 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
899 
//OCR0B = 180 - (Poti1 + 120) / 4;
 902 
//OCR0B = 180 - (Poti1 + 120) / 4;
900 
//DruckOffsetSetting = OCR0B;
 903 
//DruckOffsetSetting = OCR0B;
901 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 904 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
902 
  {
 905 
  {
903 
    int tmp_int;
 906 
    int tmp_int;
904 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 907 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
905 
    {
 908 
    {
906 
// Salvo 15.9.2007 Grenzwert fuer Hoehenschalter kleiner, damit
909 
// Salvo 15.9.2007 Grenzwert fuer Hoehenschalter kleiner, damit
907 
// mit dem Schalter die Zusatende AUS- Hoehenregler AN -- Hoehenreger und GPS an
 910 
// mit dem Schalter die Zusatende AUS- Hoehenregler AN -- Hoehenreger und GPS an
908 
//durchlaufen werden koennen
 911 
//durchlaufen werden koennen
909 
     if(Parameter_MaxHoehe < 50)
 912 
     if(Parameter_MaxHoehe < 50)
910 
// Salvo End
 913 
// Salvo End
911 
      {
 914 
      {
912 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 915 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
913 
       HoehenReglerAktiv = 0;
 916 
       HoehenReglerAktiv = 0;
914 
      }
 917 
      }
915 
      else  
 918 
      else  
916 
        HoehenReglerAktiv = 1;
 919 
        HoehenReglerAktiv = 1;
917 
    }
 920 
    }
918 
    else
 921 
    else
919 
    {
 922 
    {
920 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
 923 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
921 
     HoehenReglerAktiv = 1;
 924 
     HoehenReglerAktiv = 1;
922 
    }
 925 
    }
923 
 
 926 
 
924 
    if(Notlandung) SollHoehe = 0;
 927 
    if(Notlandung) SollHoehe = 0;
925 
    h = HoehenWert;
 928 
    h = HoehenWert;
926 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 929 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
927 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 930 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
928 
      h = GasMischanteil - h;         // vom Gas abziehen
 931 
      h = GasMischanteil - h;         // vom Gas abziehen
929 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 932 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
930 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 933 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
931 
      if(tmp_int > 50) tmp_int = 50;
 934 
      if(tmp_int > 50) tmp_int = 50;
932 
      else if(tmp_int < -50) tmp_int = -50;
 935 
      else if(tmp_int < -50) tmp_int = -50;
933 
      h -= tmp_int;
 936 
      h -= tmp_int;
934 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 937 
      hoehenregler = (hoehenregler*15 + h) / 16;      
935 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 938 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
936 
       {
 939 
       {
937 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 940 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
938 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 941 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
939 
       }  
 942 
       }  
940 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 943 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
941 
      GasMischanteil = hoehenregler;
 944 
      GasMischanteil = hoehenregler;
942 
     }
 945 
     }
943 
  }
 946 
  }
944 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
947 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
945 
// + Mischer und PI-Regler
 948 
// + Mischer und PI-Regler
946 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
949 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
947 
 
 950 
 
948 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
951 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
949 
// Gier-Anteil
 952 
// Gier-Anteil
950 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
953 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
951 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 954 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
952 
    if(GierMischanteil > 100)  GierMischanteil = 100;
 955 
    if(GierMischanteil > 100)  GierMischanteil = 100;
953 
    if(GierMischanteil < -100) GierMischanteil = -100;
 956 
    if(GierMischanteil < -100) GierMischanteil = -100;
954 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
957 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
955 
// Nick-Achse
 958 
// Nick-Achse
956 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
959 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
957 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
 960 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
958 
    SummeNick += DiffNick;                                   // I-Anteil
 961 
    SummeNick += DiffNick;                                   // I-Anteil
959 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 962 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
960 
    if(SummeNick >  16000) SummeNick =  16000;
 963 
    if(SummeNick >  16000) SummeNick =  16000;
961 
    if(SummeNick < -16000) SummeNick = -16000;
 964 
    if(SummeNick < -16000) SummeNick = -16000;
962 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
965 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
963 
    // Motor Vorn
 966 
    // Motor Vorn
964 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 967 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
965 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 968 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
966 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 969 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
967 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 970 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
968 
        Motor_Vorne = motorwert;           
 971 
        Motor_Vorne = motorwert;           
969 
    // Motor Heck
 972 
    // Motor Heck
970 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 973 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
971 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 974 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
972 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 975 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
973 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 976 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
974 
        Motor_Hinten = motorwert;              
 977 
        Motor_Hinten = motorwert;              
975 
 
 978 
 
976 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
979 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
977 
// Roll-Achse
 980 
// Roll-Achse
978 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
981 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
979 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
 982 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
980 
        SummeRoll += DiffRoll;                                                              // I-Anteil
 983 
        SummeRoll += DiffRoll;                                                              // I-Anteil
981 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 984 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
982 
    if(SummeRoll >  16000) SummeRoll =  16000;
 985 
    if(SummeRoll >  16000) SummeRoll =  16000;
983 
    if(SummeRoll < -16000) SummeRoll = -16000;
 986 
    if(SummeRoll < -16000) SummeRoll = -16000;
984 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 987 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
985 
    // Motor Links
 988 
    // Motor Links
986 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 989 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
987 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 990 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
988 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 991 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
989 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 992 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
990 
        Motor_Links = motorwert;               
 993 
        Motor_Links = motorwert;               
991 
    // Motor Rechts
 994 
    // Motor Rechts
992 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 995 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
993 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 996 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
994 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 997 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
995 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 998 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
996 
        Motor_Rechts = motorwert;
 999 
        Motor_Rechts = motorwert;
997 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1000 
   // +++++++++++++++++++++++++++++++++++++++++++++++
998 
 
 1001 
 
999 
}
 1002 
}
1000 
 
 1003 
 
1001 
 
 1004