Subversion Repositories FlightCtrl

Rev

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

Rev 565 Rev 566
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// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
51 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 51 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52 
// +  POSSIBILITY OF SUCH DAMAGE.
52 
// +  POSSIBILITY OF SUCH DAMAGE.
53 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 53 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
54 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
 54 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
55 
/*
 55 
/*
56 
Driftkompensation fuer Gyros verbessert
 56 
Driftkompensation fuer Gyros verbessert
57 
Linearsensor mit fixem Neutralwert
 57 
Linearsensor mit fixem Neutralwert
58 
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
 58 
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
59 
*/
 59 
*/
60 
 
 60 
 
61 
#include "main.h"
 61 
#include "main.h"
62 
#include "eeprom.c"
 62 
#include "eeprom.c"
63 
 
 63 
 
64 
unsigned char h,m,s;
 64 
unsigned char h,m,s;
65 
volatile unsigned int I2CTimeout = 100;
 65 
volatile unsigned int I2CTimeout = 100;
66 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 66 
volatile int MesswertNick,MesswertRoll,MesswertGier;
67 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 67 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 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 IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 73 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
74 
volatile long Integral_Gier = 0;
 74 
volatile long Integral_Gier = 0;
75 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 75 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
76 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 76 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
77 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 77 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
78 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 78 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
79 
volatile long Mess_Integral_Hoch = 0;
 79 
volatile long Mess_Integral_Hoch = 0;
80 
volatile int  KompassValue = 0;
 80 
volatile int  KompassValue = 0;
81 
volatile int  KompassStartwert = 0;
 81 
volatile int  KompassStartwert = 0;
82 
volatile int  KompassRichtung = 0;
 82 
volatile int  KompassRichtung = 0;
83 
unsigned char MAX_GAS,MIN_GAS;
 83 
unsigned char MAX_GAS,MIN_GAS;
84 
unsigned char Notlandung = 0;
 84 
unsigned char Notlandung = 0;
85 
unsigned char HoehenReglerAktiv = 0;
 85 
unsigned char HoehenReglerAktiv = 0;
86 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 86 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
87 
 
 87 
 
88 
//Salvo 12.10.2007
 88 
//Salvo 12.10.2007
89 
uint8_t magkompass_ok=0;
 89 
uint8_t magkompass_ok=0;
90 
uint8_t gps_cmd = GPS_CMD_STOP;
 90 
uint8_t gps_cmd = GPS_CMD_STOP;
91 
static int       ubat_cnt =0;
 91 
static int       ubat_cnt =0;
92 
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
 92 
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
93 
int w,v;
 93 
int w,v;
94 
//Salvo End
 94 
//Salvo End
95 
 
 95 
 
96 
 //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
 96 
 //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
97 
long GyroKomp_Int;
 97 
long GyroKomp_Int;
98 
long int GyroGier_Comp;
 98 
long int GyroGier_Comp;
99 
int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
 99 
int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
100 
short int cnt_stickgier_zero =0;
 100 
short int cnt_stickgier_zero =0;
101 
int gyrogier_kompass;
 101 
int gyrogier_kompass;
102 
// Salvo End
 102 
// Salvo End
103 
 
 103 
 
104 
float GyroFaktor;
 104 
float GyroFaktor;
105 
float IntegralFaktor;
 105 
float IntegralFaktor;
106 
volatile int  DiffNick,DiffRoll;
 106 
volatile int  DiffNick,DiffRoll;
107 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 107 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
108 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 108 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
109 
unsigned char MotorWert[5];
 109 
unsigned char MotorWert[5];
110 
volatile unsigned char SenderOkay = 0;
 110 
volatile unsigned char SenderOkay = 0;
111 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 111 
int StickNick = 0,StickRoll = 0,StickGier = 0;
112 
char MotorenEin = 0;
 112 
char MotorenEin = 0;
113 
int HoehenWert = 0;
 113 
int HoehenWert = 0;
114 
int SollHoehe = 0;
 114 
int SollHoehe = 0;
115 
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
 115 
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
116 
float Ki =  FAKTOR_I;
 116 
float Ki =  FAKTOR_I;
117 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
 117 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
118 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 118 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
119 
 
 119 
 
120 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 120 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
121 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 121 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
122 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 122 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
123 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 123 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
124 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
 124 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
125 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 125 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
126 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 126 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
127 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 127 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
128 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 128 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
129 
unsigned char Parameter_UserParam1 = 0;
 129 
unsigned char Parameter_UserParam1 = 0;
130 
unsigned char Parameter_UserParam2 = 0;
 130 
unsigned char Parameter_UserParam2 = 0;
131 
unsigned char Parameter_UserParam3 = 0;
 131 
unsigned char Parameter_UserParam3 = 0;
132 
unsigned char Parameter_UserParam4 = 0;
 132 
unsigned char Parameter_UserParam4 = 0;
133 
unsigned char Parameter_UserParam5 = 0;
 133 
unsigned char Parameter_UserParam5 = 0;
134 
unsigned char Parameter_UserParam6 = 0;
 134 
unsigned char Parameter_UserParam6 = 0;
135 
unsigned char Parameter_UserParam7 = 0;
 135 
unsigned char Parameter_UserParam7 = 0;
136 
unsigned char Parameter_UserParam8 = 0;
 136 
unsigned char Parameter_UserParam8 = 0;
137 
unsigned char Parameter_ServoNickControl = 100;
 137 
unsigned char Parameter_ServoNickControl = 100;
138 
unsigned char Parameter_LoopGasLimit = 70;
 138 
unsigned char Parameter_LoopGasLimit = 70;
139 
unsigned char Parameter_AchsKopplung1 = 0;
 139 
unsigned char Parameter_AchsKopplung1 = 0;
140 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 140 
unsigned char Parameter_AchsGegenKopplung1 = 0;
141 
unsigned char Parameter_DynamicStability = 100;
 141 
unsigned char Parameter_DynamicStability = 100;
142 
struct mk_param_struct EE_Parameter;
 142 
struct mk_param_struct EE_Parameter;
143 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 143 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
144 
 
 144 
 
145 
void Piep(unsigned char Anzahl)
 145 
void Piep(unsigned char Anzahl)
146 
{
 146 
{
147 
 while(Anzahl--)
 147 
 while(Anzahl--)
148 
 {
 148 
 {
149 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 149 
  if(MotorenEin) return; //auf keinen Fall im Flug!
150 
  beeptime = 100;
 150 
  beeptime = 100;
151 
  Delay_ms(250);
 151 
  Delay_ms(250);
152 
 }
 152 
 }
153 
}
 153 
}
154 
 
 154 
 
155 
//############################################################################
 155 
//############################################################################
156 
//  Nullwerte ermitteln
 156 
//  Nullwerte ermitteln
157 
void SetNeutral(void)
 157 
void SetNeutral(void)
158 
//############################################################################
 158 
//############################################################################
159 
{
 159 
{
160 
        // Salvo 9.12.2007
 160 
        // Salvo 9.12.2007
161 
        RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
161 
        RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
162 
        // Salvo End
 162 
        // Salvo End
163 
        NeutralAccX = 0;
 163 
        NeutralAccX = 0;
164 
        NeutralAccY = 0;
 164 
        NeutralAccY = 0;
165 
        NeutralAccZ = 0;
 165 
        NeutralAccZ = 0;
166 
    AdNeutralNick = 0; 
 166 
    AdNeutralNick = 0; 
167 
        AdNeutralRoll = 0;     
 167 
        AdNeutralRoll = 0;     
168 
        AdNeutralGier = 0;
 168 
        AdNeutralGier = 0;
169 
    Parameter_AchsKopplung1 = 0;
 169 
    Parameter_AchsKopplung1 = 0;
170 
    Parameter_AchsGegenKopplung1 = 0;
 170 
    Parameter_AchsGegenKopplung1 = 0;
171 
    CalibrierMittelwert();     
 171 
    CalibrierMittelwert();     
172 
    Delay_ms_Mess(100);
 172 
    Delay_ms_Mess(100);
173 
        CalibrierMittelwert();
 173 
        CalibrierMittelwert();
174 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 174 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
175 
     {    
 175 
     {    
176 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 176 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
177 
     }
 177 
     }
178 
 
 178 
 
179 
     AdNeutralNick= AdWertNick;
 179 
     AdNeutralNick= AdWertNick;
180 
         AdNeutralRoll= AdWertRoll;    
 180 
         AdNeutralRoll= AdWertRoll;    
181 
         AdNeutralGier= AdWertGier;
 181 
         AdNeutralGier= AdWertGier;
182 
     StartNeutralRoll = AdNeutralRoll;
 182 
     StartNeutralRoll = AdNeutralRoll;
183 
     StartNeutralNick = AdNeutralNick;
 183 
     StartNeutralNick = AdNeutralNick;
184 
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
 184 
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
185 
    {
 185 
    {
186 
      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 186 
      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
187 
          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 187 
          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
188 
          NeutralAccZ = Aktuell_az;
 188 
          NeutralAccZ = Aktuell_az;
189 
    }
 189 
    }
190 
    else
 190 
    else
191 
    {
 191 
    {
192 
      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
 192 
      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
193 
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
 193 
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
194 
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
 194 
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
195 
    }
 195 
    }
196 
   
 196 
   
197 
        Mess_IntegralNick = 0; 
 197 
        Mess_IntegralNick = 0; 
198 
    Mess_IntegralNick2 = 0;
 198 
    Mess_IntegralNick2 = 0;
199 
    Mess_IntegralRoll = 0;     
 199 
    Mess_IntegralRoll = 0;     
200 
    Mess_IntegralRoll2 = 0;
 200 
    Mess_IntegralRoll2 = 0;
201 
    Mess_Integral_Gier = 0;    
 201 
    Mess_Integral_Gier = 0;    
202 
    MesswertNick = 0;
 202 
    MesswertNick = 0;
203 
    MesswertRoll = 0;
 203 
    MesswertRoll = 0;
204 
    MesswertGier = 0;
 204 
    MesswertGier = 0;
205 
        GyroGier_Comp =0;
 205 
        GyroGier_Comp =0;
206 
    StartLuftdruck = Luftdruck;
 206 
    StartLuftdruck = Luftdruck;
207 
    HoeheD = 0;
 207 
    HoeheD = 0;
208 
    Mess_Integral_Hoch = 0;
 208 
    Mess_Integral_Hoch = 0;
209 
    KompassStartwert = KompassValue;
 209 
    KompassStartwert = KompassValue;
210 
    GPS_Neutral();
 210 
    GPS_Neutral();
211 
    beeptime = 50;  
 211 
    beeptime = 50;  
212 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 212 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
213 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
 213 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
214 
    ExternHoehenValue = 0;
 214 
    ExternHoehenValue = 0;
215 
 
 215 
 
216 
 
 216 
 
217 
//Salvo 13.10.2007 Ersatzkompass und Gas
 217 
//Salvo 13.10.2007 Ersatzkompass und Gas
218 
        GyroKomp_Int =  KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
 218 
        GyroKomp_Int =  KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
219 
        gas_mittel      =       30;
 219 
        gas_mittel      =       30;
220 
        gas_actual      =       gas_mittel;
 220 
        gas_actual      =       gas_mittel;
221 
// Salvo End
 221 
// Salvo End
222 
}
 222 
}
223 
 
 223 
 
224 
//############################################################################
 224 
//############################################################################
225 
// Bearbeitet die Messwerte
 225 
// Bearbeitet die Messwerte
226 
void Mittelwert(void)
 226 
void Mittelwert(void)
227 
//############################################################################
 227 
//############################################################################
228 
{      
 228 
{      
229 
    static signed long tmpl,tmpl2;     
 229 
    static signed long tmpl,tmpl2;     
230 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 230 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
231 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 231 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
232 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 232 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
233 
 
 233 
 
234 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
 234 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
235 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 235 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
236 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
 236 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
237 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
 237 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
238 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
 238 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
239 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
 239 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
240 
    IntegralAccZ    += Aktuell_az - NeutralAccZ;
 240 
    IntegralAccZ    += Aktuell_az - NeutralAccZ;
241 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
 241 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
242 
//Salvo 12.11.2007
 242 
//Salvo 12.11.2007
243 
                        GyroKomp_Int   += (long)MesswertGier;
 243 
                        GyroKomp_Int   += (long)MesswertGier;
244 
                        GyroGier_Comp  += (long)MesswertGier;
 244 
                        GyroGier_Comp  += (long)MesswertGier;
245 
//Salvo End
 245 
//Salvo End
246 
            Mess_Integral_Gier +=  MesswertGier;
 246 
            Mess_Integral_Gier +=  MesswertGier;
247 
            Mess_Integral_Gier2 += MesswertGier;
 247 
            Mess_Integral_Gier2 += MesswertGier;
248 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 248 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
249 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
 249 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
250 
         {
 250 
         {
251 
            tmpl = Mess_IntegralNick / 4096L;
 251 
            tmpl = Mess_IntegralNick / 4096L;
252 
            tmpl *= MesswertGier;
 252 
            tmpl *= MesswertGier;
253 
            tmpl *= Parameter_AchsKopplung1;  //125
 253 
            tmpl *= Parameter_AchsKopplung1;  //125
254 
            tmpl /= 2048L;
 254 
            tmpl /= 2048L;
255 
            tmpl2 = Mess_IntegralRoll / 4096L;
 255 
            tmpl2 = Mess_IntegralRoll / 4096L;
256 
            tmpl2 *= MesswertGier;
 256 
            tmpl2 *= MesswertGier;
257 
            tmpl2 *= Parameter_AchsKopplung1;
 257 
            tmpl2 *= Parameter_AchsKopplung1;
258 
            tmpl2 /= 2048L;
 258 
            tmpl2 /= 2048L;
259 
         }
 259 
         }
260 
      else  tmpl = tmpl2 = 0;
 260 
      else  tmpl = tmpl2 = 0;
261 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
 261 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
262 
            MesswertRoll += tmpl;
 262 
            MesswertRoll += tmpl;
263 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
 263 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
264 
            Mess_IntegralRoll2 += MesswertRoll;
 264 
            Mess_IntegralRoll2 += MesswertRoll;
265 
            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
 265 
            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
266 
            if(Mess_IntegralRoll > Umschlag180Roll)
 266 
            if(Mess_IntegralRoll > Umschlag180Roll)
267 
            {
 267 
            {
268 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
 268 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
269 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 269 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
270 
            }
 270 
            }
271 
            if(Mess_IntegralRoll <-Umschlag180Roll)
 271 
            if(Mess_IntegralRoll <-Umschlag180Roll)
272 
            {
 272 
            {
273 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
 273 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
274 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 274 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
275 
            }  
 275 
            }  
276 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
 276 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
277 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
 277 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
278 
            if(PlatinenVersion == 10)
 278 
            if(PlatinenVersion == 10)
279 
                         {
 279 
                         {
280 
              if(AdWertRoll > 1010) MesswertRoll = +1000;
 280 
              if(AdWertRoll > 1010) MesswertRoll = +1000;
281 
              if(AdWertRoll > 1017) MesswertRoll = +2000;
 281 
              if(AdWertRoll > 1017) MesswertRoll = +2000;
282 
                         }
 282 
                         }
283 
                         else
 283 
                         else
284 
                         {
 284 
                         {
285 
              if(AdWertRoll > 2020) MesswertRoll = +1000;
 285 
              if(AdWertRoll > 2020) MesswertRoll = +1000;
286 
              if(AdWertRoll > 2034) MesswertRoll = +2000;
 286 
              if(AdWertRoll > 2034) MesswertRoll = +2000;
287 
                         }
 287 
                         }
288 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
 288 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
289 
            MesswertNick -= tmpl2;
 289 
            MesswertNick -= tmpl2;
290 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
 290 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
291 
            Mess_IntegralNick2 += MesswertNick;
 291 
            Mess_IntegralNick2 += MesswertNick;
292 
            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
 292 
            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
293 
            if(Mess_IntegralNick > Umschlag180Nick)
 293 
            if(Mess_IntegralNick > Umschlag180Nick)
294 
            {
 294 
            {
295 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
 295 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
296 
             Mess_IntegralNick2 = Mess_IntegralNick;
 296 
             Mess_IntegralNick2 = Mess_IntegralNick;
297 
            }
 297 
            }
298 
            if(Mess_IntegralNick <-Umschlag180Nick)
 298 
            if(Mess_IntegralNick <-Umschlag180Nick)
299 
            {
 299 
            {
300 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
 300 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
301 
             Mess_IntegralNick2 = Mess_IntegralNick;
 301 
             Mess_IntegralNick2 = Mess_IntegralNick;
302 
            }
 302 
            }
303 
            if(AdWertNick < 15)   MesswertNick = -1000;
 303 
            if(AdWertNick < 15)   MesswertNick = -1000;
304 
            if(AdWertNick <  7)   MesswertNick = -2000;
 304 
            if(AdWertNick <  7)   MesswertNick = -2000;
305 
            if(PlatinenVersion == 10)
 305 
            if(PlatinenVersion == 10)
306 
                         {
 306 
                         {
307 
              if(AdWertNick > 1010) MesswertNick = +1000;
 307 
              if(AdWertNick > 1010) MesswertNick = +1000;
308 
              if(AdWertNick > 1017) MesswertNick = +2000;
 308 
              if(AdWertNick > 1017) MesswertNick = +2000;
309 
                         }
 309 
                         }
310 
                         else
 310 
                         else
311 
                         {
 311 
                         {
312 
              if(AdWertNick > 2020) MesswertNick = +1000;
 312 
              if(AdWertNick > 2020) MesswertNick = +1000;
313 
              if(AdWertNick > 2034) MesswertNick = +2000;
 313 
              if(AdWertNick > 2034) MesswertNick = +2000;
314 
                         }
 314 
                         }
315 
//++++++++++++++++++++++++++++++++++++++++++++++++
 315 
//++++++++++++++++++++++++++++++++++++++++++++++++
316 
// ADC einschalten
 316 
// ADC einschalten
317 
    ANALOG_ON; 
 317 
    ANALOG_ON; 
318 
//++++++++++++++++++++++++++++++++++++++++++++++++
 318 
//++++++++++++++++++++++++++++++++++++++++++++++++
319 
 
 319 
 
320 
    Integral_Gier  = Mess_Integral_Gier;
 320 
    Integral_Gier  = Mess_Integral_Gier;
321 
    IntegralNick = Mess_IntegralNick;
 321 
    IntegralNick = Mess_IntegralNick;
322 
    IntegralRoll = Mess_IntegralRoll;
 322 
    IntegralRoll = Mess_IntegralRoll;
323 
    IntegralNick2 = Mess_IntegralNick2;
 323 
    IntegralNick2 = Mess_IntegralNick2;
324 
    IntegralRoll2 = Mess_IntegralRoll2;
 324 
    IntegralRoll2 = Mess_IntegralRoll2;
325 
 
 325 
 
326 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
 326 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
327 
  {
 327 
  {
328 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
 328 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
329 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 329 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
330 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
 330 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
331 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 331 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
332 
  }
 332 
  }
333 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 333 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
334 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 334 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
335 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 335 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
336 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 336 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
337 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 337 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
338 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 338 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
339 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 339 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
340 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 340 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
341 
}
 341 
}
342 
 
 342 
 
343 
//############################################################################
 343 
//############################################################################
344 
// Messwerte beim Ermitteln der Nullage
 344 
// Messwerte beim Ermitteln der Nullage
345 
void CalibrierMittelwert(void)
 345 
void CalibrierMittelwert(void)
346 
//############################################################################
 346 
//############################################################################
347 
{                
 347 
{                
348 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 348 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
349 
        ANALOG_OFF;
 349 
        ANALOG_OFF;
350 
        MesswertNick = AdWertNick;
 350 
        MesswertNick = AdWertNick;
351 
        MesswertRoll = AdWertRoll;
 351 
        MesswertRoll = AdWertRoll;
352 
        MesswertGier = AdWertGier;
 352 
        MesswertGier = AdWertGier;
353 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
 353 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
354 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
 354 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
355 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
 355 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
356 
   // ADC einschalten
 356 
   // ADC einschalten
357 
    ANALOG_ON; 
 357 
    ANALOG_ON; 
358 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 358 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
359 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 359 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
360 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 360 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
361 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 361 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
362 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 362 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
363 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 363 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
364 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 364 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
365 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 365 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
366 
 
 366 
 
367 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 367 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
368 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 368 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
369 
}
 369 
}
370 
 
 370 
 
371 
//############################################################################
 371 
//############################################################################
372 
// Senden der Motorwerte per I2C-Bus
 372 
// Senden der Motorwerte per I2C-Bus
373 
void SendMotorData(void)
 373 
void SendMotorData(void)
374 
//############################################################################
 374 
//############################################################################
375 
{
 375 
{
376 
    if(MOTOR_OFF || !MotorenEin)
 376 
    if(MOTOR_OFF || !MotorenEin)
377 
        {
 377 
        {
378 
        Motor_Hinten = 0;
 378 
        Motor_Hinten = 0;
379 
        Motor_Vorne = 0;
 379 
        Motor_Vorne = 0;
380 
        Motor_Rechts = 0;
 380 
        Motor_Rechts = 0;
381 
        Motor_Links = 0;
 381 
        Motor_Links = 0;
382 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 382 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
383 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 383 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
384 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 384 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
385 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 385 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
386 
        }
 386 
        }
387 
 
 387 
 
388 
    DebugOut.Analog[12] = Motor_Vorne;
 388 
    DebugOut.Analog[12] = Motor_Vorne;
389 
    DebugOut.Analog[13] = Motor_Hinten;
 389 
    DebugOut.Analog[13] = Motor_Hinten;
390 
    DebugOut.Analog[14] = Motor_Links;
 390 
    DebugOut.Analog[14] = Motor_Links;
391 
    DebugOut.Analog[15] = Motor_Rechts;  
 391 
    DebugOut.Analog[15] = Motor_Rechts;  
392 
 
 392 
 
393 
    //Start I2C Interrupt Mode
 393 
    //Start I2C Interrupt Mode
394 
    twi_state = 0;
 394 
    twi_state = 0;
395 
    motor = 0;
 395 
    motor = 0;
396 
    i2c_start();
 396 
    i2c_start();
397 
}
 397 
}
398 
 
 398 
 
399 
 
 399 
 
400 
//############################################################################
 400 
//############################################################################
401 
// Trägt ggf. das Poti als Parameter ein
 401 
// Trägt ggf. das Poti als Parameter ein
402 
void ParameterZuordnung(void)
 402 
void ParameterZuordnung(void)
403 
//############################################################################
 403 
//############################################################################
404 
{
 404 
{
405 
 
 405 
 
406 
 #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;}
 406 
 #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;}
407 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 407 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
408 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 408 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
409 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 409 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
410 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 410 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
411 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 411 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
412 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 412 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
413 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 413 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
414 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 414 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
415 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 415 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
416 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 416 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
417 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 417 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
418 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 418 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
419 
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
 419 
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
420 
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
 420 
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
421 
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
 421 
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
422 
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
 422 
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
423 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 423 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
424 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 424 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
425 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
 425 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
426 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 426 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
427 
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
 427 
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
428 
 
 428 
 
429 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 429 
 Ki = (float) Parameter_I_Faktor * 0.0001;
430 
 MAX_GAS = EE_Parameter.Gas_Max;
 430 
 MAX_GAS = EE_Parameter.Gas_Max;
431 
 MIN_GAS = EE_Parameter.Gas_Min;
 431 
 MIN_GAS = EE_Parameter.Gas_Min;
432 
}
 432 
}
433 
 
 433 
 
434 
 
 434 
 
435 
//############################################################################
 435 
//############################################################################
436 
//
 436 
//
437 
void MotorRegler(void)
 437 
void MotorRegler(void)
438 
//############################################################################
 438 
//############################################################################
439 
{
 439 
{
440 
         int motorwert,pd_ergebnis,h,tmp_int;
 440 
         int motorwert,pd_ergebnis,h,tmp_int;
441 
         int GierMischanteil,GasMischanteil;
 441 
         int GierMischanteil,GasMischanteil;
442 
     static long SummeNick=0,SummeRoll=0;
 442 
     static long SummeNick=0,SummeRoll=0;
443 
     static long sollGier = 0,tmp_long,tmp_long2;
 443 
     static long sollGier = 0,tmp_long,tmp_long2;
444 
     static long IntegralFehlerNick = 0;
 444 
     static long IntegralFehlerNick = 0;
445 
     static long IntegralFehlerRoll = 0;
 445 
     static long IntegralFehlerRoll = 0;
446 
         static unsigned int RcLostTimer;
 446 
         static unsigned int RcLostTimer;
447 
         static unsigned char delay_neutral = 0;
 447 
         static unsigned char delay_neutral = 0;
448 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 448 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
449 
         static unsigned int  modell_fliegt = 0;
 449 
         static unsigned int  modell_fliegt = 0;
450 
     static int hoehenregler = 0;
 450 
     static int hoehenregler = 0;
451 
     static char TimerWerteausgabe = 0;
 451 
     static char TimerWerteausgabe = 0;
452 
     static char NeueKompassRichtungMerken = 0;
 452 
     static char NeueKompassRichtungMerken = 0;
453 
     static long ausgleichNick, ausgleichRoll;
 453 
     static long ausgleichNick, ausgleichRoll;
454 
     
 454 
     
455 
        Mittelwert();
 455 
        Mittelwert();
456 
//****** GPS Daten holen ***************
 456 
//****** GPS Daten holen ***************
457 
        short int n;
 457 
        short int n;
458 
        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
 458 
        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
459 
        n = Get_Rel_Position();
 459 
        n = Get_Rel_Position();
460 
        if (n == 0)    
 460 
        if (n == 0)    
461 
        {
 461 
        {
462 
                ROT_ON; //led blitzen lassen
 462 
                ROT_ON; //led blitzen lassen
463 
        }
 463 
        }
464 
//******PROVISORISCH***************
 464 
//******PROVISORISCH***************
465 
    GRN_ON;
 465 
    GRN_ON;
466 
 
 466 
 
467 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
467 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
468 
// Gaswert ermitteln
 468 
// Gaswert ermitteln
469 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
469 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
470 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 470 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
471 
    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
 471 
    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
472 
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
 472 
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
473 
// und dieser dann langsam zwangsweise reduziert
 473 
// und dieser dann langsam zwangsweise reduziert
474 
        if (UBat <= EE_Parameter.UnterspannungsWarnung - 2)     //Unterhalb der Piepser Schwelle aktivieren
 474 
        if (UBat <= EE_Parameter.UnterspannungsWarnung - 2)     //Unterhalb der Piepser Schwelle aktivieren
475 
        {
 475 
        {
476 
                if (ubat_cnt > 1000)
 476 
                if (ubat_cnt > 1000)
477 
                {
 477 
                {
478 
                        ubat_cnt = 0;
 478 
                        ubat_cnt = 0;
479 
                        if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
 479 
                        if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
480 
                }
 480 
                }
481 
                else ubat_cnt++;
 481 
                else ubat_cnt++;
482 
                if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
 482 
                if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
483 
        }
 483 
        }
484 
        else   //Falls UBAT wieder ok ist
 484 
        else   //Falls UBAT wieder ok ist
485 
        {
 485 
        {
486 
                if (ubat_cnt > 1000)
 486 
                if (ubat_cnt > 1000)
487 
                {
 487 
                {
488 
                        gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
 488 
                        gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
489 
                        gas_actual = GasMischanteil;
 489 
                        gas_actual = GasMischanteil;
490 
                }
 490 
                }
491 
                else
 491 
                else
492 
                {
 492 
                {
493 
                        ubat_cnt++;
 493 
                        ubat_cnt++;
494 
                        if ((ubat_cnt % 10) == 0)
 494 
                        if ((ubat_cnt % 10) == 0)
495 
                        {
 495 
                        {
496 
                                if (gas_actual < GasMischanteil) gas_actual++;
 496 
                                if (gas_actual < GasMischanteil) gas_actual++;
497 
                                else gas_actual = GasMischanteil;
 497 
                                else gas_actual = GasMischanteil;
498 
                }
 498 
                }
499 
                }
 499 
                }
500 
                GasMischanteil = gas_actual;
 500 
                GasMischanteil = gas_actual;
501 
        }      
 501 
        }      
502 
// Salvo End
 502 
// Salvo End
503 
    if(GasMischanteil < 0) GasMischanteil = 0;
 503 
    if(GasMischanteil < 0) GasMischanteil = 0;
504 
 
 504 
 
505 
 
 505 
 
506 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
506 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
507 
// Empfang schlecht
 507 
// Empfang schlecht
508 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
508 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
509 
   if(SenderOkay < 100)
 509 
   if(SenderOkay < 100)
510 
        {
 510 
        {
511 
        if(!PcZugriff)
 511 
        if(!PcZugriff)
512 
         {
 512 
         {
513 
           if(BeepMuster == 0xffff)
 513 
           if(BeepMuster == 0xffff)
514 
            {
 514 
            {
515 
             beeptime = 15000;
 515 
             beeptime = 15000;
516 
             BeepMuster = 0x0c00;
 516 
             BeepMuster = 0x0c00;
517 
            }
 517 
            }
518 
         }
 518 
         }
519 
        if(RcLostTimer) RcLostTimer--;
 519 
        if(RcLostTimer) RcLostTimer--;
520 
        else
 520 
        else
521 
         {
 521 
         {
522 
          MotorenEin = 0;
 522 
          MotorenEin = 0;
523 
          Notlandung = 0;
 523 
          Notlandung = 0;
524 
         }
 524 
         }
525 
        ROT_ON;
 525 
        ROT_ON;
526 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 526 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
527 
            {
 527 
            {
528 
            GasMischanteil = EE_Parameter.NotGas;
 528 
            GasMischanteil = EE_Parameter.NotGas;
529 
            Notlandung = 1;
 529 
            Notlandung = 1;
530 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 530 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
531 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 531 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
532 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 532 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
533 
            }
 533 
            }
534 
         else MotorenEin = 0;
 534 
         else MotorenEin = 0;
535 
        }
 535 
        }
536 
        else
 536 
        else
537 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
537 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
538 
// Emfang gut
 538 
// Emfang gut
539 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
539 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
540 
        if(SenderOkay > 140)
 540 
        if(SenderOkay > 140)
541 
            {
 541 
            {
542 
            Notlandung = 0;
 542 
            Notlandung = 0;
543 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 543 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
544 
            if(GasMischanteil > 40)
 544 
            if(GasMischanteil > 40)
545 
                {
 545 
                {
546 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 546 
                if(modell_fliegt < 0xffff) modell_fliegt++;
547 
                }
 547 
                }
548 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 548 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
549 
                {
 549 
                {
550 
                SummeNick = 0;
 550 
                SummeNick = 0;
551 
                SummeRoll = 0;
 551 
                SummeRoll = 0;
552 
                Mess_Integral_Gier = 0;
 552 
                Mess_Integral_Gier = 0;
553 
                Mess_Integral_Gier2 = 0;
 553 
                Mess_Integral_Gier2 = 0;
554 
                }
 554 
                }
555 
            if((GasMischanteil > 200) && MotorenEin == 0)
 555 
            if((GasMischanteil > 200) && MotorenEin == 0)
556 
                {
 556 
                {
557 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
557 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
558 
// auf Nullwerte kalibrieren
 558 
// auf Nullwerte kalibrieren
559 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
559 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
560 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 560 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
561 
                    {
 561 
                    {
562 
                    if(++delay_neutral > 200)  // nicht sofort
 562 
                    if(++delay_neutral > 200)  // nicht sofort
563 
                        {
 563 
                        {
564 
                        GRN_OFF;
 564 
                        GRN_OFF;
565 
                        SetNeutral();
 565 
                        SetNeutral();
566 
                        MotorenEin = 0;
 566 
                        MotorenEin = 0;
567 
                        delay_neutral = 0;
 567 
                        delay_neutral = 0;
568 
                        modell_fliegt = 0;
 568 
                        modell_fliegt = 0;
569 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 569 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
570 
                        {
 570 
                        {
571 
                         unsigned char setting=1;
 571 
                         unsigned char setting=1;
572 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 572 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
573 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 573 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
574 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 574 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
575 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 575 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
576 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 576 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
577 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 577 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
578 
                        }
 578 
                        }
579 
 
 579 
 
580 
 
 580 
 
581 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 581 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
582 
 
 582 
 
583 
                        Piep(GetActiveParamSetNumber());
 583 
                        Piep(GetActiveParamSetNumber());
584 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 584 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
585 
                        {
 585 
                        {
586 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 586 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
587 
                        }  
 587 
                        }  
588 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
 588 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
589 
                                                if (gps_home_position.status > 0 )
 589 
                                                if (gps_home_position.status > 0 )
590 
                                                {
 590 
                                                {
591 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
 591 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
592 
                                                        beeptime = 1000;
 592 
                                                        beeptime = 1000;
593 
                                                        Delay_ms(500);
 593 
                                                        Delay_ms(500);
594 
                                                }
 594 
                                                }
595 
                       }
 595 
                       }
596 
                    }
 596 
                    }
597 
                 else
 597 
                 else
598 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
 598 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
599 
                    {
 599 
                    {
600 
                    if(++delay_neutral > 200)  // nicht sofort
 600 
                    if(++delay_neutral > 200)  // nicht sofort
601 
                        {
 601 
                        {
602 
                        GRN_OFF;
 602 
                        GRN_OFF;
603 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
 603 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
604 
                        MotorenEin = 0;
 604 
                        MotorenEin = 0;
605 
                        delay_neutral = 0;
 605 
                        delay_neutral = 0;
606 
                        modell_fliegt = 0;
 606 
                        modell_fliegt = 0;
607 
                        SetNeutral();
 607 
                        SetNeutral();
- 
 
 608 
                                                if (ACC_FIXED > 0)
- 
 
 609 
                                                {
- 
 
 610 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],ACC_NICK_NEUTRAL / 256); // ACC-NeutralWerte speichern
- 
 
 611 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],ACC_NICK_NEUTRAL % 256); // ACC-NeutralWerte speichern
- 
 
 612 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],ACC_ROLL_NEUTRAL / 256);
- 
 
 613 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],ACC_ROLL_NEUTRAL % 256);
- 
 
 614 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)ACC_Z_NEUTRAL / 256);
- 
 
 615 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)ACC_Z_NEUTRAL % 256);
- 
 
 616 
                                                }
- 
 
 617 
                                                else
- 
 
 618 
                                                {
608 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
 619 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
609 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
 620 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
610 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
 621 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
611 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
 622 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
612 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
 623 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
613 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
 624 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
- 
 
 625 
                                                }
614 
                        Piep(GetActiveParamSetNumber());
 626 
                        Piep(GetActiveParamSetNumber());
615 
                        }
 627 
                        }
616 
                    }
 628 
                    }
617 
 
 629 
 
618 
                 else delay_neutral = 0;
 630 
                 else delay_neutral = 0;
619 
                }
 631 
                }
620 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
632 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
621 
// Gas ist unten
 633 
// Gas ist unten
622 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
634 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
623 
            if(GasMischanteil < 35)
 635 
            if(GasMischanteil < 35)
624 
                {
 636 
                {
625 
                // Starten
 637 
                // Starten
626 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 638 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
627 
                    {
 639 
                    {
628 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
640 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
629 
// Einschalten
 641 
// Einschalten
630 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
642 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
631 
                    if(++delay_einschalten > 200)
 643 
                    if(++delay_einschalten > 200)
632 
                        {
 644 
                        {
633 
                                                int n;
 645 
                                                int n;
634 
                                                // Salvo 9.12.2007
 646 
                                                // Salvo 9.12.2007
635 
                                                RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
647 
                                                RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
636 
                                                // Salvo End
 648 
                                                // Salvo End
637 
                        delay_einschalten = 200;
 649 
                        delay_einschalten = 200;
638 
                        modell_fliegt = 1;
 650 
                        modell_fliegt = 1;
639 
                        MotorenEin = 1;
 651 
                        MotorenEin = 1;
640 
                        sollGier = 0;
 652 
                        sollGier = 0;
641 
                        Mess_Integral_Gier = 0;
 653 
                        Mess_Integral_Gier = 0;
642 
                        Mess_Integral_Gier2 = 0;
 654 
                        Mess_Integral_Gier2 = 0;
643 
                        Mess_IntegralNick = 0;
 655 
                        Mess_IntegralNick = 0;
644 
                        Mess_IntegralRoll = 0;
 656 
                        Mess_IntegralRoll = 0;
645 
                        Mess_IntegralNick2 = IntegralNick;
 657 
                        Mess_IntegralNick2 = IntegralNick;
646 
                        Mess_IntegralRoll2 = IntegralRoll;
 658 
                        Mess_IntegralRoll2 = IntegralRoll;
647 
                        SummeNick = 0;
 659 
                        SummeNick = 0;
648 
                        SummeRoll = 0;
 660 
                        SummeRoll = 0;
649 
                                                n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
 661 
                                                n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
650 
                        }          
 662 
                        }          
651 
                    }  
 663 
                    }  
652 
                    else delay_einschalten = 0;
 664 
                    else delay_einschalten = 0;
653 
                //Auf Neutralwerte setzen
 665 
                //Auf Neutralwerte setzen
654 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
666 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
655 
// Auschalten
 667 
// Auschalten
656 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
668 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
657 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 669 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
658 
                    {
 670 
                    {
659 
                    if(++delay_ausschalten > 200)  // nicht sofort
 671 
                    if(++delay_ausschalten > 200)  // nicht sofort
660 
                        {
 672 
                        {
661 
                                                // Salvo 9.12.2007
 673 
                                                // Salvo 9.12.2007
662 
                                                RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
674 
                                                RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
663 
                                                // Salvo End
 675 
                                                // Salvo End
664 
                        MotorenEin = 0;
 676 
                        MotorenEin = 0;
665 
                        delay_ausschalten = 200;
 677 
                        delay_ausschalten = 200;
666 
                        modell_fliegt = 0;
 678 
                        modell_fliegt = 0;
667 
                        }
 679 
                        }
668 
                    }
 680 
                    }
669 
                else delay_ausschalten = 0;
 681 
                else delay_ausschalten = 0;
670 
                }
 682 
                }
671 
            }
 683 
            }
672 
 
 684 
 
673 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
685 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
674 
// neue Werte von der Funke
 686 
// neue Werte von der Funke
675 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
687 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
676 
 if(!NewPpmData-- || Notlandung)  
 688 
 if(!NewPpmData-- || Notlandung)  
677 
  {
 689 
  {
678 
    int tmp_int;
 690 
    int tmp_int;
679 
    ParameterZuordnung();
 691 
    ParameterZuordnung();
680 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 692 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
681 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 693 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
682 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 694 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
683 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 695 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
684 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 696 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
685 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 697 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
686 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 698 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
687 
 
 699 
 
688 
#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
 700 
#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
689 
if(DubWiseKeys[1]) beeptime = 10;
 701 
if(DubWiseKeys[1]) beeptime = 10;
690 
    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
 702 
    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
691 
    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
 703 
    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
692 
    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
 704 
    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
693 
    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
 705 
    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
694 
    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
 706 
    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
695 
    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
 707 
    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
696 
 
 708 
 
697 
    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
 709 
    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
698 
    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
 710 
    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
699 
    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
 711 
    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
700 
    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
 712 
    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
701 
 
 713 
 
702 
    StickNick += ExternStickNick / 8;
 714 
    StickNick += ExternStickNick / 8;
703 
    StickRoll += ExternStickRoll / 8;
 715 
    StickRoll += ExternStickRoll / 8;
704 
    StickGier += ExternStickGier;
 716 
    StickGier += ExternStickGier;
705 
 
 717 
 
706 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 718 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
707 
    if(GyroFaktor < 0) GyroFaktor = 0;
 719 
    if(GyroFaktor < 0) GyroFaktor = 0;
708 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 720 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
709 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
 721 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
710 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 722 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
711 
     {
 723 
     {
712 
      if(IntegralNick >  60000)  
 724 
      if(IntegralNick >  60000)  
713 
      {
 725 
      {
714 
       StickNick -=  8 * EE_Parameter.Stick_P;      
 726 
       StickNick -=  8 * EE_Parameter.Stick_P;      
715 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 727 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
716 
      }
 728 
      }
717 
      else
 729 
      else
718 
      if(IntegralNick < -60000)  
 730 
      if(IntegralNick < -60000)  
719 
      {
 731 
      {
720 
       StickNick += 8 * EE_Parameter.Stick_P;
 732 
       StickNick += 8 * EE_Parameter.Stick_P;
721 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 733 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
722 
      }
 734 
      }
723 
      if(IntegralRoll >  60000)  
 735 
      if(IntegralRoll >  60000)  
724 
      {
 736 
      {
725 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
 737 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
726 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 738 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
727 
      }
 739 
      }
728 
      else
 740 
      else
729 
      if(IntegralRoll < -60000)  
 741 
      if(IntegralRoll < -60000)  
730 
      {
 742 
      {
731 
       StickRoll += 8 * EE_Parameter.Stick_P;
 743 
       StickRoll += 8 * EE_Parameter.Stick_P;
732 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 744 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
733 
      }
 745 
      }
734 
     }
 746 
     }
735 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
747 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
736 
// Looping?
 748 
// Looping?
737 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
749 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
738 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
 750 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
739 
  else
 751 
  else
740 
   {
 752 
   {
741 
     {
 753 
     {
742 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
 754 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
743 
     }  
 755 
     }  
744 
   }
 756 
   }
745 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
 757 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
746 
   else
 758 
   else
747 
   {
 759 
   {
748 
   if(Looping_Rechts) // Hysterese
 760 
   if(Looping_Rechts) // Hysterese
749 
     {
 761 
     {
750 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
 762 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
751 
     }
 763 
     }
752 
   }
 764 
   }
753 
 
 765 
 
754 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
 766 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
755 
  else
 767 
  else
756 
   {
 768 
   {
757 
    if(Looping_Oben)  // Hysterese
 769 
    if(Looping_Oben)  // Hysterese
758 
     {
 770 
     {
759 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
 771 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
760 
     }  
 772 
     }  
761 
   }
 773 
   }
762 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
 774 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
763 
   else
 775 
   else
764 
   {
 776 
   {
765 
    if(Looping_Unten) // Hysterese
 777 
    if(Looping_Unten) // Hysterese
766 
     {
 778 
     {
767 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
 779 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
768 
     }
 780 
     }
769 
   }
 781 
   }
770 
 
 782 
 
771 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
 783 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
772 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
 784 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
773 
  } // Ende neue Funken-Werte
 785 
  } // Ende neue Funken-Werte
774 
 
 786 
 
775 
  if(Looping_Roll) beeptime = 100;
 787 
  if(Looping_Roll) beeptime = 100;
776 
  if(Looping_Roll || Looping_Nick)
 788 
  if(Looping_Roll || Looping_Nick)
777 
   {
 789 
   {
778 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 790 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
779 
   }
 791 
   }
780 
   
 792 
   
781 
 
 793 
 
782 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
794 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
783 
// Bei Empfangsausfall im Flug
795 
// Bei Empfangsausfall im Flug
784 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
796 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
785 
   if(Notlandung)
 797 
   if(Notlandung)
786 
    {
 798 
    {
787 
     StickGier = 0;
 799 
     StickGier = 0;
788 
     StickNick = 0;
 800 
     StickNick = 0;
789 
     StickRoll = 0;
 801 
     StickRoll = 0;
790 
     GyroFaktor  = 0.1;
 802 
     GyroFaktor  = 0.1;
791 
     IntegralFaktor = 0.005;
 803 
     IntegralFaktor = 0.005;
792 
     Looping_Roll = 0;
 804 
     Looping_Roll = 0;
793 
     Looping_Nick = 0;
 805 
     Looping_Nick = 0;
794 
    }  
 806 
    }  
795 
 
 807 
 
796 
 
 808 
 
797 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
809 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
798 
// Integrale auf ACC-Signal abgleichen
 810 
// Integrale auf ACC-Signal abgleichen
799 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
811 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
800 
#define ABGLEICH_ANZAHL 256L
 812 
#define ABGLEICH_ANZAHL 256L
801 
 
 813 
 
802 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
 814 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
803 
 MittelIntegralRoll  += IntegralRoll;
 815 
 MittelIntegralRoll  += IntegralRoll;
804 
 MittelIntegralNick2 += IntegralNick2;
 816 
 MittelIntegralNick2 += IntegralNick2;
805 
 MittelIntegralRoll2 += IntegralRoll2;
 817 
 MittelIntegralRoll2 += IntegralRoll2;
806 
 
 818 
 
807 
 if(Looping_Nick || Looping_Roll)
 819 
 if(Looping_Nick || Looping_Roll)
808 
  {
 820 
  {
809 
    IntegralAccNick = 0;
 821 
    IntegralAccNick = 0;
810 
    IntegralAccRoll = 0;
 822 
    IntegralAccRoll = 0;
811 
    MittelIntegralNick = 0;
 823 
    MittelIntegralNick = 0;
812 
    MittelIntegralRoll = 0;
 824 
    MittelIntegralRoll = 0;
813 
    MittelIntegralNick2 = 0;
 825 
    MittelIntegralNick2 = 0;
814 
    MittelIntegralRoll2 = 0;
 826 
    MittelIntegralRoll2 = 0;
815 
    Mess_IntegralNick2 = Mess_IntegralNick;
 827 
    Mess_IntegralNick2 = Mess_IntegralNick;
816 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
 828 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
817 
    ZaehlMessungen = 0;
 829 
    ZaehlMessungen = 0;
818 
    LageKorrekturNick = 0;
 830 
    LageKorrekturNick = 0;
819 
    LageKorrekturRoll = 0;
 831 
    LageKorrekturRoll = 0;
820 
  }
 832 
  }
821 
 
 833 
 
822 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
834 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
823 
  if(!Looping_Nick && !Looping_Roll)
 835 
  if(!Looping_Nick && !Looping_Roll)
824 
  {
 836 
  {
825 
   long tmp_long, tmp_long2;
 837 
   long tmp_long, tmp_long2;
826 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
 838 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
827 
    tmp_long /= 16;
 839 
    tmp_long /= 16;
828 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
 840 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
829 
    tmp_long2 /= 16;
 841 
    tmp_long2 /= 16;
830 
 #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
 842 
 #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
831 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 843 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
832 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 844 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
833 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 845 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
834 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 846 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
835 
    Mess_IntegralNick -= tmp_long;
 847 
    Mess_IntegralNick -= tmp_long;
836 
    Mess_IntegralRoll -= tmp_long2;
 848 
    Mess_IntegralRoll -= tmp_long2;
837 
  }
 849 
  }
838 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
850 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
839 
 
 851 
 
840 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 852 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
841 
 {
 853 
 {
842 
  static int cnt = 0;
 854 
  static int cnt = 0;
843 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
 855 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
844 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
 856 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
845 
  if(!Looping_Nick && !Looping_Roll)
 857 
  if(!Looping_Nick && !Looping_Roll)
846 
  {
 858 
  {
847 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
 859 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
848 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
 860 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
849 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
 861 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
850 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
 862 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
851 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 863 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
852 
#define MAX_I 0//(Poti2/10)
 864 
#define MAX_I 0//(Poti2/10)
853 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
 865 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
854 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
 866 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
855 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 867 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
856 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 868 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
857 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
869 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
858 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
 870 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
859 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 871 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
860 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 872 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
861 
 
 873 
 
862 
//    Mess_IntegralNick -= ausgleichNick;
 874 
//    Mess_IntegralNick -= ausgleichNick;
863 
//    Mess_IntegralRoll -= ausgleichRoll;
 875 
//    Mess_IntegralRoll -= ausgleichRoll;
864 
 
 876 
 
865 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
877 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
866 
// Gyro-Drift ermitteln
 878 
// Gyro-Drift ermitteln
867 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
879 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
868 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
 880 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
869 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
 881 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
870 
//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
882 
//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
871 
//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
883 
//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
872 
    tmp_long  = IntegralNick2 - IntegralNick;
 884 
    tmp_long  = IntegralNick2 - IntegralNick;
873 
    tmp_long2 = IntegralRoll2 - IntegralRoll;
 885 
    tmp_long2 = IntegralRoll2 - IntegralRoll;
874 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
 886 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
875 
 
 887 
 
876 
    IntegralFehlerNick = tmp_long;
 888 
    IntegralFehlerNick = tmp_long;
877 
    IntegralFehlerRoll = tmp_long2;
 889 
    IntegralFehlerRoll = tmp_long2;
878 
    Mess_IntegralNick2 -= IntegralFehlerNick;
 890 
    Mess_IntegralNick2 -= IntegralFehlerNick;
879 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
 891 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
880 
 
 892 
 
881 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 893 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
882 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 894 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
883 
 
 895 
 
884 
//Salvo Ersatzkompass Ueberlauf korrigieren
 896 
//Salvo Ersatzkompass Ueberlauf korrigieren
885 
                if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
 897 
                if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
886 
                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
 898 
                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
887 
                ROT_OFF;
 899 
                ROT_OFF;
888 
// Salvo End
 900 
// Salvo End
889 
 
 901 
 
890 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
 902 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
891 
DebugOut.Analog[18] = IntegralAccRoll / 26;
 903 
DebugOut.Analog[18] = IntegralAccRoll / 26;
892 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 904 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
893 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 905 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
894 
DebugOut.Analog[21] = MittelIntegralNick / 26;
 906 
DebugOut.Analog[21] = MittelIntegralNick / 26;
895 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
 907 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
896 
DebugOut.Analog[28] = ausgleichNick;
 908 
DebugOut.Analog[28] = ausgleichNick;
897 
DebugOut.Analog[29] = ausgleichRoll;
 909 
DebugOut.Analog[29] = ausgleichRoll;
898 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
 910 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
899 
*/
 911 
*/
900 
 
 912 
 
901 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 913 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
902 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 914 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
903 
#define BEWEGUNGS_LIMIT 20000
 915 
#define BEWEGUNGS_LIMIT 20000
904 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
 916 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
905 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 917 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
906 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
 918 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
907 
        {
 919 
        {
908 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
 920 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
909 
         {
 921 
         {
910 
           if(last_n_p)
 922 
           if(last_n_p)
911 
           {
 923 
           {
912 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 924 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
913 
            ausgleichNick = IntegralFehlerNick / 8;
 925 
            ausgleichNick = IntegralFehlerNick / 8;
914 
            if(ausgleichNick > 5000) ausgleichNick = 5000;
 926 
            if(ausgleichNick > 5000) ausgleichNick = 5000;
915 
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 927 
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
916 
           }
 928 
           }
917 
           else last_n_p = 1;
 929 
           else last_n_p = 1;
918 
         } else  last_n_p = 0;
 930 
         } else  last_n_p = 0;
919 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
 931 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
920 
         {
 932 
         {
921 
           if(last_n_n)
 933 
           if(last_n_n)
922 
            {
 934 
            {
923 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 935 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
924 
             ausgleichNick = IntegralFehlerNick / 8;
 936 
             ausgleichNick = IntegralFehlerNick / 8;
925 
             if(ausgleichNick < -5000) ausgleichNick = -5000;
 937 
             if(ausgleichNick < -5000) ausgleichNick = -5000;
926 
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 938 
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
927 
            }
 939 
            }
928 
           else last_n_n = 1;
 940 
           else last_n_n = 1;
929 
         } else  last_n_n = 0;
 941 
         } else  last_n_n = 0;
930 
        } else cnt = 0;
 942 
        } else cnt = 0;
931 
//Salvo 11.12.2007
 943 
//Salvo 11.12.2007
932 
                w = (abs(Mittelwert_AccNick));
 944 
                w = (abs(Mittelwert_AccNick));
933 
                v = (abs(Mittelwert_AccRoll));
 945 
                v = (abs(Mittelwert_AccRoll));
934 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
 946 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
935 
                {
 947 
                {
936 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 948 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
937 
                if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
 949 
                if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
938 
                if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 950 
                if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
939 
                }
 951 
                }
940 
//Salvo End
 952 
//Salvo End
941 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 953 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
942 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 954 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
943 
 
 955 
 
944 
        ausgleichRoll = 0;
 956 
        ausgleichRoll = 0;
945 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
 957 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
946 
        {
 958 
        {
947 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
 959 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
948 
         {
 960 
         {
949 
           if(last_r_p)
 961 
           if(last_r_p)
950 
           {
 962 
           {
951 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 963 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
952 
            ausgleichRoll = IntegralFehlerRoll / 8;
 964 
            ausgleichRoll = IntegralFehlerRoll / 8;
953 
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
 965 
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
954 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 966 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
955 
           }
 967 
           }
956 
           else last_r_p = 1;
 968 
           else last_r_p = 1;
957 
         } else  last_r_p = 0;
 969 
         } else  last_r_p = 0;
958 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
 970 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
959 
         {
 971 
         {
960 
           if(last_r_n)
 972 
           if(last_r_n)
961 
           {
 973 
           {
962 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 974 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
963 
            ausgleichRoll = IntegralFehlerRoll / 8;
 975 
            ausgleichRoll = IntegralFehlerRoll / 8;
964 
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
 976 
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
965 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 977 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
966 
           }
 978 
           }
967 
           else last_r_n = 1;
 979 
           else last_r_n = 1;
968 
         } else  last_r_n = 0;
 980 
         } else  last_r_n = 0;
969 
        } else
 981 
        } else
970 
        {
 982 
        {
971 
         cnt = 0;
 983 
         cnt = 0;
972 
        }
 984 
        }
973 
//Salvo 11.12.2007
 985 
//Salvo 11.12.2007
974 
                w = (abs(Mittelwert_AccNick));
 986 
                w = (abs(Mittelwert_AccNick));
975 
                v = (abs(Mittelwert_AccRoll));
 987 
                v = (abs(Mittelwert_AccRoll));
976 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
 988 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in wwagrechter Lage nachtrimmen
977 
                {
 989 
                {
978 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 990 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
979 
                if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 991 
                if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
980 
                if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 992 
                if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
981 
             }
 993 
             }
982 
//Salvo End
 994 
//Salvo End
983 
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 995 
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
984 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 996 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
985 
  }
 997 
  }
986 
  else
 998 
  else
987 
  {
 999 
  {
988 
   LageKorrekturRoll = 0;
 1000 
   LageKorrekturRoll = 0;
989 
   LageKorrekturNick = 0;
 1001 
   LageKorrekturNick = 0;
990 
  }
 1002 
  }
991 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 1003 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
992 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1004 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
993 
   MittelIntegralNick_Alt = MittelIntegralNick;      
 1005 
   MittelIntegralNick_Alt = MittelIntegralNick;      
994 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
 1006 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
995 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1007 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
996 
    IntegralAccNick = 0;
 1008 
    IntegralAccNick = 0;
997 
    IntegralAccRoll = 0;
 1009 
    IntegralAccRoll = 0;
998 
    IntegralAccZ = 0;
 1010 
    IntegralAccZ = 0;
999 
    MittelIntegralNick = 0;
 1011 
    MittelIntegralNick = 0;
1000 
    MittelIntegralRoll = 0;
 1012 
    MittelIntegralRoll = 0;
1001 
    MittelIntegralNick2 = 0;
 1013 
    MittelIntegralNick2 = 0;
1002 
    MittelIntegralRoll2 = 0;
 1014 
    MittelIntegralRoll2 = 0;
1003 
    ZaehlMessungen = 0;
 1015 
    ZaehlMessungen = 0;
1004 
 
 1016 
 
1005 
 
 1017 
 
1006 
 } // Ende Abgleich
 1018 
 } // Ende Abgleich
1007 
 
 1019 
 
1008 
        // Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
 1020 
        // Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
1009 
        if ((Kompass_Neuer_Wert > 0))
 1021 
        if ((Kompass_Neuer_Wert > 0))
1010 
        {
 1022 
        {
1011 
           Kompass_Neuer_Wert = 0;
 1023 
           Kompass_Neuer_Wert = 0;
1012 
           w = (abs(Mittelwert_AccNick));
 1024 
           w = (abs(Mittelwert_AccNick));
1013 
           v = (abs(Mittelwert_AccRoll));
 1025 
           v = (abs(Mittelwert_AccRoll));
1014 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro  nur mit Magnetkompass aktualisieren wenn alles ok
 1026 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro  nur mit Magnetkompass aktualisieren wenn alles ok
1015 
           {
 1027 
           {
1016 
                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
 1028 
                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
1017 
                 {
 1029 
                 {
1018 
 
 1030 
 
1019 
                  if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
 1031 
                  if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
1020 
                        {
 1032 
                        {
1021 
                                if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
 1033 
                                if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
1022 
                                if (cnt_stickgier_zero > 1) // nur Abgleichen wenn keine Stickbewegung da
 1034 
                                if (cnt_stickgier_zero > 1) // nur Abgleichen wenn keine Stickbewegung da
1023 
                                {
 1035 
                                {
1024 
                                        w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
 1036 
                                        w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
1025 
                                        v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
 1037 
                                        v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
1026 
                                        if (v <-180) v +=360; // Uberlaufkorrektur
 1038 
                                        if (v <-180) v +=360; // Uberlaufkorrektur
1027 
                                        if (v > 180) v -=360; // Uberlaufkorrektur
 1039 
                                        if (v > 180) v -=360; // Uberlaufkorrektur
1028 
 
 1040 
 
1029 
                                        v = w -v;  //Differenz Gyro zu Kompass ist der Driftfehler
 1041 
                                        v = w -v;  //Differenz Gyro zu Kompass ist der Driftfehler
1030 
 
 1042 
 
1031 
                                        #define GIER_COMP_MAX 4
 1043 
                                        #define GIER_COMP_MAX 4
1032 
                                        if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
 1044 
                                        if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
1033 
                                        if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
 1045 
                                        if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
1034 
                                        if (abs(w) > 1)
 1046 
                                        if (abs(w) > 1)
1035 
                                        {      
 1047 
                                        {      
1036 
                                                GyroGier_Comp           = 0;
 1048 
                                                GyroGier_Comp           = 0;
1037 
                                                gyrogier_kompass        = KompassValue; // Kompasswert merken
 1049 
                                                gyrogier_kompass        = KompassValue; // Kompasswert merken
1038 
                                                AdNeutralGier           -= v;
 1050 
                                                AdNeutralGier           -= v;
1039 
                                        }
 1051 
                                        }
1040 
                                }
 1052 
                                }
1041 
                        }
 1053 
                        }
1042 
                  else
 1054 
                  else
1043 
                  {
 1055 
                  {
1044 
                        gyrogier_kompass        = KompassValue; // Kompasswert merken
 1056 
                        gyrogier_kompass        = KompassValue; // Kompasswert merken
1045 
                        cnt_stickgier_zero      = 0;
 1057 
                        cnt_stickgier_zero      = 0;
1046 
                        GyroGier_Comp = 0;
 1058 
                        GyroGier_Comp = 0;
1047 
                  }
 1059 
                  }
1048 
 
 1060 
 
1049 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
1061 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
1050 
                  GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
 1062 
                  GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
1051 
 
 1063 
 
1052 
                  w = KompassValue - GyroKomp_Int;
 1064 
                  w = KompassValue - GyroKomp_Int;
1053 
                  if ((w > 0) && (w < 180))
 1065 
                  if ((w > 0) && (w < 180))
1054 
                  {
 1066 
                  {
1055 
                   ++GyroKomp_Int;
 1067 
                   ++GyroKomp_Int;
1056 
                  }
 1068 
                  }
1057 
                  else if ((w > 0) && (w >= 180))
 1069 
                  else if ((w > 0) && (w >= 180))
1058 
                  {
 1070 
                  {
1059 
                   --GyroKomp_Int;
 1071 
                   --GyroKomp_Int;
1060 
                  }
 1072 
                  }
1061 
                  else if ((w < 0) && (w >= -180))
 1073 
                  else if ((w < 0) && (w >= -180))
1062 
                  {
 1074 
                  {
1063 
                   --GyroKomp_Int;
 1075 
                   --GyroKomp_Int;
1064 
                  }
 1076 
                  }
1065 
                  else if ((w < 0) && (w < -180))
 1077 
                  else if ((w < 0) && (w < -180))
1066 
                  {
 1078 
                  {
1067 
                   ++GyroKomp_Int;
 1079 
                   ++GyroKomp_Int;
1068 
                  }
 1080 
                  }
1069 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
 1081 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
1070 
                  GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
 1082 
                  GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
1071 
                 }
 1083 
                 }
1072 
           }
 1084 
           }
1073 
           else
 1085 
           else
1074 
           {
 1086 
           {
1075 
            magkompass_ok = 0;
 1087 
            magkompass_ok = 0;
1076 
                GyroGier_Comp = 0;
 1088 
                GyroGier_Comp = 0;
1077 
      }
 1089 
      }
1078 
        Kompass_Value_Old       =       KompassValue;
 1090 
        Kompass_Value_Old       =       KompassValue;
1079 
        }
 1091 
        }
1080 
// Salvo End *************************
 1092 
// Salvo End *************************
1081 
 
 1093 
 
1082 
// Salvo 6.10.2007
1094 
// Salvo 6.10.2007
1083 
        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
 1095 
        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
1084 
        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
 1096 
        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
1085 
        if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
 1097 
        if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
1086 
        && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0) && (GasMischanteil > 30))
 1098 
        && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0) && (GasMischanteil > 30))
1087 
        {
 1099 
        {
1088 
                if (Parameter_MaxHoehe > 200)  
 1100 
                if (Parameter_MaxHoehe > 200)  
1089 
                {      
 1101 
                {      
1090 
                        if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
 1102 
                        if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
1091 
                        else gps_cmd = GPS_CMD_REQ_HOME;
 1103 
                        else gps_cmd = GPS_CMD_REQ_HOME;
1092 
                        n = GPS_CRTL(gps_cmd);
 1104 
                        n = GPS_CRTL(gps_cmd);
1093 
                }
 1105 
                }
1094 
                else
 1106 
                else
1095 
                {
 1107 
                {
1096 
                        if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
 1108 
                        if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
1097 
                        else gps_cmd = GPS_CMD_REQ_HOLD;
 1109 
                        else gps_cmd = GPS_CMD_REQ_HOLD;
1098 
                        n= GPS_CRTL(gps_cmd);
 1110 
                        n= GPS_CRTL(gps_cmd);
1099 
                }
 1111 
                }
1100 
        }
 1112 
        }
1101 
        else
 1113 
        else
1102 
        {
 1114 
        {
1103 
                if (gps_cmd != GPS_CMD_STOP)
 1115 
                if (gps_cmd != GPS_CMD_STOP)
1104 
                {
 1116 
                {
1105 
                        gps_cmd = GPS_CMD_STOP;
 1117 
                        gps_cmd = GPS_CMD_STOP;
1106 
                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
 1118 
                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
1107 
                }
 1119 
                }
1108 
        }
 1120 
        }
1109 
        if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 10)  LED_J16_OFF; //led im GPS Mode schnell blinken lassen
 1121 
        if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12)  LED_J16_OFF; //led im GPS Mode schnell blinken lassen
1110 
 
 1122 
 
1111 
 
 1123 
 
1112 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1124 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1113 
//  Gieren
 1125 
//  Gieren
1114 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1126 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1115 
    if(abs(StickGier) > 20) // war 35
1127 
    if(abs(StickGier) > 20) // war 35
1116 
     {
 1128 
     {
1117 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 1129 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
1118 
     }
 1130 
     }
1119 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 1131 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1120 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 1132 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
1121 
    sollGier = tmp_int;
 1133 
    sollGier = tmp_int;
1122 
    Mess_Integral_Gier -= tmp_int;  
 1134 
    Mess_Integral_Gier -= tmp_int;  
1123 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
 1135 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
1124 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 1136 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1125 
 
 1137 
 
1126 
 
 1138 
 
1127 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1139 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1128 
//  Kompass
 1140 
//  Kompass
1129 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1141 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1130 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
 1142 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
1131 
     {
 1143 
     {
1132 
           if(v > w) w = v; // grösste Neigung ermitteln
 1144 
           if(v > w) w = v; // grösste Neigung ermitteln
1133 
 
 1145 
 
1134 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
 1146 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
1135 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
 1147 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
1136 
        {
 1148 
        {
1137 
                 KompassStartwert = KompassValue;
 1149 
                 KompassStartwert = KompassValue;
1138 
         NeueKompassRichtungMerken = 0;
 1150 
         NeueKompassRichtungMerken = 0;
1139 
        }
 1151 
        }
1140 
// Salvo 13.9.2007
 1152 
// Salvo 13.9.2007
1141 
       w=0;
 1153 
       w=0;
1142 
// Salvo End
 1154 
// Salvo End
1143 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 1155 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
1144 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 1156 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
1145 
       if(w > 0)
 1157 
       if(w > 0)
1146 
        {
 1158 
        {
1147 
// Salvo Kompasssteuerung **********************       
1159 
// Salvo Kompasssteuerung **********************       
1148 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 1160 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
1149 
// Salvo End
 1161 
// Salvo End
1150 
        }
 1162 
        }
1151 
     }
 1163 
     }
1152 
 
 1164 
 
1153 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1165 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1154 
 
 1166 
 
1155 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1167 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1156 
//  Debugwerte zuordnen
 1168 
//  Debugwerte zuordnen
1157 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1169 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1158 
  if(!TimerWerteausgabe--)
 1170 
  if(!TimerWerteausgabe--)
1159 
   {
 1171 
   {
1160 
    TimerWerteausgabe = 24;
 1172 
    TimerWerteausgabe = 24;
1161 
 
 1173 
 
1162 
        // Salvo 13.12.2007 Beleuchtung steuern
 1174 
        // Salvo 13.12.2007 Beleuchtung steuern
1163 
        if (!(beeptime & BeepMuster)) LED_J16_FLASH;
 1175 
        if (!(beeptime & BeepMuster)) LED_J16_FLASH;
1164 
        else if (MotorenEin) LED_J16_ON;
 1176 
        else if (MotorenEin) LED_J16_ON;
1165 
        else LED_J16_OFF;
 1177 
        else LED_J16_OFF;
1166 
        // Salvo End
 1178 
        // Salvo End
1167 
 
 1179 
 
1168 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 1180 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
1169 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 1181 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
1170 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 1182 
    DebugOut.Analog[2] = Mittelwert_AccNick;
1171 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 1183 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
1172 
    DebugOut.Analog[4] = MesswertGier;
 1184 
    DebugOut.Analog[4] = MesswertGier;
1173 
    DebugOut.Analog[5] = HoehenWert;
 1185 
    DebugOut.Analog[5] = HoehenWert;
1174 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
 1186 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
1175 
    DebugOut.Analog[8] = KompassValue;
 1187 
    DebugOut.Analog[8] = KompassValue;
1176 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 1188 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
1177 
 
 1189 
 
1178 
// Diverse parameter Debugging
 1190 
// Diverse parameter Debugging
1179 
        DebugOut.Analog[16] =  dataset_cnt;
 1191 
        DebugOut.Analog[16] =  dataset_cnt;
1180 
        DebugOut.Analog[17] =  UBat;
 1192 
        DebugOut.Analog[17] =  UBat;
1181 
        DebugOut.Analog[18] =  MesswertNick;
 1193 
        DebugOut.Analog[18] =  MesswertNick;
1182 
        DebugOut.Analog[19] =  MesswertRoll;
 1194 
        DebugOut.Analog[19] =  MesswertRoll;
1183 
        DebugOut.Analog[20] =  MesswertGier;
 1195 
        DebugOut.Analog[20] =  MesswertGier;
1184 
        DebugOut.Analog[21] =  StickNick;
 1196 
        DebugOut.Analog[21] =  StickNick;
1185 
        DebugOut.Analog[22] =  StickRoll;
 1197 
        DebugOut.Analog[22] =  StickRoll;
1186 
        DebugOut.Analog[23] =  StickGier;
 1198 
        DebugOut.Analog[23] =  StickGier;
1187 
// GPS Debugging
 1199 
// GPS Debugging
1188 
//    DebugOut.Analog[10] = GPS_dist_2trgt/10;
 1200 
//    DebugOut.Analog[10] = GPS_dist_2trgt/10;
1189 
//      DebugOut.Analog[11] = GPS_hdng_rel_2trgt;
 1201 
//      DebugOut.Analog[11] = GPS_hdng_rel_2trgt;
1190 
    DebugOut.Analog[10] = Parameter_UserParam1;
 1202 
    DebugOut.Analog[10] = Parameter_UserParam1;
1191 
        DebugOut.Analog[11] = Parameter_UserParam3;
 1203 
        DebugOut.Analog[11] = Parameter_UserParam3;
1192 
        DebugOut.Analog[24] =  GPS_Nick;
 1204 
        DebugOut.Analog[24] =  GPS_Nick;
1193 
        DebugOut.Analog[25] =  GPS_Roll;
 1205 
        DebugOut.Analog[25] =  GPS_Roll;
1194 
        DebugOut.Analog[26] =  gps_rel_act_position.utm_east; //in 10cm ausgeben
 1206 
        DebugOut.Analog[26] =  gps_rel_act_position.utm_east; //in 10cm ausgeben
1195 
        DebugOut.Analog[27] =  gps_rel_act_position.utm_north;
 1207 
        DebugOut.Analog[27] =  gps_rel_act_position.utm_north;
1196 
        DebugOut.Analog[28] =  gps_rel_act_position.utm_alt;
 1208 
        DebugOut.Analog[28] =  gps_rel_act_position.utm_alt;
1197 
        DebugOut.Analog[29] =  gps_state + (gps_sub_state*10)+(50*gps_cmd);
 1209 
        DebugOut.Analog[29] =  gps_state + (gps_sub_state*10)+(50*gps_cmd);
1198 
        DebugOut.Analog[30] =  GPS_dist_2trgt;
 1210 
        DebugOut.Analog[30] =  GPS_dist_2trgt;
1199 
        DebugOut.Analog[31] =  (int) GyroGier_Comp;
 1211 
        DebugOut.Analog[31] =  (int) GyroGier_Comp;
1200 
/*    DebugOut.Analog[16] = motor_rx[0];
 1212 
/*    DebugOut.Analog[16] = motor_rx[0];
1201 
    DebugOut.Analog[17] = motor_rx[1];
 1213 
    DebugOut.Analog[17] = motor_rx[1];
1202 
    DebugOut.Analog[18] = motor_rx[2];
 1214 
    DebugOut.Analog[18] = motor_rx[2];
1203 
    DebugOut.Analog[19] = motor_rx[3];
 1215 
    DebugOut.Analog[19] = motor_rx[3];
1204 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
 1216 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1205 
    DebugOut.Analog[20] /= 14;
 1217 
    DebugOut.Analog[20] /= 14;
1206 
    DebugOut.Analog[21] = motor_rx[4];
 1218 
    DebugOut.Analog[21] = motor_rx[4];
1207 
    DebugOut.Analog[22] = motor_rx[5];
 1219 
    DebugOut.Analog[22] = motor_rx[5];
1208 
    DebugOut.Analog[23] = motor_rx[6];
 1220 
    DebugOut.Analog[23] = motor_rx[6];
1209 
    DebugOut.Analog[24] = motor_rx[7];
 1221 
    DebugOut.Analog[24] = motor_rx[7];
1210 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 1222 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1211 
*/
 1223 
*/
1212 
//    DebugOut.Analog[9] = MesswertNick;
 1224 
//    DebugOut.Analog[9] = MesswertNick;
1213 
//    DebugOut.Analog[9] = SollHoehe;
 1225 
//    DebugOut.Analog[9] = SollHoehe;
1214 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 1226 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
1215 
//    DebugOut.Analog[11] = KompassStartwert;
 1227 
//    DebugOut.Analog[11] = KompassStartwert;
1216 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 1228 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
1217 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 1229 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
1218 
//    DebugOut.Analog[9] = KompassRichtung;    
 1230 
//    DebugOut.Analog[9] = KompassRichtung;    
1219 
//    DebugOut.Analog[10] = GasMischanteil;
 1231 
//    DebugOut.Analog[10] = GasMischanteil;
1220 
//    DebugOut.Analog[3] = HoeheD * 32;
 1232 
//    DebugOut.Analog[3] = HoeheD * 32;
1221 
//    DebugOut.Analog[4] = hoehenregler;
 1233 
//    DebugOut.Analog[4] = hoehenregler;
1222 
  }
 1234 
  }
1223 
 
 1235 
 
1224 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1236 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1225 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 1237 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1226 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1238 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1227 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 1239 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
1228 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 1240 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
1229 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 1241 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
1230 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 1242 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
1231 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 1243 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
1232 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 1244 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
1233 
 
 1245 
 
1234 
//DebugOut.Analog[28] = MesswertRoll;
 1246 
//DebugOut.Analog[28] = MesswertRoll;
1235 
//DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
 1247 
//DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
1236 
//DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
 1248 
//DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
1237 
 
 1249 
 
1238 
    // Maximalwerte abfangen
 1250 
    // Maximalwerte abfangen
1239 
    #define MAX_SENSOR  2048
 1251 
    #define MAX_SENSOR  2048
1240 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 1252 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1241 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 1253 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1242 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 1254 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1243 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 1255 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1244 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 1256 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
1245 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 1257 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1246 
 
 1258 
 
1247 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1259 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1248 
// Höhenregelung
 1260 
// Höhenregelung
1249 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 1261 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1250 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1262 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1251 
//OCR0B = 180 - (Poti1 + 120) / 4;
 1263 
//OCR0B = 180 - (Poti1 + 120) / 4;
1252 
//DruckOffsetSetting = OCR0B;
 1264 
//DruckOffsetSetting = OCR0B;
1253 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 1265 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
1254 
  {
 1266 
  {
1255 
    int tmp_int;
 1267 
    int tmp_int;
1256 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 1268 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1257 
    {
 1269 
    {
1258 
     if(Parameter_MaxHoehe < 50)
 1270 
     if(Parameter_MaxHoehe < 50)
1259 
      {
 1271 
      {
1260 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 1272 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
1261 
       HoehenReglerAktiv = 0;
 1273 
       HoehenReglerAktiv = 0;
1262 
      }
 1274 
      }
1263 
      else  
 1275 
      else  
1264 
        HoehenReglerAktiv = 1;
 1276 
        HoehenReglerAktiv = 1;
1265 
    }
 1277 
    }
1266 
    else
 1278 
    else
1267 
    {
 1279 
    {
1268 
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
 1280 
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
1269 
     HoehenReglerAktiv = 1;
 1281 
     HoehenReglerAktiv = 1;
1270 
    }
 1282 
    }
1271 
 
 1283 
 
1272 
    if(Notlandung) SollHoehe = 0;
 1284 
    if(Notlandung) SollHoehe = 0;
1273 
    h = HoehenWert;
 1285 
    h = HoehenWert;
1274 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 1286 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
1275 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 1287 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
1276 
      h = GasMischanteil - h;         // vom Gas abziehen
 1288 
      h = GasMischanteil - h;         // vom Gas abziehen
1277 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 1289 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
1278 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 1290 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
1279 
      if(tmp_int > 50) tmp_int = 50;
 1291 
      if(tmp_int > 50) tmp_int = 50;
1280 
      else if(tmp_int < -50) tmp_int = -50;
 1292 
      else if(tmp_int < -50) tmp_int = -50;
1281 
      h -= tmp_int;
 1293 
      h -= tmp_int;
1282 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 1294 
      hoehenregler = (hoehenregler*15 + h) / 16;      
1283 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 1295 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
1284 
       {
 1296 
       {
1285 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 1297 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
1286 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 1298 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
1287 
       }  
 1299 
       }  
1288 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 1300 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
1289 
      GasMischanteil = hoehenregler;
 1301 
      GasMischanteil = hoehenregler;
1290 
     }
 1302 
     }
1291 
  }
 1303 
  }
1292 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1304 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1293 
// + Mischer und PI-Regler
 1305 
// + Mischer und PI-Regler
1294 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1306 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1295 
  DebugOut.Analog[7] = GasMischanteil;
 1307 
  DebugOut.Analog[7] = GasMischanteil;
1296 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1308 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1297 
// Gier-Anteil
 1309 
// Gier-Anteil
1298 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1310 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1299 
#define MUL_G  1.0
 1311 
#define MUL_G  1.0
1300 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 1312 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
1301 
//GierMischanteil = 0;
 1313 
//GierMischanteil = 0;
1302 
 
 1314 
 
1303 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
 1315 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
1304 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
 1316 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
1305 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
 1317 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
1306 
    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
 1318 
    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
1307 
 
 1319 
 
1308 
    if(GasMischanteil < 20) GierMischanteil = 0;
 1320 
    if(GasMischanteil < 20) GierMischanteil = 0;
1309 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1321 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1310 
// Nick-Achse
 1322 
// Nick-Achse
1311 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1323 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1312 
    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
 1324 
    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
1313 
    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
 1325 
    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
1314 
    else  SummeNick += DiffNick; // I-Anteil bei HH
1326 
    else  SummeNick += DiffNick; // I-Anteil bei HH
1315 
    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
 1327 
    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
1316 
    if(SummeNick >  16000) SummeNick =  16000;
 1328 
    if(SummeNick >  16000) SummeNick =  16000;
1317 
    if(SummeNick < -16000) SummeNick = -16000;
 1329 
    if(SummeNick < -16000) SummeNick = -16000;
1318 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1330 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1319 
    // Motor Vorn
 1331 
    // Motor Vorn
1320 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1332 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1321 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
 1333 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1322 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1334 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1323 
 
 1335 
 
1324 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 1336 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
1325 
        if ((motorwert < 0)) motorwert = 0;
 1337 
        if ((motorwert < 0)) motorwert = 0;
1326 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1338 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
1327 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1339 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1328 
        Motor_Vorne = motorwert;           
 1340 
        Motor_Vorne = motorwert;           
1329 
    // Motor Heck
 1341 
    // Motor Heck
1330 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 1342 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
1331 
        if ((motorwert < 0)) motorwert = 0;
 1343 
        if ((motorwert < 0)) motorwert = 0;
1332 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1344 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
1333 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1345 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1334 
        Motor_Hinten = motorwert;              
 1346 
        Motor_Hinten = motorwert;              
1335 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1347 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1336 
// Roll-Achse
 1348 
// Roll-Achse
1337 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1349 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1338 
        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
 1350 
        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
1339 
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
 1351 
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
1340 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
 1352 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1341 
    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
 1353 
    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
1342 
    if(SummeRoll >  16000) SummeRoll =  16000;
 1354 
    if(SummeRoll >  16000) SummeRoll =  16000;
1343 
    if(SummeRoll < -16000) SummeRoll = -16000;
 1355 
    if(SummeRoll < -16000) SummeRoll = -16000;
1344 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 1356 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
1345 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1357 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1346 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
 1358 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1347 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1359 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1348 
    // Motor Links
 1360 
    // Motor Links
1349 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1361 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1350 
        if ((motorwert < 0)) motorwert = 0;
 1362 
        if ((motorwert < 0)) motorwert = 0;
1351 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1363 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1352 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1364 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1353 
        Motor_Links = motorwert;               
 1365 
        Motor_Links = motorwert;               
1354 
    // Motor Rechts
 1366 
    // Motor Rechts
1355 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1367 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
1356 
        if ((motorwert < 0)) motorwert = 0;
 1368 
        if ((motorwert < 0)) motorwert = 0;
1357 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1369 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1358 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1370 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1359 
        Motor_Rechts = motorwert;
 1371 
        Motor_Rechts = motorwert;
1360 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1372 
   // +++++++++++++++++++++++++++++++++++++++++++++++
1361 
}
 1373 
}
1362 
 
 1374 
 
1363 
 
 1375