Subversion Repositories FlightCtrl

Rev

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

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