Subversion Repositories FlightCtrl

Rev

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

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