Subversion Repositories FlightCtrl

Rev

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

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