Subversion Repositories FlightCtrl

Rev

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

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