Subversion Repositories FlightCtrl

Rev

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

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