Subversion Repositories FlightCtrl

Rev

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

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