Subversion Repositories FlightCtrl

Rev

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

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