Subversion Repositories FlightCtrl

Rev

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

Rev 1154 Rev 1157
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 
 
 54 
 
55 
#include "main.h"
 55 
#include "main.h"
56 
#include "parameter.h"
 56 
#include "parameter.h"
57 
#include "pitch.h"
 57 
#include "pitch.h"
58 
#include "altcon.h"
 58 
#include "altcon.h"
59 
#include "eeprom.c"
 59 
#include "eeprom.c"
60 
 
 60 
 
61 
unsigned char h, m, s;
 61 
unsigned char h, m, s;
62 
volatile unsigned int I2CTimeout = 100;
 62 
volatile unsigned int I2CTimeout = 100;
63 
volatile int MesswertNick, MesswertRoll, MesswertGier, MesswertGierBias;
 63 
volatile int MesswertNick, MesswertRoll, MesswertGier, MesswertGierBias;
64 
int AdNeutralGierBias;
 64 
int AdNeutralGierBias;
65 
int AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralGier = 0, StartNeutralRoll = 0, StartNeutralNick = 0;
 65 
int AdNeutralNick = 0, AdNeutralRoll = 0, AdNeutralGier = 0, StartNeutralRoll = 0, StartNeutralNick = 0;
66 
int Mittelwert_AccNick, Mittelwert_AccRoll, Mittelwert_AccHoch, NeutralAccX = 0, NeutralAccY = 0;
 66 
int Mittelwert_AccNick, Mittelwert_AccRoll, Mittelwert_AccHoch, NeutralAccX = 0, NeutralAccY = 0;
67 
int NaviAccNick, NaviAccRoll, NaviCntAcc = 0;
 67 
int NaviAccNick, NaviAccRoll, NaviCntAcc = 0;
68 
volatile float NeutralAccZ = 0;
 68 
volatile float NeutralAccZ = 0;
69 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 69 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
70 
long IntegralNick = 0, IntegralNick2 = 0;
 70 
long IntegralNick = 0, IntegralNick2 = 0;
71 
long IntegralRoll = 0, IntegralRoll2 = 0;
 71 
long IntegralRoll = 0, IntegralRoll2 = 0;
72 
long IntegralAccNick = 0, IntegralAccRoll = 0, IntegralAccZ = 0;
 72 
long IntegralAccNick = 0, IntegralAccRoll = 0, IntegralAccZ = 0;
73 
long Integral_Gier = 0;
 73 
long Integral_Gier = 0;
74 
long Mess_IntegralNick = 0, Mess_IntegralNick2 = 0;
 74 
long Mess_IntegralNick = 0, Mess_IntegralNick2 = 0;
75 
long Mess_IntegralRoll = 0, Mess_IntegralRoll2 = 0;
 75 
long Mess_IntegralRoll = 0, Mess_IntegralRoll2 = 0;
76 
long Mess_Integral_Gier = 0, Mess_Integral_Gier2 = 0;
 76 
long Mess_Integral_Gier = 0, Mess_Integral_Gier2 = 0;
77 
long MittelIntegralNick, MittelIntegralRoll, MittelIntegralNick2, MittelIntegralRoll2;
 77 
long MittelIntegralNick, MittelIntegralRoll, MittelIntegralNick2, MittelIntegralRoll2;
78 
volatile long Mess_Integral_Hoch = 0;
 78 
volatile long Mess_Integral_Hoch = 0;
79 
volatile int KompassValue = 0;
 79 
volatile int KompassValue = 0;
80 
volatile int KompassStartwert = 0;
 80 
volatile int KompassStartwert = 0;
81 
volatile int KompassRichtung = 0;
 81 
volatile int KompassRichtung = 0;
82 
unsigned int KompassSignalSchlecht = 500;
 82 
unsigned int KompassSignalSchlecht = 500;
83 
unsigned char MAX_GAS, MIN_GAS;
 83 
unsigned char MAX_GAS, MIN_GAS;
84 
unsigned char Notlandung = 0;
 84 
unsigned char Notlandung = 0;
85 
unsigned char HoehenReglerAktiv = 0;
 85 
unsigned char HoehenReglerAktiv = 0;
86 
unsigned char TrichterFlug = 0;
 86 
unsigned char TrichterFlug = 0;
87 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 87 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
88 
long ErsatzKompass;
 88 
long ErsatzKompass;
89 
int ErsatzKompassInGrad; // Kompasswert in Grad
 89 
int ErsatzKompassInGrad; // Kompasswert in Grad
90 
int GierGyroFehler = 0;
 90 
int GierGyroFehler = 0;
91 
float GyroFaktor;
 91 
float GyroFaktor;
92 
float IntegralFaktor;
 92 
float IntegralFaktor;
93 
volatile int DiffNick, DiffRoll;
 93 
volatile int DiffNick, DiffRoll;
94 
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 94 
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
95 
volatile unsigned char Motor_Vorne, Motor_Hinten, Motor_Rechts, Motor_Links, Count;
 95 
volatile unsigned char Motor_Vorne, Motor_Hinten, Motor_Rechts, Motor_Links, Count;
96 
volatile unsigned char SenderOkay = 0;
 96 
volatile unsigned char SenderOkay = 0;
97 
int StickNick = 0, StickRoll = 0, StickGier = 0, StickGas = 0;
 97 
int StickNick = 0, StickRoll = 0, StickGier = 0, StickGas = 0;
98 
char MotorenEin = 0;
 98 
char MotorenEin = 0;
99 
int HoehenWert = 0;
 99 
int HoehenWert = 0;
100 
int SollHoehe = 0;
 100 
int SollHoehe = 0;
101 
int LageKorrekturRoll = 0, LageKorrekturNick = 0;
 101 
int LageKorrekturRoll = 0, LageKorrekturNick = 0;
102 
float Ki = FAKTOR_I;
 102 
float Ki = FAKTOR_I;
103 
unsigned char Looping_Nick = 0, Looping_Roll = 0;
 103 
unsigned char Looping_Nick = 0, Looping_Roll = 0;
104 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 104 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
105 
 
 105 
 
106 
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
 106 
unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250
107 
unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250
 107 
unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250
108 
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
 108 
unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32
109 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 109 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
110 
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
 110 
unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250
111 
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
 111 
unsigned char Parameter_Gyro_P = 150; // Wert : 10-250
112 
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
 112 
unsigned char Parameter_Gyro_I = 150; // Wert : 0-250
113 
unsigned char Parameter_Gier_P = 2; // Wert : 1-20
 113 
unsigned char Parameter_Gier_P = 2; // Wert : 1-20
114 
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
 114 
unsigned char Parameter_I_Faktor = 10; // Wert : 1-20
115 
unsigned char Parameter_UserParam1 = 0;
 115 
unsigned char Parameter_UserParam1 = 0;
116 
unsigned char Parameter_UserParam2 = 0;
 116 
unsigned char Parameter_UserParam2 = 0;
117 
unsigned char Parameter_UserParam3 = 0;
 117 
unsigned char Parameter_UserParam3 = 0;
118 
unsigned char Parameter_UserParam4 = 0;
 118 
unsigned char Parameter_UserParam4 = 0;
119 
unsigned char Parameter_UserParam5 = 0;
 119 
unsigned char Parameter_UserParam5 = 0;
120 
unsigned char Parameter_UserParam6 = 0;
 120 
unsigned char Parameter_UserParam6 = 0;
121 
unsigned char Parameter_UserParam7 = 0;
 121 
unsigned char Parameter_UserParam7 = 0;
122 
unsigned char Parameter_UserParam8 = 0;
 122 
unsigned char Parameter_UserParam8 = 0;
123 
unsigned char Parameter_ServoNickControl = 100;
 123 
unsigned char Parameter_ServoNickControl = 100;
124 
unsigned char Parameter_LoopGasLimit = 70;
 124 
unsigned char Parameter_LoopGasLimit = 70;
125 
unsigned char Parameter_AchsKopplung1 = 0;
 125 
unsigned char Parameter_AchsKopplung1 = 0;
126 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 126 
unsigned char Parameter_AchsGegenKopplung1 = 0;
127 
unsigned char Parameter_DynamicStability = 100;
 127 
unsigned char Parameter_DynamicStability = 100;
128 
unsigned char Parameter_J16Bitmask; // for the J16 Output
 128 
unsigned char Parameter_J16Bitmask; // for the J16 Output
129 
unsigned char Parameter_J16Timing; // for the J16 Output
 129 
unsigned char Parameter_J16Timing; // for the J16 Output
130 
unsigned char Parameter_J16Brightness; // for the J16 Output
 130 
unsigned char Parameter_J16Brightness; // for the J16 Output
131 
unsigned char Parameter_J17Bitmask; // for the J17 Output
 131 
unsigned char Parameter_J17Bitmask; // for the J17 Output
132 
unsigned char Parameter_J17Timing; // for the J17 Output
 132 
unsigned char Parameter_J17Timing; // for the J17 Output
133 
unsigned char Parameter_J17Brightness; // for the J17 Output
 133 
unsigned char Parameter_J17Brightness; // for the J17 Output
134 
unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
 134 
unsigned char Parameter_NaviGpsModeControl; // Parameters for the Naviboard
135 
unsigned char Parameter_NaviGpsGain;
 135 
unsigned char Parameter_NaviGpsGain;
136 
unsigned char Parameter_NaviGpsP;
 136 
unsigned char Parameter_NaviGpsP;
137 
unsigned char Parameter_NaviGpsI;
 137 
unsigned char Parameter_NaviGpsI;
138 
unsigned char Parameter_NaviGpsD;
 138 
unsigned char Parameter_NaviGpsD;
139 
unsigned char Parameter_NaviGpsACC;
 139 
unsigned char Parameter_NaviGpsACC;
140 
unsigned char Parameter_NaviOperatingRadius;
 140 
unsigned char Parameter_NaviOperatingRadius;
141 
unsigned char Parameter_NaviWindCorrection;
 141 
unsigned char Parameter_NaviWindCorrection;
142 
unsigned char Parameter_NaviSpeedCompensation;
 142 
unsigned char Parameter_NaviSpeedCompensation;
143 
unsigned char Parameter_ExternalControl;
 143 
unsigned char Parameter_ExternalControl;
144 
struct mk_param_struct EE_Parameter;
 144 
struct mk_param_struct EE_Parameter;
145 
signed int ExternStickNick = 0, ExternStickRoll = 0, ExternStickGier = 0, ExternHoehenValue = -20;
 145 
signed int ExternStickNick = 0, ExternStickRoll = 0, ExternStickGier = 0, ExternHoehenValue = -20;
146 
int MaxStickNick = 0, MaxStickRoll = 0;
 146 
int MaxStickNick = 0, MaxStickRoll = 0;
147 
unsigned int modell_fliegt = 0;
 147 
unsigned int modell_fliegt = 0;
148 
unsigned char MikroKopterFlags = 0;
 148 
unsigned char MikroKopterFlags = 0;
149 
 
 149 
 
150 
void Piep(unsigned char Anzahl) {
 150 
void Piep(unsigned char Anzahl) {
151 
    while (Anzahl--) {
 151 
    while (Anzahl--) {
152 
        if (MotorenEin) return; //auf keinen Fall im Flug!
 152 
        if (MotorenEin) return; //auf keinen Fall im Flug!
153 
        beeptime = 100;
 153 
        beeptime = 100;
154 
        Delay_ms(250);
 154 
        Delay_ms(250);
155 
    }
 155 
    }
156 
}
 156 
}
157 
 
 157 
 
158 
//############################################################################
 158 
//############################################################################
159 
//  Nullwerte ermitteln
 159 
//  Nullwerte ermitteln
160 
 
 160 
 
161 
void SetNeutral(void)
 161 
void SetNeutral(void)
162 
//############################################################################
 162 
//############################################################################
163 
{
 163 
{
164 
    NeutralAccX = 0;
 164 
    NeutralAccX = 0;
165 
    NeutralAccY = 0;
 165 
    NeutralAccY = 0;
166 
    NeutralAccZ = 0;
 166 
    NeutralAccZ = 0;
167 
    AdNeutralNick = 0;
 167 
    AdNeutralNick = 0;
168 
    AdNeutralRoll = 0;
 168 
    AdNeutralRoll = 0;
169 
    AdNeutralGier = 0;
 169 
    AdNeutralGier = 0;
170 
    AdNeutralGierBias = 0;
 170 
    AdNeutralGierBias = 0;
171 
    Parameter_AchsKopplung1 = 0;
 171 
    Parameter_AchsKopplung1 = 0;
172 
    Parameter_AchsGegenKopplung1 = 0;
 172 
    Parameter_AchsGegenKopplung1 = 0;
173 
    CalibrierMittelwert();
 173 
    CalibrierMittelwert();
174 
    Delay_ms_Mess(100);
 174 
    Delay_ms_Mess(100);
175 
    CalibrierMittelwert();
 175 
    CalibrierMittelwert();
176 
    if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
 176 
    if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
177 
    {
 177 
    {
178 
        if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 178 
        if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
179 
    }
 179 
    }
180 
 
 180 
 
181 
    AdNeutralNick = AdWertNick;
 181 
    AdNeutralNick = AdWertNick;
182 
    AdNeutralRoll = AdWertRoll;
 182 
    AdNeutralRoll = AdWertRoll;
183 
    AdNeutralGier = AdWertGier;
 183 
    AdNeutralGier = AdWertGier;
184 
    AdNeutralGierBias = AdWertGier;
 184 
    AdNeutralGierBias = AdWertGier;
185 
    StartNeutralRoll = AdNeutralRoll;
 185 
    StartNeutralRoll = AdNeutralRoll;
186 
    StartNeutralNick = AdNeutralNick;
 186 
    StartNeutralNick = AdNeutralNick;
187 
    if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) {
 187 
    if (eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4) {
188 
        NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 188 
        NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
189 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 189 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
190 
        NeutralAccZ = Aktuell_az;
 190 
        NeutralAccZ = Aktuell_az;
191 
    } else {
 191 
    } else {
192 
        NeutralAccX = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK + 1]);
 192 
        NeutralAccX = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK + 1]);
193 
        NeutralAccY = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL + 1]);
 193 
        NeutralAccY = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL + 1]);
194 
        NeutralAccZ = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z + 1]);
 194 
        NeutralAccZ = (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int) eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z + 1]);
195 
    }
 195 
    }
196 
 
 196 
 
197 
    Mess_IntegralNick = 0;
 197 
    Mess_IntegralNick = 0;
198 
    Mess_IntegralNick2 = 0;
 198 
    Mess_IntegralNick2 = 0;
199 
    Mess_IntegralRoll = 0;
 199 
    Mess_IntegralRoll = 0;
200 
    Mess_IntegralRoll2 = 0;
 200 
    Mess_IntegralRoll2 = 0;
201 
    Mess_Integral_Gier = 0;
 201 
    Mess_Integral_Gier = 0;
202 
    MesswertNick = 0;
 202 
    MesswertNick = 0;
203 
    MesswertRoll = 0;
 203 
    MesswertRoll = 0;
204 
    MesswertGier = 0;
 204 
    MesswertGier = 0;
205 
    Delay_ms_Mess(100);
 205 
    Delay_ms_Mess(100);
206 
    StartLuftdruck = Luftdruck;
 206 
    StartLuftdruck = Luftdruck;
207 
    HoeheD = 0;
 207 
    HoeheD = 0;
208 
    Mess_Integral_Hoch = 0;
 208 
    Mess_Integral_Hoch = 0;
209 
    KompassStartwert = KompassValue;
 209 
    KompassStartwert = KompassValue;
210 
    GPS_Neutral();
 210 
    GPS_Neutral();
211 
    beeptime = 50;
 211 
    beeptime = 50;
212 
    Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
 212 
    Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
213 
    Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
 213 
    Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
214 
    ExternHoehenValue = 0;
 214 
    ExternHoehenValue = 0;
215 
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
 215 
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
216 
    GierGyroFehler = 0;
 216 
    GierGyroFehler = 0;
217 
    SendVersionToNavi = 1;
 217 
    SendVersionToNavi = 1;
218 
    LED_Init();
 218 
    LED_Init();
219 
    MikroKopterFlags |= FLAG_CALIBRATE;
 219 
    MikroKopterFlags |= FLAG_CALIBRATE;
220 
    FromNaviCtrl_Value.Kalman_K = -1;
 220 
    FromNaviCtrl_Value.Kalman_K = -1;
221 
    FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
 221 
    FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
222 
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
 222 
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
223 
}
 223 
}
224 
 
 224 
 
225 
static inline void LesePotis(void) {
 225 
static inline void LesePotis(void) {
226 
    /*  Warum 110? Knüppel geht von -125 bis 125!
 226 
    /*  Warum 110? Knüppel geht von -125 bis 125!
227 
        if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 227 
        if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
228 
        if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 228 
        if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
229 
        if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 229 
        if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
230 
        if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 230 
        if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
231 
     */
 231 
     */
232 
    if (Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125) Poti1++;
 232 
    if (Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125) Poti1++;
233 
    else if (Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125 && Poti1) Poti1--;
 233 
    else if (Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 125 && Poti1) Poti1--;
234 
    if (Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125) Poti2++;
 234 
    if (Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125) Poti2++;
235 
    else if (Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125 && Poti2) Poti2--;
 235 
    else if (Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 125 && Poti2) Poti2--;
236 
    if (Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125) Poti3++;
 236 
    if (Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125) Poti3++;
237 
    else if (Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125 && Poti3) Poti3--;
 237 
    else if (Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 125 && Poti3) Poti3--;
238 
    if (Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125) Poti4++;
 238 
    if (Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125) Poti4++;
239 
    else if (Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125 && Poti4) Poti4--;
 239 
    else if (Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 125 && Poti4) Poti4--;
240 
    if (Poti1 < 0) Poti1 = 0;
 240 
    if (Poti1 < 0) Poti1 = 0;
241 
    else if (Poti1 > 255) Poti1 = 255;
 241 
    else if (Poti1 > 255) Poti1 = 255;
242 
    if (Poti2 < 0) Poti2 = 0;
 242 
    if (Poti2 < 0) Poti2 = 0;
243 
    else if (Poti2 > 255) Poti2 = 255;
 243 
    else if (Poti2 > 255) Poti2 = 255;
244 
    if (Poti3 < 0) Poti3 = 0;
 244 
    if (Poti3 < 0) Poti3 = 0;
245 
    else if (Poti3 > 255) Poti3 = 255;
 245 
    else if (Poti3 > 255) Poti3 = 255;
246 
    if (Poti4 < 0) Poti4 = 0;
 246 
    if (Poti4 < 0) Poti4 = 0;
247 
    else if (Poti4 > 255) Poti4 = 255;
 247 
    else if (Poti4 > 255) Poti4 = 255;
248 
}
 248 
}
249 
 
 249 
 
250 
//############################################################################
 250 
//############################################################################
251 
// Bearbeitet die Messwerte
 251 
// Bearbeitet die Messwerte
252 
 
 252 
 
253 
void Mittelwert(void)
 253 
void Mittelwert(void)
254 
//############################################################################
 254 
//############################################################################
255 
{
 255 
{
256 
    static signed long tmpl, tmpl2;
 256 
    static signed long tmpl, tmpl2;
257 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 257 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
258 
    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
 258 
    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
259 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 259 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
260 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 260 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
261 
 
 261 
 
262 
    //DebugOut.Analog[26] = MesswertNick;
 262 
    //DebugOut.Analog[26] = MesswertNick;
263 
    // DebugOut.Analog[28] = MesswertRoll;
 263 
    // DebugOut.Analog[28] = MesswertRoll;
264 
 
 264 
 
265 
    // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
 265 
    // Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
266 
    Mittelwert_AccNick = ((long) Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long) AdWertAccNick))) / 2L;
 266 
    Mittelwert_AccNick = ((long) Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long) AdWertAccNick))) / 2L;
267 
    Mittelwert_AccRoll = ((long) Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long) AdWertAccRoll))) / 2L;
 267 
    Mittelwert_AccRoll = ((long) Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long) AdWertAccRoll))) / 2L;
268 
    Mittelwert_AccHoch = ((long) Mittelwert_AccHoch * 1 + ((long) AdWertAccHoch)) / 2L;
 268 
    Mittelwert_AccHoch = ((long) Mittelwert_AccHoch * 1 + ((long) AdWertAccHoch)) / 2L;
269 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
 269 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
270 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
 270 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
271 
    NaviAccNick += AdWertAccNick;
 271 
    NaviAccNick += AdWertAccNick;
272 
    NaviAccRoll += AdWertAccRoll;
 272 
    NaviAccRoll += AdWertAccRoll;
273 
    NaviCntAcc++;
 273 
    NaviCntAcc++;
274 
    IntegralAccZ += Aktuell_az - NeutralAccZ;
 274 
    IntegralAccZ += Aktuell_az - NeutralAccZ;
275 
    // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
 275 
    // Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
276 
    ErsatzKompass += MesswertGier;
 276 
    ErsatzKompass += MesswertGier;
277 
    Mess_Integral_Gier += MesswertGier;
 277 
    Mess_Integral_Gier += MesswertGier;
278 
    //            Mess_Integral_Gier2 += MesswertGier;
 278 
    //            Mess_Integral_Gier2 += MesswertGier;
279 
    if (ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
 279 
    if (ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag
280 
    if (ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 280 
    if (ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
281 
    // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 281 
    // Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
282 
    if (!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) {
 282 
    if (!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) {
283 
        tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
 283 
        tmpl = (MesswertGierBias * Mess_IntegralNick) / 2048L;
284 
        tmpl *= Parameter_AchsKopplung1; //125
 284 
        tmpl *= Parameter_AchsKopplung1; //125
285 
        tmpl /= 4096L;
 285 
        tmpl /= 4096L;
286 
        tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
 286 
        tmpl2 = (MesswertGierBias * Mess_IntegralRoll) / 2048L;
287 
        tmpl2 *= Parameter_AchsKopplung1;
 287 
        tmpl2 *= Parameter_AchsKopplung1;
288 
        tmpl2 /= 4096L;
 288 
        tmpl2 /= 4096L;
289 
        if (labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
 289 
        if (labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
290 
    } else tmpl = tmpl2 = 0;
 290 
    } else tmpl = tmpl2 = 0;
291 
    // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
 291 
    // Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
292 
    MesswertRoll += tmpl;
 292 
    MesswertRoll += tmpl;
293 
    MesswertRoll += (tmpl2 * Parameter_AchsGegenKopplung1) / 512L; //109
 293 
    MesswertRoll += (tmpl2 * Parameter_AchsGegenKopplung1) / 512L; //109
294 
    Mess_IntegralRoll2 += MesswertRoll;
 294 
    Mess_IntegralRoll2 += MesswertRoll;
295 
    Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
 295 
    Mess_IntegralRoll += MesswertRoll - LageKorrekturRoll;
296 
    if (Mess_IntegralRoll > Umschlag180Roll) {
 296 
    if (Mess_IntegralRoll > Umschlag180Roll) {
297 
        Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
 297 
        Mess_IntegralRoll = -(Umschlag180Roll - 25000L);
298 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
 298 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
299 
    }
 299 
    }
300 
    if (Mess_IntegralRoll <-Umschlag180Roll) {
 300 
    if (Mess_IntegralRoll <-Umschlag180Roll) {
301 
        Mess_IntegralRoll = (Umschlag180Roll - 25000L);
 301 
        Mess_IntegralRoll = (Umschlag180Roll - 25000L);
302 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
 302 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
303 
    }
 303 
    }
304 
    if (AdWertRoll < 15) MesswertRoll = -1000;
 304 
    if (AdWertRoll < 15) MesswertRoll = -1000;
305 
    if (AdWertRoll < 7) MesswertRoll = -2000;
 305 
    if (AdWertRoll < 7) MesswertRoll = -2000;
306 
    if (PlatinenVersion == 10) {
 306 
    if (PlatinenVersion == 10) {
307 
        if (AdWertRoll > 1010) MesswertRoll = +1000;
 307 
        if (AdWertRoll > 1010) MesswertRoll = +1000;
308 
        if (AdWertRoll > 1017) MesswertRoll = +2000;
 308 
        if (AdWertRoll > 1017) MesswertRoll = +2000;
309 
    } else {
 309 
    } else {
310 
        if (AdWertRoll > 2020) MesswertRoll = +1000;
 310 
        if (AdWertRoll > 2020) MesswertRoll = +1000;
311 
        if (AdWertRoll > 2034) MesswertRoll = +2000;
 311 
        if (AdWertRoll > 2034) MesswertRoll = +2000;
312 
    }
 312 
    }
313 
    // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
 313 
    // Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
314 
    MesswertNick -= tmpl2;
 314 
    MesswertNick -= tmpl2;
315 
    MesswertNick -= (tmpl * Parameter_AchsGegenKopplung1) / 512L;
 315 
    MesswertNick -= (tmpl * Parameter_AchsGegenKopplung1) / 512L;
316 
    Mess_IntegralNick2 += MesswertNick;
 316 
    Mess_IntegralNick2 += MesswertNick;
317 
    Mess_IntegralNick += MesswertNick - LageKorrekturNick;
 317 
    Mess_IntegralNick += MesswertNick - LageKorrekturNick;
318 
 
 318 
 
319 
    if (Mess_IntegralNick > Umschlag180Nick) {
 319 
    if (Mess_IntegralNick > Umschlag180Nick) {
320 
        Mess_IntegralNick = -(Umschlag180Nick - 25000L);
 320 
        Mess_IntegralNick = -(Umschlag180Nick - 25000L);
321 
        Mess_IntegralNick2 = Mess_IntegralNick;
 321 
        Mess_IntegralNick2 = Mess_IntegralNick;
322 
    }
 322 
    }
323 
    if (Mess_IntegralNick <-Umschlag180Nick) {
 323 
    if (Mess_IntegralNick <-Umschlag180Nick) {
324 
        Mess_IntegralNick = (Umschlag180Nick - 25000L);
 324 
        Mess_IntegralNick = (Umschlag180Nick - 25000L);
325 
        Mess_IntegralNick2 = Mess_IntegralNick;
 325 
        Mess_IntegralNick2 = Mess_IntegralNick;
326 
    }
 326 
    }
327 
    if (AdWertNick < 15) MesswertNick = -1000;
 327 
    if (AdWertNick < 15) MesswertNick = -1000;
328 
    if (AdWertNick < 7) MesswertNick = -2000;
 328 
    if (AdWertNick < 7) MesswertNick = -2000;
329 
    if (PlatinenVersion == 10) {
 329 
    if (PlatinenVersion == 10) {
330 
        if (AdWertNick > 1010) MesswertNick = +1000;
 330 
        if (AdWertNick > 1010) MesswertNick = +1000;
331 
        if (AdWertNick > 1017) MesswertNick = +2000;
 331 
        if (AdWertNick > 1017) MesswertNick = +2000;
332 
    } else {
 332 
    } else {
333 
        if (AdWertNick > 2020) MesswertNick = +1000;
 333 
        if (AdWertNick > 2020) MesswertNick = +1000;
334 
        if (AdWertNick > 2034) MesswertNick = +2000;
 334 
        if (AdWertNick > 2034) MesswertNick = +2000;
335 
    }
 335 
    }
336 
    //++++++++++++++++++++++++++++++++++++++++++++++++
 336 
    //++++++++++++++++++++++++++++++++++++++++++++++++
337 
    // ADC einschalten
 337 
    // ADC einschalten
338 
    ANALOG_ON;
 338 
    ANALOG_ON;
339 
    //++++++++++++++++++++++++++++++++++++++++++++++++
 339 
    //++++++++++++++++++++++++++++++++++++++++++++++++
340 
 
 340 
 
341 
    Integral_Gier = Mess_Integral_Gier;
 341 
    Integral_Gier = Mess_Integral_Gier;
342 
    IntegralNick = Mess_IntegralNick;
 342 
    IntegralNick = Mess_IntegralNick;
343 
    IntegralRoll = Mess_IntegralRoll;
 343 
    IntegralRoll = Mess_IntegralRoll;
344 
    IntegralNick2 = Mess_IntegralNick2;
 344 
    IntegralNick2 = Mess_IntegralNick2;
345 
    IntegralRoll2 = Mess_IntegralRoll2;
 345 
    IntegralRoll2 = Mess_IntegralRoll2;
346 
 
 346 
 
347 
    if (EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) {
 347 
    if (EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) {
348 
        if (MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
 348 
        if (MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200);
349 
        else if (MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 349 
        else if (MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
350 
        if (MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
 350 
        if (MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200);
351 
        else if (MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 351 
        else if (MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
352 
    }
 352 
    }
353 
    LesePotis();
 353 
    LesePotis();
354 
}
 354 
}
355 
 
 355 
 
356 
//############################################################################
 356 
//############################################################################
357 
// Messwerte beim Ermitteln der Nullage
 357 
// Messwerte beim Ermitteln der Nullage
358 
 
 358 
 
359 
void CalibrierMittelwert(void)
 359 
void CalibrierMittelwert(void)
360 
//############################################################################
 360 
//############################################################################
361 
{
 361 
{
362 
    if (PlatinenVersion == 13) SucheGyroOffset();
 362 
    if (PlatinenVersion == 13) SucheGyroOffset();
363 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 363 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
364 
    ANALOG_OFF;
 364 
    ANALOG_OFF;
365 
    MesswertNick = AdWertNick;
 365 
    MesswertNick = AdWertNick;
366 
    MesswertRoll = AdWertRoll;
 366 
    MesswertRoll = AdWertRoll;
367 
    MesswertGier = AdWertGier;
 367 
    MesswertGier = AdWertGier;
368 
    Mittelwert_AccNick = ACC_AMPLIFY * (long) AdWertAccNick;
 368 
    Mittelwert_AccNick = ACC_AMPLIFY * (long) AdWertAccNick;
369 
    Mittelwert_AccRoll = ACC_AMPLIFY * (long) AdWertAccRoll;
 369 
    Mittelwert_AccRoll = ACC_AMPLIFY * (long) AdWertAccRoll;
370 
    Mittelwert_AccHoch = (long) AdWertAccHoch;
 370 
    Mittelwert_AccHoch = (long) AdWertAccHoch;
371 
    // ADC einschalten
 371 
    // ADC einschalten
372 
    ANALOG_ON;
 372 
    ANALOG_ON;
373 
 
 373 
 
374 
    LesePotis();
 374 
    LesePotis();
375 
 
 375 
 
376 
    Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 376 
    Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
377 
    Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
 377 
    Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
378 
}
 378 
}
379 
 
 379 
 
380 
//############################################################################
 380 
//############################################################################
381 
// Senden der Motorwerte per I2C-Bus
 381 
// Senden der Motorwerte per I2C-Bus
382 
 
 382 
 
383 
void SendMotorData(void)
 383 
void SendMotorData(void)
384 
//############################################################################
 384 
//############################################################################
385 
{
 385 
{
386 
    DebugOut.Analog[12] = Motor_Vorne;
 386 
    DebugOut.Analog[12] = Motor_Vorne;
387 
    DebugOut.Analog[13] = Motor_Hinten;
 387 
    DebugOut.Analog[13] = Motor_Hinten;
388 
    DebugOut.Analog[14] = Motor_Links;
 388 
    DebugOut.Analog[14] = Motor_Links;
389 
    DebugOut.Analog[15] = Motor_Rechts;
 389 
    DebugOut.Analog[15] = Motor_Rechts;
390 
 
 390 
 
391 
    if (!(MotorenEin && PARAM_ENGINE_ENABLED)) {
 391 
    if (!(MotorenEin && PARAM_ENGINE_ENABLED)) {
392 
        Motor_Hinten = 0;
 392 
        Motor_Hinten = 0;
393 
        Motor_Vorne = 0;
 393 
        Motor_Vorne = 0;
394 
        Motor_Rechts = 0;
 394 
        Motor_Rechts = 0;
395 
        Motor_Links = 0;
 395 
        Motor_Links = 0;
396 
        if (MotorTest[0]) Motor_Vorne = MotorTest[0];
 396 
        if (MotorTest[0]) Motor_Vorne = MotorTest[0];
397 
        if (MotorTest[1]) Motor_Hinten = MotorTest[1];
 397 
        if (MotorTest[1]) Motor_Hinten = MotorTest[1];
398 
        if (MotorTest[2]) Motor_Links = MotorTest[2];
 398 
        if (MotorTest[2]) Motor_Links = MotorTest[2];
399 
        if (MotorTest[3]) Motor_Rechts = MotorTest[3];
 399 
        if (MotorTest[3]) Motor_Rechts = MotorTest[3];
400 
        MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
 400 
        MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
401 
    } else MikroKopterFlags |= FLAG_MOTOR_RUN;
 401 
    } else MikroKopterFlags |= FLAG_MOTOR_RUN;
402 
 
 402 
 
403 
    //Start I2C Interrupt Mode
 403 
    //Start I2C Interrupt Mode
404 
    twi_state = 0;
 404 
    twi_state = 0;
405 
    motor = 0;
 405 
    motor = 0;
406 
    i2c_start();
 406 
    i2c_start();
407 
}
 407 
}
408 
 
 408 
 
409 
 
 409 
 
410 
 
 410 
 
411 
//############################################################################
 411 
//############################################################################
412 
// Trägt ggf. das Poti als Parameter ein
 412 
// Trägt ggf. das Poti als Parameter ein
413 
 
 413 
 
414 
void ParameterZuordnung(void)
 414 
void ParameterZuordnung(void)
415 
//############################################################################
 415 
//############################################################################
416 
{
 416 
{
417 
#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;}
 417 
#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;}
418 
#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; }
 418 
#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; }
419 
    CHK_POTI(Parameter_MaxHoehe, EE_Parameter.MaxHoehe, 0, 255);
 419 
    CHK_POTI(Parameter_MaxHoehe, EE_Parameter.MaxHoehe, 0, 255);
420 
    CHK_POTI_MM(Parameter_Luftdruck_D, EE_Parameter.Luftdruck_D, 0, 100);
 420 
    CHK_POTI_MM(Parameter_Luftdruck_D, EE_Parameter.Luftdruck_D, 0, 100);
421 
    CHK_POTI_MM(Parameter_Hoehe_P, EE_Parameter.Hoehe_P, 0, 100);
 421 
    CHK_POTI_MM(Parameter_Hoehe_P, EE_Parameter.Hoehe_P, 0, 100);
422 
    CHK_POTI(Parameter_Hoehe_ACC_Wirkung, EE_Parameter.Hoehe_ACC_Wirkung, 0, 255);
 422 
    CHK_POTI(Parameter_Hoehe_ACC_Wirkung, EE_Parameter.Hoehe_ACC_Wirkung, 0, 255);
423 
    CHK_POTI(Parameter_KompassWirkung, EE_Parameter.KompassWirkung, 0, 255);
 423 
    CHK_POTI(Parameter_KompassWirkung, EE_Parameter.KompassWirkung, 0, 255);
424 
    CHK_POTI_MM(Parameter_Gyro_P, EE_Parameter.Gyro_P, 10, 255);
 424 
    CHK_POTI_MM(Parameter_Gyro_P, EE_Parameter.Gyro_P, 10, 255);
425 
    CHK_POTI(Parameter_Gyro_I, EE_Parameter.Gyro_I, 0, 255);
 425 
    CHK_POTI(Parameter_Gyro_I, EE_Parameter.Gyro_I, 0, 255);
426 
    CHK_POTI(Parameter_I_Faktor, EE_Parameter.I_Faktor, 0, 255);
 426 
    CHK_POTI(Parameter_I_Faktor, EE_Parameter.I_Faktor, 0, 255);
427 
    CHK_POTI(Parameter_UserParam1, EE_Parameter.UserParam1, 0, 255);
 427 
    CHK_POTI(Parameter_UserParam1, EE_Parameter.UserParam1, 0, 255);
428 
    CHK_POTI(Parameter_UserParam2, EE_Parameter.UserParam2, 0, 255);
 428 
    CHK_POTI(Parameter_UserParam2, EE_Parameter.UserParam2, 0, 255);
429 
    CHK_POTI(Parameter_UserParam3, EE_Parameter.UserParam3, 0, 255);
 429 
    CHK_POTI(Parameter_UserParam3, EE_Parameter.UserParam3, 0, 255);
430 
    CHK_POTI(Parameter_UserParam4, EE_Parameter.UserParam4, 0, 255);
 430 
    CHK_POTI(Parameter_UserParam4, EE_Parameter.UserParam4, 0, 255);
431 
    CHK_POTI(Parameter_UserParam5, EE_Parameter.UserParam5, 0, 255);
 431 
    CHK_POTI(Parameter_UserParam5, EE_Parameter.UserParam5, 0, 255);
432 
    CHK_POTI(Parameter_UserParam6, EE_Parameter.UserParam6, 0, 255);
 432 
    CHK_POTI(Parameter_UserParam6, EE_Parameter.UserParam6, 0, 255);
433 
    CHK_POTI(Parameter_UserParam7, EE_Parameter.UserParam7, 0, 255);
 433 
    CHK_POTI(Parameter_UserParam7, EE_Parameter.UserParam7, 0, 255);
434 
    CHK_POTI(Parameter_UserParam8, EE_Parameter.UserParam8, 0, 255);
 434 
    CHK_POTI(Parameter_UserParam8, EE_Parameter.UserParam8, 0, 255);
435 
    CHK_POTI(Parameter_ServoNickControl, EE_Parameter.ServoNickControl, 0, 255);
 435 
    CHK_POTI(Parameter_ServoNickControl, EE_Parameter.ServoNickControl, 0, 255);
436 
    CHK_POTI(Parameter_LoopGasLimit, EE_Parameter.LoopGasLimit, 0, 255);
 436 
    CHK_POTI(Parameter_LoopGasLimit, EE_Parameter.LoopGasLimit, 0, 255);
437 
    CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1, 0, 255);
 437 
    CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1, 0, 255);
438 
    CHK_POTI(Parameter_AchsGegenKopplung1, EE_Parameter.AchsGegenKopplung1, 0, 255);
 438 
    CHK_POTI(Parameter_AchsGegenKopplung1, EE_Parameter.AchsGegenKopplung1, 0, 255);
439 
    CHK_POTI(Parameter_DynamicStability, EE_Parameter.DynamicStability, 0, 255);
 439 
    CHK_POTI(Parameter_DynamicStability, EE_Parameter.DynamicStability, 0, 255);
440 
 
 440 
 
441 
    CHK_POTI_MM(Parameter_J16Timing, EE_Parameter.J16Timing, 1, 255);
 441 
    CHK_POTI_MM(Parameter_J16Timing, EE_Parameter.J16Timing, 1, 255);
442 
    CHK_POTI_MM(Parameter_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250);
 442 
    CHK_POTI_MM(Parameter_J16Brightness, PARAM_LED_BRIGHTNESS_J16, 0, 250);
443 
    CHK_POTI_MM(Parameter_J17Timing, EE_Parameter.J17Timing, 1, 255);
 443 
    CHK_POTI_MM(Parameter_J17Timing, EE_Parameter.J17Timing, 1, 255);
444 
    CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250);
 444 
    CHK_POTI_MM(Parameter_J17Brightness, PARAM_LED_BRIGHTNESS_J17, 0, 250);
445 
 
 445 
 
446 
    // CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
 446 
    // CHK_POTI(Parameter_NaviGpsModeControl,EE_Parameter.NaviGpsModeControl,0,255);
447 
    //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
 447 
    //CHK_POTI(Parameter_NaviGpsGain,EE_Parameter.NaviGpsGain,0,255);
448 
    // CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
 448 
    // CHK_POTI(Parameter_NaviGpsP,EE_Parameter.NaviGpsP,0,255);
449 
    // CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
 449 
    // CHK_POTI(Parameter_NaviGpsI,EE_Parameter.NaviGpsI,0,255);
450 
    // CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
 450 
    // CHK_POTI(Parameter_NaviGpsD,EE_Parameter.NaviGpsD,0,255);
451 
    // CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
 451 
    // CHK_POTI(Parameter_NaviGpsACC,EE_Parameter.NaviGpsACC,0,255);
452 
    // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
 452 
    // CHK_POTI_MM(Parameter_NaviOperatingRadius,EE_Parameter.NaviOperatingRadius,10,255);
453 
    // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
 453 
    // CHK_POTI(Parameter_NaviWindCorrection,EE_Parameter.NaviWindCorrection,0,255);
454 
    // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
 454 
    // CHK_POTI(Parameter_NaviSpeedCompensation,EE_Parameter.NaviSpeedCompensation,0,255);
455 
 
 455 
 
456 
    CHK_POTI(Parameter_ExternalControl, EE_Parameter.ExternalControl, 0, 255);
 456 
    CHK_POTI(Parameter_ExternalControl, EE_Parameter.ExternalControl, 0, 255);
457 
 
 457 
 
458 
    Ki = (float) Parameter_I_Faktor * 0.0001;
 458 
    Ki = (float) Parameter_I_Faktor * 0.0001;
459 
    MAX_GAS = EE_Parameter.Gas_Max;
 459 
    MAX_GAS = EE_Parameter.Gas_Max;
460 
    MIN_GAS = EE_Parameter.Gas_Min;
 460 
    MIN_GAS = EE_Parameter.Gas_Min;
461 
}
 461 
}
462 
 
 462 
 
463 
 
 463 
 
464 
 
 464 
 
465 
//############################################################################
 465 
//############################################################################
466 
//
 466 
//
467 
 
 467 
 
468 
void MotorRegler(void)
 468 
void MotorRegler(void)
469 
//############################################################################
 469 
//############################################################################
470 
{
 470 
{
471 
    int motorwert, pd_ergebnis, tmp_int;
 471 
    int motorwert, pd_ergebnis, tmp_int;
472 
    int GierMischanteil, GasMischanteil;
 472 
    int GierMischanteil, GasMischanteil;
473 
    static long SummeNick = 0, SummeRoll = 0;
 473 
    static long SummeNick = 0, SummeRoll = 0;
474 
    static long sollGier = 0, tmp_long, tmp_long2;
 474 
    static long sollGier = 0, tmp_long, tmp_long2;
475 
    static long IntegralFehlerNick = 0;
 475 
    static long IntegralFehlerNick = 0;
476 
    static long IntegralFehlerRoll = 0;
 476 
    static long IntegralFehlerRoll = 0;
477 
    static unsigned int RcLostTimer;
 477 
    static unsigned int RcLostTimer;
478 
    static unsigned char delay_neutral = 0;
 478 
    static unsigned char delay_neutral = 0;
479 
    static unsigned char delay_einschalten = 0, delay_ausschalten = 0;
 479 
    static unsigned char delay_einschalten = 0, delay_ausschalten = 0;
480 
    static char TimerWerteausgabe = 0;
 480 
    static char TimerWerteausgabe = 0;
481 
    static char NeueKompassRichtungMerken = 0;
 481 
    static char NeueKompassRichtungMerken = 0;
482 
    static long ausgleichNick, ausgleichRoll;
 482 
    static long ausgleichNick, ausgleichRoll;
483 
 
 483 
 
484 
    Mittelwert();
 484 
    Mittelwert();
485 
 
 485 
 
486 
    GRN_ON;
 486 
    GRN_ON;
487 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 487 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
488 
    // Gaswert ermitteln
 488 
    // Gaswert ermitteln
489 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 489 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
490 
    GasMischanteil = StickGas;
 490 
    GasMischanteil = StickGas;
491 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 491 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
492 
    // Empfang schlecht
 492 
    // Empfang schlecht
493 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 493 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
494 
    if (SenderOkay < 100) {
 494 
    if (SenderOkay < 100) {
495 
        if (!PcZugriff) {
 495 
        if (!PcZugriff) {
496 
            if (BeepMuster == 0xffff) {
 496 
            if (BeepMuster == 0xffff) {
497 
                beeptime = 15000;
 497 
                beeptime = 15000;
498 
                BeepMuster = 0x0c00;
 498 
                BeepMuster = 0x0c00;
499 
            }
 499 
            }
500 
        }
 500 
        }
501 
        if (RcLostTimer) RcLostTimer--;
 501 
        if (RcLostTimer) RcLostTimer--;
502 
        else {
 502 
        else {
503 
            MotorenEin = 0;
 503 
            MotorenEin = 0;
504 
            Notlandung = 0;
 504 
            Notlandung = 0;
505 
        }
 505 
        }
506 
        ROT_ON;
 506 
        ROT_ON;
507 
        if (modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
 507 
        if (modell_fliegt > 1000) // wahrscheinlich in der Luft --> langsam absenken
508 
        {
 508 
        {
509 
            GasMischanteil = EE_Parameter.NotGas;
 509 
            GasMischanteil = EE_Parameter.NotGas;
510 
            Notlandung = 1;
 510 
            Notlandung = 1;
511 
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 511 
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
512 
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 512 
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
513 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 513 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
514 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 514 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
515 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 515 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
516 
        } else MotorenEin = 0;
 516 
        } else MotorenEin = 0;
517 
    } else
 517 
    } else
518 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 518 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
519 
        // Emfang gut
 519 
        // Emfang gut
520 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 520 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
521 
        if (SenderOkay > 140) {
 521 
        if (SenderOkay > 140) {
522 
        Notlandung = 0;
 522 
        Notlandung = 0;
523 
        RcLostTimer = EE_Parameter.NotGasZeit * 50;
 523 
        RcLostTimer = EE_Parameter.NotGasZeit * 50;
524 
        if (GasMischanteil > 40 && MotorenEin) {
 524 
        if (GasMischanteil > 40 && MotorenEin) {
525 
            if (modell_fliegt < 0xffff) modell_fliegt++;
 525 
            if (modell_fliegt < 0xffff) modell_fliegt++;
526 
        }
 526 
        }
527 
        if ((modell_fliegt < 256)) {
 527 
        if ((modell_fliegt < 256)) {
528 
            SummeNick = 0;
 528 
            SummeNick = 0;
529 
            SummeRoll = 0;
 529 
            SummeRoll = 0;
530 
            if (modell_fliegt == 250) {
 530 
            if (modell_fliegt == 250) {
531 
                NeueKompassRichtungMerken = 1;
 531 
                NeueKompassRichtungMerken = 1;
532 
                sollGier = 0;
 532 
                sollGier = 0;
533 
                Mess_Integral_Gier = 0;
 533 
                Mess_Integral_Gier = 0;
534 
                //                  Mess_Integral_Gier2 = 0;
 534 
                //                  Mess_Integral_Gier2 = 0;
535 
            }
 535 
            }
536 
        } else MikroKopterFlags |= FLAG_FLY;
 536 
        } else MikroKopterFlags |= FLAG_FLY;
537 
 
 537 
 
538 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) {
 538 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) {
539 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 539 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
540 
            // auf Nullwerte kalibrieren
 540 
            // auf Nullwerte kalibrieren
541 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 541 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
542 
            if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
 542 
            if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte
543 
            {
 543 
            {
544 
                if (++delay_neutral > 200) // nicht sofort
 544 
                if (++delay_neutral > 200) // nicht sofort
545 
                {
 545 
                {
546 
                    GRN_OFF;
 546 
                    GRN_OFF;
547 
                    MotorenEin = 0;
 547 
                    MotorenEin = 0;
548 
                    delay_neutral = 0;
 548 
                    delay_neutral = 0;
549 
                    modell_fliegt = 0;
 549 
                    modell_fliegt = 0;
550 
                    if (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) {
 550 
                    if (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70) {
551 
                        unsigned char setting = 1;
 551 
                        unsigned char setting = 1;
552 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 552 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
553 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 553 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
554 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 554 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
555 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 555 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
556 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 556 
                        if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
557 
                        SetActiveParamSetNumber(setting); // aktiven Datensatz merken
 557 
                        SetActiveParamSetNumber(setting); // aktiven Datensatz merken
558 
                    }
 558 
                    }
559 
                    //                        else
 559 
                    //                        else
560 
                    if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) {
 560 
                    if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70) {
561 
                        WinkelOut.CalcState = 1;
 561 
                        WinkelOut.CalcState = 1;
562 
                        beeptime = 1000;
 562 
                        beeptime = 1000;
563 
                    } else {
 563 
                    } else {
564 
                        ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 564 
                        ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) & EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
565 
                        if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
 565 
                        if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert?
566 
                        {
 566 
                        {
567 
                            if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 567 
                            if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
568 
                        }
 568 
                        }
569 
                        SetNeutral();
 569 
                        SetNeutral();
570 
                        Piep(GetActiveParamSetNumber());
 570 
                        Piep(GetActiveParamSetNumber());
571 
                    }
 571 
                    }
572 
                }
 572 
                }
573 
            } else
 573 
            } else
574 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
 574 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern
575 
            {
 575 
            {
576 
                if (++delay_neutral > 200) // nicht sofort
 576 
                if (++delay_neutral > 200) // nicht sofort
577 
                {
 577 
                {
578 
                    GRN_OFF;
 578 
                    GRN_OFF;
579 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK], 0xff); // Werte löschen
 579 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK], 0xff); // Werte löschen
580 
                    MotorenEin = 0;
 580 
                    MotorenEin = 0;
581 
                    delay_neutral = 0;
 581 
                    delay_neutral = 0;
582 
                    modell_fliegt = 0;
 582 
                    modell_fliegt = 0;
583 
                    SetNeutral();
 583 
                    SetNeutral();
584 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK], NeutralAccX / 256); // ACC-NeutralWerte speichern
 584 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK], NeutralAccX / 256); // ACC-NeutralWerte speichern
585 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK + 1], NeutralAccX % 256); // ACC-NeutralWerte speichern
 585 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK + 1], NeutralAccX % 256); // ACC-NeutralWerte speichern
586 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL], NeutralAccY / 256);
 586 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL], NeutralAccY / 256);
587 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL + 1], NeutralAccY % 256);
 587 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL + 1], NeutralAccY % 256);
588 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z], (int) NeutralAccZ / 256);
 588 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z], (int) NeutralAccZ / 256);
589 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z + 1], (int) NeutralAccZ % 256);
 589 
                    eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z + 1], (int) NeutralAccZ % 256);
590 
                    Piep(GetActiveParamSetNumber());
 590 
                    Piep(GetActiveParamSetNumber());
591 
                }
 591 
                }
592 
            } else delay_neutral = 0;
 592 
            } else delay_neutral = 0;
593 
        }
 593 
        }
594 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 594 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
595 
        // Gas ist unten
 595 
        // Gas ist unten
596 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 596 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
597 
        if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35 - 125) {
 597 
        if (PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35 - 125) {
598 
            // Starten
 598 
            // Starten
599 
            if (!MotorenEin && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) {
 599 
            if (!MotorenEin && PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) {
600 
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 600 
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
601 
                // Einschalten
 601 
                // Einschalten
602 
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 602 
                // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
603 
                if (++delay_einschalten > 200) {
 603 
                if (++delay_einschalten > 200) {
604 
                    delay_einschalten = 200;
 604 
                    delay_einschalten = 200;
605 
                    modell_fliegt = 1;
 605 
                    modell_fliegt = 1;
606 
                    MotorenEin = 1;
 606 
                    MotorenEin = 1;
607 
                    sollGier = 0;
 607 
                    sollGier = 0;
608 
                    Mess_Integral_Gier = 0;
 608 
                    Mess_Integral_Gier = 0;
609 
                    Mess_Integral_Gier2 = 0;
 609 
                    Mess_Integral_Gier2 = 0;
610 
                    Mess_IntegralNick = 0;
 610 
                    Mess_IntegralNick = 0;
611 
                    Mess_IntegralRoll = 0;
 611 
                    Mess_IntegralRoll = 0;
612 
                    Mess_IntegralNick2 = IntegralNick;
 612 
                    Mess_IntegralNick2 = IntegralNick;
613 
                    Mess_IntegralRoll2 = IntegralRoll;
 613 
                    Mess_IntegralRoll2 = IntegralRoll;
614 
                    SummeNick = 0;
 614 
                    SummeNick = 0;
615 
                    SummeRoll = 0;
 615 
                    SummeRoll = 0;
616 
                    MikroKopterFlags |= FLAG_START;
 616 
                    MikroKopterFlags |= FLAG_START;
617 
 
 617 
 
618 
                    // Beim Einschalten automatisch kalibrieren
 618 
                    // Beim Einschalten automatisch kalibrieren
619 
                    if (PARAM_CAL_ON_START) {
 619 
                    if (PARAM_CAL_ON_START) {
620 
                        if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) {
 620 
                        if ((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) {
621 
                            if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) {
 621 
                            if ((MessLuftdruck > 950) || (MessLuftdruck < 750)) {
622 
                                SucheLuftruckOffset();
 622 
                                SucheLuftruckOffset();
623 
                            }
 623 
                            }
624 
                        }
 624 
                        }
625 
 
 625 
 
626 
                        SetNeutral();
 626 
                        SetNeutral();
627 
                    }
 627 
                    }
628 
                }
 628 
                }
629 
            } else delay_einschalten = 0;
 629 
            } else delay_einschalten = 0;
630 
            //Auf Neutralwerte setzen
 630 
            //Auf Neutralwerte setzen
631 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 631 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
632 
            // Auschalten
 632 
            // Auschalten
633 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 633 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
634 
            if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) {
 634 
            if (PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) {
635 
                if (++delay_ausschalten > 200) // nicht sofort
 635 
                if (++delay_ausschalten > 200) // nicht sofort
636 
                {
 636 
                {
637 
                    MotorenEin = 0;
 637 
                    MotorenEin = 0;
638 
                    delay_ausschalten = 200;
 638 
                    delay_ausschalten = 200;
639 
                    modell_fliegt = 0;
 639 
                    modell_fliegt = 0;
640 
                }
 640 
                }
641 
            } else delay_ausschalten = 0;
 641 
            } else delay_ausschalten = 0;
642 
        }
 642 
        }
643 
    }
 643 
    }
644 
 
 644 
 
645 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 645 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
646 
    // neue Werte von der Funke
 646 
    // neue Werte von der Funke
647 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 647 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
648 
    if (!NewPpmData-- || Notlandung) {
 648 
    if (!NewPpmData-- || Notlandung) {
649 
        static int chanNickPrev = 0;
 649 
        static int chanNickPrev = 0;
650 
        static int chanRollPrev = 0;
 650 
        static int chanRollPrev = 0;
651 
 
 651 
 
652 
        static int stick_nick, stick_roll;
 652 
        static int stick_nick, stick_roll;
653 
 
 653 
 
654 
        ParameterZuordnung();
 654 
        ParameterZuordnung();
655 
 
 655 
 
656 
#define MAX_CHAN_VAL 125L
 656 
#define MAX_CHAN_VAL 125L
657 
#define COS45        7071L              // cos( -45 ) * 10000
 657 
#define COS45        7071L              // cos( -45 ) * 10000
658 
 
 658 
 
659 
        long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
 659 
        long chanNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
660 
        long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
 660 
        long chanRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
661 
 
 661 
 
662 
        int chanNickDiff;
 662 
        int chanNickDiff;
663 
        int chanRollDiff;
 663 
        int chanRollDiff;
664 
 
 664 
 
665 
        /* Über Parameter läßt sich zwischen "+" und "X" - Formations
 665 
        /* Über Parameter läßt sich zwischen "+" und "X" - Formations
666 
         * umschalten (sh. parameter.h)
 666 
         * umschalten (sh. parameter.h)
667 
         */
 667 
         */
668 
        if (PARAM_X_FORMATION) {
 668 
        if (PARAM_X_FORMATION) {
669 
 
 669 
 
670 
            chanRoll = -chanRoll;
 670 
            chanRoll = -chanRoll;
671 
 
 671 
 
672 
            // Stick-Koordinatensystem um -45° (rechts) drehen
 672 
            // Stick-Koordinatensystem um -45° (rechts) drehen
673 
            chanNick *= COS45;
 673 
            chanNick *= COS45;
674 
            chanRoll *= COS45;
 674 
            chanRoll *= COS45;
675 
 
 675 
 
676 
            int chanNickTemp = (chanNick - chanRoll) / 10000L;
 676 
            int chanNickTemp = (chanNick - chanRoll) / 10000L;
677 
            int chanRollTemp = (chanRoll + chanNick) / 10000L;
 677 
            int chanRollTemp = (chanRoll + chanNick) / 10000L;
678 
 
 678 
 
679 
            chanNick = chanNickTemp;
 679 
            chanNick = chanNickTemp;
680 
            chanRoll = -chanRollTemp;
 680 
            chanRoll = -chanRollTemp;
681 
 
 681 
 
682 
            if (chanNick > MAX_CHAN_VAL)
 682 
            if (chanNick > MAX_CHAN_VAL)
683 
                chanNick = MAX_CHAN_VAL;
 683 
                chanNick = MAX_CHAN_VAL;
684 
            if (chanNick < -MAX_CHAN_VAL)
 684 
            if (chanNick < -MAX_CHAN_VAL)
685 
                chanNick = -MAX_CHAN_VAL;
 685 
                chanNick = -MAX_CHAN_VAL;
686 
            if (chanRoll > MAX_CHAN_VAL)
 686 
            if (chanRoll > MAX_CHAN_VAL)
687 
                chanRoll = MAX_CHAN_VAL;
 687 
                chanRoll = MAX_CHAN_VAL;
688 
            if (chanRoll < -MAX_CHAN_VAL)
 688 
            if (chanRoll < -MAX_CHAN_VAL)
689 
                chanRoll = -MAX_CHAN_VAL;
 689 
                chanRoll = -MAX_CHAN_VAL;
690 
        }
 690 
        }
691 
 
 691 
 
692 
        chanNickDiff = ((chanNick - chanNickPrev) / 3) * 3;
 692 
        chanNickDiff = ((chanNick - chanNickPrev) / 3) * 3;
693 
        chanRollDiff = ((chanRoll - chanRollPrev) / 3) * 3;
 693 
        chanRollDiff = ((chanRoll - chanRollPrev) / 3) * 3;
694 
 
 694 
 
695 
        chanNickPrev = chanNick;
 695 
        chanNickPrev = chanNick;
696 
        chanRollPrev = chanRoll;
 696 
        chanRollPrev = chanRoll;
697 
 
 697 
 
698 
        stick_nick = (stick_nick * 3 + ((int) chanNick) * EE_Parameter.Stick_P) / 4;
 698 
        stick_nick = (stick_nick * 3 + ((int) chanNick) * EE_Parameter.Stick_P) / 4;
699 
        stick_nick += chanNickDiff * EE_Parameter.Stick_D;
 699 
        stick_nick += chanNickDiff * EE_Parameter.Stick_D;
700 
        StickNick = stick_nick - GPS_Nick;
 700 
        StickNick = stick_nick - GPS_Nick;
701 
 
 701 
 
702 
        stick_roll = (stick_roll * 3 + ((int) chanRoll) * EE_Parameter.Stick_P) / 4;
 702 
        stick_roll = (stick_roll * 3 + ((int) chanRoll) * EE_Parameter.Stick_P) / 4;
703 
        stick_roll += chanRollDiff * EE_Parameter.Stick_D;
 703 
        stick_roll += chanRollDiff * EE_Parameter.Stick_D;
704 
        StickRoll = stick_roll - GPS_Roll;
 704 
        StickRoll = stick_roll - GPS_Roll;
705 
 
 705 
 
706 
        StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 706 
        StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
707 
 
 707 
 
708 
        // Gaswert übernehmen
 708 
        // Gaswert übernehmen
709 
        StickGas = pitch_value();
 709 
        StickGas = pitchValueFP();
710 
 
 710 
 
711 
        GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN);
 711 
        GyroFaktor = ((float) Parameter_Gyro_P + 10.0) / (256 / STICK_GAIN);
712 
        IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
 712 
        IntegralFaktor = ((float) Parameter_Gyro_I) / (44000 / STICK_GAIN);
713 
 
 713 
 
714 
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 714 
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
715 
        //+ Analoge Steuerung per Seriell
 715 
        //+ Analoge Steuerung per Seriell
716 
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 716 
        //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
717 
        if (ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) {
 717 
        if (ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) {
718 
            StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
 718 
            StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
719 
            StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
 719 
            StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
720 
            StickGier += ExternControl.Gier;
 720 
            StickGier += ExternControl.Gier;
721 
            ExternHoehenValue = (int) ExternControl.Hight * (int) EE_Parameter.Hoehe_Verstaerkung;
 721 
            ExternHoehenValue = (int) ExternControl.Hight * (int) EE_Parameter.Hoehe_Verstaerkung;
722 
            if (ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
 722 
            if (ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
723 
        }
 723 
        }
724 
        if (StickGas < 0) StickGas = 0;
 724 
        if (StickGas < 0) StickGas = 0;
725 
 
 725 
 
726 
        if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
 726 
        if (EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0;
727 
        if (GyroFaktor < 0) GyroFaktor = 0;
 727 
        if (GyroFaktor < 0) GyroFaktor = 0;
728 
        if (IntegralFaktor < 0) IntegralFaktor = 0;
 728 
        if (IntegralFaktor < 0) IntegralFaktor = 0;
729 
 
 729 
 
730 
        if (abs(StickNick / STICK_GAIN) > MaxStickNick) {
 730 
        if (abs(StickNick / STICK_GAIN) > MaxStickNick) {
731 
            MaxStickNick = abs(StickNick) / STICK_GAIN;
 731 
            MaxStickNick = abs(StickNick) / STICK_GAIN;
732 
            if (MaxStickNick > 100) MaxStickNick = 100;
 732 
            if (MaxStickNick > 100) MaxStickNick = 100;
733 
        } else MaxStickNick--;
 733 
        } else MaxStickNick--;
734 
        if (abs(StickRoll / STICK_GAIN) > MaxStickRoll) {
 734 
        if (abs(StickRoll / STICK_GAIN) > MaxStickRoll) {
735 
            MaxStickRoll = abs(StickRoll) / STICK_GAIN;
 735 
            MaxStickRoll = abs(StickRoll) / STICK_GAIN;
736 
            if (MaxStickRoll > 100) MaxStickRoll = 100;
 736 
            if (MaxStickRoll > 100) MaxStickRoll = 100;
737 
        } else MaxStickRoll--;
 737 
        } else MaxStickRoll--;
738 
        if (Notlandung) {
 738 
        if (Notlandung) {
739 
            MaxStickNick = 0;
 739 
            MaxStickNick = 0;
740 
            MaxStickRoll = 0;
 740 
            MaxStickRoll = 0;
741 
        }
 741 
        }
742 
 
 742 
 
743 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 743 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
744 
        // Looping?
 744 
        // Looping?
745 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 745 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
746 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1;
 746 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS) Looping_Links = 1;
747 
        else {
 747 
        else {
748 
            {
 748 
            {
749 
                if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
 749 
                if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
750 
            }
 750 
            }
751 
        }
 751 
        }
752 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
 752 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
753 
        else {
 753 
        else {
754 
            if (Looping_Rechts) // Hysterese
 754 
            if (Looping_Rechts) // Hysterese
755 
            {
 755 
            {
756 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
 756 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
757 
            }
 757 
            }
758 
        }
 758 
        }
759 
 
 759 
 
760 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
 760 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
761 
        else {
 761 
        else {
762 
            if (Looping_Oben) // Hysterese
 762 
            if (Looping_Oben) // Hysterese
763 
            {
 763 
            {
764 
                if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
 764 
                if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
765 
            }
 765 
            }
766 
        }
 766 
        }
767 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
 767 
        if ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
768 
        else {
 768 
        else {
769 
            if (Looping_Unten) // Hysterese
 769 
            if (Looping_Unten) // Hysterese
770 
            {
 770 
            {
771 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
 771 
                if (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
772 
            }
 772 
            }
773 
        }
 773 
        }
774 
 
 774 
 
775 
        if (Looping_Links || Looping_Rechts) Looping_Roll = 1;
 775 
        if (Looping_Links || Looping_Rechts) Looping_Roll = 1;
776 
        else Looping_Roll = 0;
 776 
        else Looping_Roll = 0;
777 
        if (Looping_Oben || Looping_Unten) {
 777 
        if (Looping_Oben || Looping_Unten) {
778 
            Looping_Nick = 1;
 778 
            Looping_Nick = 1;
779 
            Looping_Roll = 0;
 779 
            Looping_Roll = 0;
780 
            Looping_Links = 0;
 780 
            Looping_Links = 0;
781 
            Looping_Rechts = 0;
 781 
            Looping_Rechts = 0;
782 
        } else Looping_Nick = 0;
 782 
        } else Looping_Nick = 0;
783 
    } // Ende neue Funken-Werte
 783 
    } // Ende neue Funken-Werte
784 
 
 784 
 
785 
    if (Looping_Roll || Looping_Nick) {
 785 
    if (Looping_Roll || Looping_Nick) {
786 
        if (GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 786 
        if (GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
787 
    }
 787 
    }
788 
 
 788 
 
789 
 
 789 
 
790 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 790 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
791 
    // Bei Empfangsausfall im Flug
 791 
    // Bei Empfangsausfall im Flug
792 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 792 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
793 
    if (Notlandung) {
 793 
    if (Notlandung) {
794 
        StickGier = 0;
 794 
        StickGier = 0;
795 
        StickNick = 0;
 795 
        StickNick = 0;
796 
        StickRoll = 0;
 796 
        StickRoll = 0;
797 
        GyroFaktor = (float) 100 / (256.0 / STICK_GAIN);
 797 
        GyroFaktor = (float) 100 / (256.0 / STICK_GAIN);
798 
        IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
 798 
        IntegralFaktor = (float) 120 / (44000 / STICK_GAIN);
799 
        Looping_Roll = 0;
 799 
        Looping_Roll = 0;
800 
        Looping_Nick = 0;
 800 
        Looping_Nick = 0;
801 
    }
 801 
    }
802 
 
 802 
 
803 
 
 803 
 
804 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 804 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
805 
    // Integrale auf ACC-Signal abgleichen
 805 
    // Integrale auf ACC-Signal abgleichen
806 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 806 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
807 
#define ABGLEICH_ANZAHL 256L
 807 
#define ABGLEICH_ANZAHL 256L
808 
 
 808 
 
809 
    MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
 809 
    MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren
810 
    MittelIntegralRoll += IntegralRoll;
 810 
    MittelIntegralRoll += IntegralRoll;
811 
    MittelIntegralNick2 += IntegralNick2;
 811 
    MittelIntegralNick2 += IntegralNick2;
812 
    MittelIntegralRoll2 += IntegralRoll2;
 812 
    MittelIntegralRoll2 += IntegralRoll2;
813 
 
 813 
 
814 
    if (Looping_Nick || Looping_Roll) {
 814 
    if (Looping_Nick || Looping_Roll) {
815 
        IntegralAccNick = 0;
 815 
        IntegralAccNick = 0;
816 
        IntegralAccRoll = 0;
 816 
        IntegralAccRoll = 0;
817 
        MittelIntegralNick = 0;
 817 
        MittelIntegralNick = 0;
818 
        MittelIntegralRoll = 0;
 818 
        MittelIntegralRoll = 0;
819 
        MittelIntegralNick2 = 0;
 819 
        MittelIntegralNick2 = 0;
820 
        MittelIntegralRoll2 = 0;
 820 
        MittelIntegralRoll2 = 0;
821 
        Mess_IntegralNick2 = Mess_IntegralNick;
 821 
        Mess_IntegralNick2 = Mess_IntegralNick;
822 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
 822 
        Mess_IntegralRoll2 = Mess_IntegralRoll;
823 
        ZaehlMessungen = 0;
 823 
        ZaehlMessungen = 0;
824 
        LageKorrekturNick = 0;
 824 
        LageKorrekturNick = 0;
825 
        LageKorrekturRoll = 0;
 825 
        LageKorrekturRoll = 0;
826 
    }
 826 
    }
827 
 
 827 
 
828 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 828 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
829 
    if (!Looping_Nick && !Looping_Roll) {
 829 
    if (!Looping_Nick && !Looping_Roll) {
830 
        long tmp_long, tmp_long2;
 830 
        long tmp_long, tmp_long2;
831 
        if (FromNaviCtrl_Value.Kalman_K != -1) {
 831 
        if (FromNaviCtrl_Value.Kalman_K != -1) {
832 
            tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
 832 
            tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
833 
            tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
 833 
            tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
834 
            tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
 834 
            tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
835 
            tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
 835 
            tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
836 
            if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
 836 
            if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
837 
                tmp_long /= 2;
 837 
                tmp_long /= 2;
838 
                tmp_long2 /= 2;
 838 
                tmp_long2 /= 2;
839 
            }
 839 
            }
840 
            if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
 840 
            if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
841 
                tmp_long /= 3;
 841 
                tmp_long /= 3;
842 
                tmp_long2 /= 3;
 842 
                tmp_long2 /= 3;
843 
            }
 843 
            }
844 
            if (tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
 844 
            if (tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
845 
            if (tmp_long < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
 845 
            if (tmp_long < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
846 
            if (tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
 846 
            if (tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
847 
            if (tmp_long2 < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
 847 
            if (tmp_long2 < (long) - FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) - FromNaviCtrl_Value.Kalman_MaxFusion;
848 
        } else {
 848 
        } else {
849 
            tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
 849 
            tmp_long = (long) (IntegralNick / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccNick);
850 
            tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
 850 
            tmp_long2 = (long) (IntegralRoll / EE_Parameter.GyroAccFaktor - (long) Mittelwert_AccRoll);
851 
            tmp_long /= 16;
 851 
            tmp_long /= 16;
852 
            tmp_long2 /= 16;
 852 
            tmp_long2 /= 16;
853 
            if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
 853 
            if ((MaxStickNick > 64) || (MaxStickRoll > 64)) {
854 
                tmp_long /= 3;
 854 
                tmp_long /= 3;
855 
                tmp_long2 /= 3;
 855 
                tmp_long2 /= 3;
856 
            }
 856 
            }
857 
            if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
 857 
            if (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) {
858 
                tmp_long /= 3;
 858 
                tmp_long /= 3;
859 
                tmp_long2 /= 3;
 859 
                tmp_long2 /= 3;
860 
            }
 860 
            }
861 
#define AUSGLEICH 32
 861 
#define AUSGLEICH 32
862 
            if (tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
 862 
            if (tmp_long > AUSGLEICH) tmp_long = AUSGLEICH;
863 
            if (tmp_long < -AUSGLEICH) tmp_long = -AUSGLEICH;
 863 
            if (tmp_long < -AUSGLEICH) tmp_long = -AUSGLEICH;
864 
            if (tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
 864 
            if (tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH;
865 
            if (tmp_long2 <-AUSGLEICH) tmp_long2 = -AUSGLEICH;
 865 
            if (tmp_long2 <-AUSGLEICH) tmp_long2 = -AUSGLEICH;
866 
        }
 866 
        }
867 
 
 867 
 
868 
        Mess_IntegralNick -= tmp_long;
 868 
        Mess_IntegralNick -= tmp_long;
869 
        Mess_IntegralRoll -= tmp_long2;
 869 
        Mess_IntegralRoll -= tmp_long2;
870 
    }
 870 
    }
871 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 871 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
872 
    if (ZaehlMessungen >= ABGLEICH_ANZAHL) {
 872 
    if (ZaehlMessungen >= ABGLEICH_ANZAHL) {
873 
        static int cnt = 0;
 873 
        static int cnt = 0;
874 
        static char last_n_p, last_n_n, last_r_p, last_r_n;
 874 
        static char last_n_p, last_n_n, last_r_p, last_r_n;
875 
        static long MittelIntegralNick_Alt, MittelIntegralRoll_Alt;
 875 
        static long MittelIntegralNick_Alt, MittelIntegralRoll_Alt;
876 
        if (!Looping_Nick && !Looping_Roll && !TrichterFlug) {
 876 
        if (!Looping_Nick && !Looping_Roll && !TrichterFlug) {
877 
            MittelIntegralNick /= ABGLEICH_ANZAHL;
 877 
            MittelIntegralNick /= ABGLEICH_ANZAHL;
878 
            MittelIntegralRoll /= ABGLEICH_ANZAHL;
 878 
            MittelIntegralRoll /= ABGLEICH_ANZAHL;
879 
            IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
 879 
            IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
880 
            IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
 880 
            IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
881 
            IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
 881 
            IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL;
882 
#define MAX_I 0//(Poti2/10)
 882 
#define MAX_I 0//(Poti2/10)
883 
            // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
 883 
            // Nick ++++++++++++++++++++++++++++++++++++++++++++++++
884 
            IntegralFehlerNick = (long) (MittelIntegralNick - (long) IntegralAccNick);
 884 
            IntegralFehlerNick = (long) (MittelIntegralNick - (long) IntegralAccNick);
885 
            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 885 
            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
886 
            // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
 886 
            // Roll ++++++++++++++++++++++++++++++++++++++++++++++++
887 
            IntegralFehlerRoll = (long) (MittelIntegralRoll - (long) IntegralAccRoll);
 887 
            IntegralFehlerRoll = (long) (MittelIntegralRoll - (long) IntegralAccRoll);
888 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 888 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
889 
 
 889 
 
890 
            LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 890 
            LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
891 
            LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 891 
            LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
892 
 
 892 
 
893 
            if (((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)) {
 893 
            if (((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1)) {
894 
                LageKorrekturNick /= 2;
 894 
                LageKorrekturNick /= 2;
895 
                LageKorrekturRoll /= 2;
 895 
                LageKorrekturRoll /= 2;
896 
            }
 896 
            }
897 
 
 897 
 
898 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 898 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
899 
            // Gyro-Drift ermitteln
 899 
            // Gyro-Drift ermitteln
900 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 900 
            // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
901 
            MittelIntegralNick2 /= ABGLEICH_ANZAHL;
 901 
            MittelIntegralNick2 /= ABGLEICH_ANZAHL;
902 
            MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
 902 
            MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
903 
            tmp_long = IntegralNick2 - IntegralNick;
 903 
            tmp_long = IntegralNick2 - IntegralNick;
904 
            tmp_long2 = IntegralRoll2 - IntegralRoll;
 904 
            tmp_long2 = IntegralRoll2 - IntegralRoll;
905 
            //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
 905 
            //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
906 
 
 906 
 
907 
            IntegralFehlerNick = tmp_long;
 907 
            IntegralFehlerNick = tmp_long;
908 
            IntegralFehlerRoll = tmp_long2;
 908 
            IntegralFehlerRoll = tmp_long2;
909 
            Mess_IntegralNick2 -= IntegralFehlerNick;
 909 
            Mess_IntegralNick2 -= IntegralFehlerNick;
910 
            Mess_IntegralRoll2 -= IntegralFehlerRoll;
 910 
            Mess_IntegralRoll2 -= IntegralFehlerRoll;
911 
 
 911 
 
912 
            //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 912 
            //    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
913 
            //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 913 
            //    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
914 
            if (GierGyroFehler > ABGLEICH_ANZAHL / 2) {
 914 
            if (GierGyroFehler > ABGLEICH_ANZAHL / 2) {
915 
                AdNeutralGier++;
 915 
                AdNeutralGier++;
916 
                AdNeutralGierBias++;
 916 
                AdNeutralGierBias++;
917 
            }
 917 
            }
918 
            if (GierGyroFehler <-ABGLEICH_ANZAHL / 2) {
 918 
            if (GierGyroFehler <-ABGLEICH_ANZAHL / 2) {
919 
                AdNeutralGier--;
 919 
                AdNeutralGier--;
920 
                AdNeutralGierBias--;
 920 
                AdNeutralGierBias--;
921 
            }
 921 
            }
922 
 
 922 
 
923 
            DebugOut.Analog[22] = MittelIntegralRoll / 26;
 923 
            DebugOut.Analog[22] = MittelIntegralRoll / 26;
924 
            //DebugOut.Analog[24] = GierGyroFehler;
 924 
            //DebugOut.Analog[24] = GierGyroFehler;
925 
            GierGyroFehler = 0;
 925 
            GierGyroFehler = 0;
926 
 
 926 
 
927 
 
 927 
 
928 
            /*DebugOut.Analog[17] = IntegralAccNick / 26;
 928 
            /*DebugOut.Analog[17] = IntegralAccNick / 26;
929 
            DebugOut.Analog[18] = IntegralAccRoll / 26;
 929 
            DebugOut.Analog[18] = IntegralAccRoll / 26;
930 
            DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 930 
            DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
931 
            DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 931 
            DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
932 
             */
 932 
             */
933 
            //DebugOut.Analog[21] = MittelIntegralNick / 26;
 933 
            //DebugOut.Analog[21] = MittelIntegralNick / 26;
934 
            //MittelIntegralRoll = MittelIntegralRoll;
 934 
            //MittelIntegralRoll = MittelIntegralRoll;
935 
            //DebugOut.Analog[28] = ausgleichNick;
 935 
            //DebugOut.Analog[28] = ausgleichNick;
936 
            /*
 936 
            /*
937 
            DebugOut.Analog[29] = ausgleichRoll;
 937 
            DebugOut.Analog[29] = ausgleichRoll;
938 
            DebugOut.Analog[30] = LageKorrekturRoll * 10;
 938 
            DebugOut.Analog[30] = LageKorrekturRoll * 10;
939 
             */
 939 
             */
940 
 
 940 
 
941 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 941 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
942 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 942 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
943 
#define BEWEGUNGS_LIMIT 20000
 943 
#define BEWEGUNGS_LIMIT 20000
944 
            // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
 944 
            // Nick +++++++++++++++++++++++++++++++++++++++++++++++++
945 
            cnt = 1; // + labs(IntegralFehlerNick) / 4096;
 945 
            cnt = 1; // + labs(IntegralFehlerNick) / 4096;
946 
            if (labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
 946 
            if (labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
947 
                if (IntegralFehlerNick > FEHLER_LIMIT2) {
 947 
                if (IntegralFehlerNick > FEHLER_LIMIT2) {
948 
                    if (last_n_p) {
 948 
                    if (last_n_p) {
949 
                        cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 949 
                        cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
950 
                        ausgleichNick = IntegralFehlerNick / 8;
 950 
                        ausgleichNick = IntegralFehlerNick / 8;
951 
                        if (ausgleichNick > 5000) ausgleichNick = 5000;
 951 
                        if (ausgleichNick > 5000) ausgleichNick = 5000;
952 
                        LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 952 
                        LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
953 
                    } else last_n_p = 1;
 953 
                    } else last_n_p = 1;
954 
                } else last_n_p = 0;
 954 
                } else last_n_p = 0;
955 
                if (IntegralFehlerNick < -FEHLER_LIMIT2) {
 955 
                if (IntegralFehlerNick < -FEHLER_LIMIT2) {
956 
                    if (last_n_n) {
 956 
                    if (last_n_n) {
957 
                        cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 957 
                        cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
958 
                        ausgleichNick = IntegralFehlerNick / 8;
 958 
                        ausgleichNick = IntegralFehlerNick / 8;
959 
                        if (ausgleichNick < -5000) ausgleichNick = -5000;
 959 
                        if (ausgleichNick < -5000) ausgleichNick = -5000;
960 
                        LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 960 
                        LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
961 
                    } else last_n_n = 1;
 961 
                    } else last_n_n = 1;
962 
                } else last_n_n = 0;
 962 
                } else last_n_n = 0;
963 
            } else {
 963 
            } else {
964 
                cnt = 0;
 964 
                cnt = 0;
965 
                KompassSignalSchlecht = 1000;
 965 
                KompassSignalSchlecht = 1000;
966 
            }
 966 
            }
967 
            if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 967 
            if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
968 
            if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
 968 
            if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
969 
            if (IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
 969 
            if (IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt;
970 
            if (IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
 970 
            if (IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt;
971 
 
 971 
 
972 
            // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 972 
            // Roll +++++++++++++++++++++++++++++++++++++++++++++++++
973 
            cnt = 1; // + labs(IntegralFehlerNick) / 4096;
 973 
            cnt = 1; // + labs(IntegralFehlerNick) / 4096;
974 
 
 974 
 
975 
            ausgleichRoll = 0;
 975 
            ausgleichRoll = 0;
976 
            if (labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
 976 
            if (labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3 * 16)) {
977 
                if (IntegralFehlerRoll > FEHLER_LIMIT2) {
 977 
                if (IntegralFehlerRoll > FEHLER_LIMIT2) {
978 
                    if (last_r_p) {
 978 
                    if (last_r_p) {
979 
                        cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 979 
                        cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
980 
                        ausgleichRoll = IntegralFehlerRoll / 8;
 980 
                        ausgleichRoll = IntegralFehlerRoll / 8;
981 
                        if (ausgleichRoll > 5000) ausgleichRoll = 5000;
 981 
                        if (ausgleichRoll > 5000) ausgleichRoll = 5000;
982 
                        LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 982 
                        LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
983 
                    } else last_r_p = 1;
 983 
                    } else last_r_p = 1;
984 
                } else last_r_p = 0;
 984 
                } else last_r_p = 0;
985 
                if (IntegralFehlerRoll < -FEHLER_LIMIT2) {
 985 
                if (IntegralFehlerRoll < -FEHLER_LIMIT2) {
986 
                    if (last_r_n) {
 986 
                    if (last_r_n) {
987 
                        cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 987 
                        cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
988 
                        ausgleichRoll = IntegralFehlerRoll / 8;
 988 
                        ausgleichRoll = IntegralFehlerRoll / 8;
989 
                        if (ausgleichRoll < -5000) ausgleichRoll = -5000;
 989 
                        if (ausgleichRoll < -5000) ausgleichRoll = -5000;
990 
                        LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 990 
                        LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
991 
                    } else last_r_n = 1;
 991 
                    } else last_r_n = 1;
992 
                } else last_r_n = 0;
 992 
                } else last_r_n = 0;
993 
            } else {
 993 
            } else {
994 
                cnt = 0;
 994 
                cnt = 0;
995 
                KompassSignalSchlecht = 1000;
 995 
                KompassSignalSchlecht = 1000;
996 
            }
 996 
            }
997 
            if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 997 
            if (cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
998 
            if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
 998 
            if (cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift / 16;
999 
            if (IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
 999 
            if (IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt;
1000 
            if (IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
 1000 
            if (IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt;
1001 
        } else {
 1001 
        } else {
1002 
            LageKorrekturRoll = 0;
 1002 
            LageKorrekturRoll = 0;
1003 
            LageKorrekturNick = 0;
 1003 
            LageKorrekturNick = 0;
1004 
            TrichterFlug = 0;
 1004 
            TrichterFlug = 0;
1005 
        }
 1005 
        }
1006 
 
 1006 
 
1007 
        if (!IntegralFaktor) {
 1007 
        if (!IntegralFaktor) {
1008 
            LageKorrekturRoll = 0;
 1008 
            LageKorrekturRoll = 0;
1009 
            LageKorrekturNick = 0;
 1009 
            LageKorrekturNick = 0;
1010 
        } // z.B. bei HH
 1010 
        } // z.B. bei HH
1011 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1011 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1012 
        MittelIntegralNick_Alt = MittelIntegralNick;
 1012 
        MittelIntegralNick_Alt = MittelIntegralNick;
1013 
        MittelIntegralRoll_Alt = MittelIntegralRoll;
 1013 
        MittelIntegralRoll_Alt = MittelIntegralRoll;
1014 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1014 
        // +++++++++++++++++++++++++++++++++++++++++++++++++++++
1015 
        IntegralAccNick = 0;
 1015 
        IntegralAccNick = 0;
1016 
        IntegralAccRoll = 0;
 1016 
        IntegralAccRoll = 0;
1017 
        IntegralAccZ = 0;
 1017 
        IntegralAccZ = 0;
1018 
        MittelIntegralNick = 0;
 1018 
        MittelIntegralNick = 0;
1019 
        MittelIntegralRoll = 0;
 1019 
        MittelIntegralRoll = 0;
1020 
        MittelIntegralNick2 = 0;
 1020 
        MittelIntegralNick2 = 0;
1021 
        MittelIntegralRoll2 = 0;
 1021 
        MittelIntegralRoll2 = 0;
1022 
        ZaehlMessungen = 0;
 1022 
        ZaehlMessungen = 0;
1023 
    }
 1023 
    }
1024 
    //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
 1024 
    //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor);
1025 
 
 1025 
 
1026 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1026 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1027 
    //  Gieren
 1027 
    //  Gieren
1028 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1028 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1029 
    //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
 1029 
    //    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1030 
    if (abs(StickGier) > 15) // war 35
 1030 
    if (abs(StickGier) > 15) // war 35
1031 
    {
 1031 
    {
1032 
        KompassSignalSchlecht = 1000;
 1032 
        KompassSignalSchlecht = 1000;
1033 
        if (!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) {
 1033 
        if (!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) {
1034 
            NeueKompassRichtungMerken = 1;
 1034 
            NeueKompassRichtungMerken = 1;
1035 
        };
 1035 
        };
1036 
    }
 1036 
    }
1037 
    tmp_int = (long) EE_Parameter.Gier_P * ((long) StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 1037 
    tmp_int = (long) EE_Parameter.Gier_P * ((long) StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1038 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 1038 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
1039 
    sollGier = tmp_int;
 1039 
    sollGier = tmp_int;
1040 
    Mess_Integral_Gier -= tmp_int;
 1040 
    Mess_Integral_Gier -= tmp_int;
1041 
    if (Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
 1041 
    if (Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen
1042 
    if (Mess_Integral_Gier <-50000) Mess_Integral_Gier = -50000;
 1042 
    if (Mess_Integral_Gier <-50000) Mess_Integral_Gier = -50000;
1043 
 
 1043 
 
1044 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1044 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1045 
    //  Kompass
 1045 
    //  Kompass
1046 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1046 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1047 
    //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
 1047 
    //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll);
1048 
 
 1048 
 
1049 
    if (KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) {
 1049 
    if (KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) {
1050 
        int w, v, r, fehler, korrektur;
 1050 
        int w, v, r, fehler, korrektur;
1051 
        w = abs(IntegralNick / 512); // mit zunehmender Neigung den Einfluss drosseln
 1051 
        w = abs(IntegralNick / 512); // mit zunehmender Neigung den Einfluss drosseln
1052 
        v = abs(IntegralRoll / 512);
 1052 
        v = abs(IntegralRoll / 512);
1053 
        if (v > w) w = v; // grösste Neigung ermitteln
 1053 
        if (v > w) w = v; // grösste Neigung ermitteln
1054 
        korrektur = w / 8 + 1;
 1054 
        korrektur = w / 8 + 1;
1055 
        fehler = ((540 + KompassValue - (ErsatzKompass / GIER_GRAD_FAKTOR)) % 360) - 180;
 1055 
        fehler = ((540 + KompassValue - (ErsatzKompass / GIER_GRAD_FAKTOR)) % 360) - 180;
1056 
        if (NeueKompassRichtungMerken) {
 1056 
        if (NeueKompassRichtungMerken) {
1057 
            fehler = 0;
 1057 
            fehler = 0;
1058 
            ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
 1058 
            ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
1059 
        }
 1059 
        }
1060 
        if (!KompassSignalSchlecht && w < 25) {
 1060 
        if (!KompassSignalSchlecht && w < 25) {
1061 
            GierGyroFehler += fehler;
 1061 
            GierGyroFehler += fehler;
1062 
            if (NeueKompassRichtungMerken) {
 1062 
            if (NeueKompassRichtungMerken) {
1063 
                beeptime = 200;
 1063 
                beeptime = 200;
1064 
                //         KompassStartwert = KompassValue;
 1064 
                //         KompassStartwert = KompassValue;
1065 
                KompassStartwert = (ErsatzKompass / GIER_GRAD_FAKTOR);
 1065 
                KompassStartwert = (ErsatzKompass / GIER_GRAD_FAKTOR);
1066 
                NeueKompassRichtungMerken = 0;
 1066 
                NeueKompassRichtungMerken = 0;
1067 
            }
 1067 
            }
1068 
        }
 1068 
        }
1069 
        ErsatzKompass += (fehler * 8) / korrektur;
 1069 
        ErsatzKompass += (fehler * 8) / korrektur;
1070 
        w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
 1070 
        w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren
1071 
        w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
 1071 
        w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln
1072 
        if (w >= 0) {
 1072 
        if (w >= 0) {
1073 
            if (!KompassSignalSchlecht) {
 1073 
            if (!KompassSignalSchlecht) {
1074 
                v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
 1074 
                v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
1075 
                r = ((540 + (ErsatzKompass / GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
 1075 
                r = ((540 + (ErsatzKompass / GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
1076 
                //           r = KompassRichtung;
 1076 
                //           r = KompassRichtung;
1077 
                v = (r * w) / v; // nach Kompass ausrichten
 1077 
                v = (r * w) / v; // nach Kompass ausrichten
1078 
                w = 3 * Parameter_KompassWirkung;
 1078 
                w = 3 * Parameter_KompassWirkung;
1079 
                if (v > w) v = w; // Begrenzen
 1079 
                if (v > w) v = w; // Begrenzen
1080 
                else
 1080 
                else
1081 
                    if (v < -w) v = -w;
 1081 
                    if (v < -w) v = -w;
1082 
                Mess_Integral_Gier += v;
 1082 
                Mess_Integral_Gier += v;
1083 
            }
 1083 
            }
1084 
            if (KompassSignalSchlecht) KompassSignalSchlecht--;
 1084 
            if (KompassSignalSchlecht) KompassSignalSchlecht--;
1085 
        } else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 1085 
        } else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
1086 
    }
 1086 
    }
1087 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1087 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1088 
 
 1088 
 
1089 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1089 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1090 
    //  Debugwerte zuordnen
 1090 
    //  Debugwerte zuordnen
1091 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1091 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1092 
    if (!TimerWerteausgabe--) {
 1092 
    if (!TimerWerteausgabe--) {
1093 
        TimerWerteausgabe = 24;
 1093 
        TimerWerteausgabe = 24;
1094 
 
 1094 
 
1095 
        DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 1095 
        DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
1096 
        DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 1096 
        DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
1097 
        DebugOut.Analog[2] = Mittelwert_AccNick;
 1097 
        DebugOut.Analog[2] = Mittelwert_AccNick;
1098 
        DebugOut.Analog[3] = Mittelwert_AccRoll;
 1098 
        DebugOut.Analog[3] = Mittelwert_AccRoll;
1099 
        DebugOut.Analog[4] = MesswertGier;
 1099 
        DebugOut.Analog[4] = MesswertGier;
1100 
        DebugOut.Analog[5] = HoehenWert;
 1100 
        DebugOut.Analog[5] = HoehenWert;
1101 
        DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 1101 
        DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
1102 
        DebugOut.Analog[8] = KompassValue;
 1102 
        DebugOut.Analog[8] = KompassValue;
1103 
        DebugOut.Analog[9] = UBat;
 1103 
        DebugOut.Analog[9] = UBat;
1104 
        DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
 1104 
        DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
1105 
        DebugOut.Analog[10] = SenderOkay;
 1105 
        DebugOut.Analog[10] = SenderOkay;
1106 
        //DebugOut.Analog[16] = Mittelwert_AccHoch;
 1106 
        //DebugOut.Analog[16] = Mittelwert_AccHoch;
1107 
        DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
 1107 
        DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
1108 
        DebugOut.Analog[18] = (int) FromNaviCtrl_Value.OsdBar;
 1108 
        DebugOut.Analog[18] = (int) FromNaviCtrl_Value.OsdBar;
1109 
        DebugOut.Analog[19] = WinkelOut.CalcState;
 1109 
        DebugOut.Analog[19] = WinkelOut.CalcState;
1110 
        DebugOut.Analog[20] = ServoValue;
 1110 
        DebugOut.Analog[20] = ServoValue;
1111 
        //    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
 1111 
        //    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1112 
        //    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
 1112 
        //    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1113 
        //    DebugOut.Analog[30] = GPS_Nick;
 1113 
        //    DebugOut.Analog[30] = GPS_Nick;
1114 
        //    DebugOut.Analog[31] = GPS_Roll;
 1114 
        //    DebugOut.Analog[31] = GPS_Roll;
1115 
 
 1115 
 
1116 
 
 1116 
 
1117 
        //    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
 1117 
        //    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
1118 
        //    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
 1118 
        //    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
1119 
 
 1119 
 
1120 
        /*    DebugOut.Analog[16] = motor_rx[0];
 1120 
        /*    DebugOut.Analog[16] = motor_rx[0];
1121 
            DebugOut.Analog[17] = motor_rx[1];
 1121 
            DebugOut.Analog[17] = motor_rx[1];
1122 
            DebugOut.Analog[18] = motor_rx[2];
 1122 
            DebugOut.Analog[18] = motor_rx[2];
1123 
            DebugOut.Analog[19] = motor_rx[3];
 1123 
            DebugOut.Analog[19] = motor_rx[3];
1124 
            DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
 1124 
            DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1125 
            DebugOut.Analog[20] /= 14;
 1125 
            DebugOut.Analog[20] /= 14;
1126 
            DebugOut.Analog[21] = motor_rx[4];
 1126 
            DebugOut.Analog[21] = motor_rx[4];
1127 
            DebugOut.Analog[22] = motor_rx[5];
 1127 
            DebugOut.Analog[22] = motor_rx[5];
1128 
            DebugOut.Analog[23] = motor_rx[6];
 1128 
            DebugOut.Analog[23] = motor_rx[6];
1129 
            DebugOut.Analog[24] = motor_rx[7];
 1129 
            DebugOut.Analog[24] = motor_rx[7];
1130 
            DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 1130 
            DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1131 
         */
 1131 
         */
1132 
        //    DebugOut.Analog[9] = MesswertNick;
 1132 
        //    DebugOut.Analog[9] = MesswertNick;
1133 
        //    DebugOut.Analog[9] = SollHoehe;
 1133 
        //    DebugOut.Analog[9] = SollHoehe;
1134 
        //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 1134 
        //    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
1135 
        //    DebugOut.Analog[11] = KompassStartwert;
 1135 
        //    DebugOut.Analog[11] = KompassStartwert;
1136 
        //    DebugOut.Analog[10] = Parameter_Gyro_I;
 1136 
        //    DebugOut.Analog[10] = Parameter_Gyro_I;
1137 
        //    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
 1137 
        //    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
1138 
        //    DebugOut.Analog[9] = KompassRichtung;
 1138 
        //    DebugOut.Analog[9] = KompassRichtung;
1139 
        //    DebugOut.Analog[10] = GasMischanteil;
 1139 
        //    DebugOut.Analog[10] = GasMischanteil;
1140 
        //    DebugOut.Analog[3] = HoeheD * 32;
 1140 
        //    DebugOut.Analog[3] = HoeheD * 32;
1141 
        //    DebugOut.Analog[4] = hoehenregler;
 1141 
        //    DebugOut.Analog[4] = hoehenregler;
1142 
    }
 1142 
    }
1143 
 
 1143 
 
1144 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1144 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1145 
    //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 1145 
    //  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1146 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1146 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1147 
 
 1147 
 
1148 
    if (Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 1148 
    if (Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
1149 
    else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 1149 
    else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
1150 
    if (Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 1150 
    if (Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
1151 
    else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 1151 
    else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
1152 
    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
 1152 
    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
1153 
 
 1153 
 
1154 
    DebugOut.Analog[21] = MesswertNick;
 1154 
    DebugOut.Analog[21] = MesswertNick;
1155 
    DebugOut.Analog[22] = MesswertRoll;
 1155 
    DebugOut.Analog[22] = MesswertRoll;
1156 
 
 1156 
 
1157 
    // Maximalwerte abfangen
 1157 
    // Maximalwerte abfangen
1158 
#define MAX_SENSOR  (4096*STICK_GAIN)
 1158 
#define MAX_SENSOR  (4096*STICK_GAIN)
1159 
    if (MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
 1159 
    if (MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR;
1160 
    if (MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 1160 
    if (MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1161 
    if (MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
 1161 
    if (MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR;
1162 
    if (MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 1162 
    if (MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1163 
    if (MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
 1163 
    if (MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR;
1164 
    if (MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 1164 
    if (MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1165 
 
 1165 
 
1166 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1166 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1167 
    // Gas-Mischanteil
 1167 
    // Gas-Mischanteil
1168 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1168 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1169 
 
 1169 
 
1170 
    // Zur besseren Auflösung hochskalieren
 1170 
    // Zur besseren Auflösung hochskalieren
1171 
    GasMischanteil *= STICK_GAIN;
 1171 
    GasMischanteil *= STICK_GAIN;
1172 
 
 1172 
 
1173 
        // Fehlerwert der Höhenregelung einmischen
 1173 
        // Fehlerwert der Höhenregelung einmischen
1174 
        GasMischanteil -= altcon_error();
 1174 
        GasMischanteil -= altcon_error();
1175 
 
 1175 
 
1176 
        // Mindestens auf Minimalgas stellen
 1176 
        // Mindestens auf Minimalgas stellen
1177 
        if( GasMischanteil < MIN_GAS )
 1177 
        if( GasMischanteil < MIN_GAS )
1178 
                GasMischanteil = MIN_GAS;
 1178 
                GasMischanteil = MIN_GAS;
1179 
 
 1179 
 
1180 
        // Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren)
 1180 
        // Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren)
1181 
        if( GasMischanteil > ( MAX_GAS - 20 ) * STICK_GAIN )
 1181 
        if( GasMischanteil > ( MAX_GAS - 20 ) * STICK_GAIN )
1182 
                GasMischanteil = ( MAX_GAS - 20 ) * STICK_GAIN;
 1182 
                GasMischanteil = ( MAX_GAS - 20 ) * STICK_GAIN;
1183 
 
 1183 
 
1184 
    // Mindestens auf Minimalgas stellen
 1184 
    // Mindestens auf Minimalgas stellen
1185 
    if (GasMischanteil < MIN_GAS)
 1185 
    if (GasMischanteil < MIN_GAS)
1186 
        GasMischanteil = MIN_GAS;
 1186 
        GasMischanteil = MIN_GAS;
1187 
 
 1187 
 
1188 
    // Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren)
 1188 
    // Begrenzung des Gasmischanteils auf MAX_GAS - 20 (Reserve für Motoren)
1189 
    if (GasMischanteil > (MAX_GAS - 20) * STICK_GAIN)
 1189 
    if (GasMischanteil > (MAX_GAS - 20) * STICK_GAIN)
1190 
        GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
 1190 
        GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1191 
 
 1191 
 
1192 
    DebugOut.Analog[7] = GasMischanteil;
 1192 
    DebugOut.Analog[7] = GasMischanteil;
1193 
 
 1193 
 
1194 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1194 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1195 
    // Gier-Anteil
 1195 
    // Gier-Anteil
1196 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1196 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1197 
 
 1197 
 
1198 
    GierMischanteil = MesswertGier - (sollGier * STICK_GAIN); // Regler für Gier
 1198 
    GierMischanteil = MesswertGier - (sollGier * STICK_GAIN); // Regler für Gier
1199 
 
 1199 
 
1200 
#define MIN_GIERGAS  ( 40 * STICK_GAIN )  // unter diesem Gaswert trotzdem Gieren
 1200 
#define MIN_GIERGAS  ( 40 * STICK_GAIN )  // unter diesem Gaswert trotzdem Gieren
1201 
 
 1201 
 
1202 
    // Reduzierten Gieranteil berechnen
 1202 
    // Reduzierten Gieranteil berechnen
1203 
    if (GasMischanteil < MIN_GIERGAS) {
 1203 
    if (GasMischanteil < MIN_GIERGAS) {
1204 
        GierMischanteil = (GierMischanteil * GasMischanteil) / MIN_GIERGAS;
 1204 
        GierMischanteil = (GierMischanteil * GasMischanteil) / MIN_GIERGAS;
1205 
    }
 1205 
    }
1206 
 
 1206 
 
1207 
    // Gieranteil darf nicht größer als der halbe Gasanteil sein
 1207 
    // Gieranteil darf nicht größer als der halbe Gasanteil sein
1208 
    if (GierMischanteil > (GasMischanteil >> 1))
 1208 
    if (GierMischanteil > (GasMischanteil >> 1))
1209 
        GierMischanteil = GasMischanteil >> 1;
 1209 
        GierMischanteil = GasMischanteil >> 1;
1210 
    if (GierMischanteil < -(GasMischanteil >> 1))
 1210 
    if (GierMischanteil < -(GasMischanteil >> 1))
1211 
        GierMischanteil = -(GasMischanteil >> 1);
 1211 
        GierMischanteil = -(GasMischanteil >> 1);
1212 
 
 1212 
 
1213 
    tmp_int = MAX_GAS * STICK_GAIN;
 1213 
    tmp_int = MAX_GAS * STICK_GAIN;
1214 
 
 1214 
 
1215 
    // Gieranteil darf die Gasreserve nicht überschreiten
 1215 
    // Gieranteil darf die Gasreserve nicht überschreiten
1216 
    if (GierMischanteil > ((tmp_int - GasMischanteil)))
 1216 
    if (GierMischanteil > ((tmp_int - GasMischanteil)))
1217 
        GierMischanteil = ((tmp_int - GasMischanteil));
 1217 
        GierMischanteil = ((tmp_int - GasMischanteil));
1218 
    if (GierMischanteil < -((tmp_int - GasMischanteil)))
 1218 
    if (GierMischanteil < -((tmp_int - GasMischanteil)))
1219 
        GierMischanteil = -((tmp_int - GasMischanteil));
 1219 
        GierMischanteil = -((tmp_int - GasMischanteil));
1220 
 
 1220 
 
1221 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1221 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1222 
    // Nick-Achse
 1222 
    // Nick-Achse
1223 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1223 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1224 
    DiffNick = MesswertNick - StickNick; // Differenz bestimmen
 1224 
    DiffNick = MesswertNick - StickNick; // Differenz bestimmen
1225 
    if (IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
 1225 
    if (IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung
1226 
    else SummeNick += DiffNick; // I-Anteil bei HH
 1226 
    else SummeNick += DiffNick; // I-Anteil bei HH
1227 
    if (SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
 1227 
    if (SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L);
1228 
    if (SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
 1228 
    if (SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1229 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
 1229 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick
1230 
    // Motor Vorn
 1230 
    // Motor Vorn
1231 
    tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
 1231 
    tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
1232 
    if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
 1232 
    if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
1233 
    if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1233 
    if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1234 
 
 1234 
 
1235 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
 1235 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer
1236 
    motorwert /= STICK_GAIN;
 1236 
    motorwert /= STICK_GAIN;
1237 
    if ((motorwert < 0)) motorwert = 0;
 1237 
    if ((motorwert < 0)) motorwert = 0;
1238 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
 1238 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
1239 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
 1239 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1240 
    Motor_Vorne = motorwert;
 1240 
    Motor_Vorne = motorwert;
1241 
    // Motor Heck
 1241 
    // Motor Heck
1242 
    motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 1242 
    motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
1243 
    motorwert /= STICK_GAIN;
 1243 
    motorwert /= STICK_GAIN;
1244 
    if ((motorwert < 0)) motorwert = 0;
 1244 
    if ((motorwert < 0)) motorwert = 0;
1245 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
 1245 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
1246 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
 1246 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1247 
    Motor_Hinten = motorwert;
 1247 
    Motor_Hinten = motorwert;
1248 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1248 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1249 
    // Roll-Achse
 1249 
    // Roll-Achse
1250 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1250 
    // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1251 
    DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
 1251 
    DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen
1252 
    if (IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll; // I-Anteil bei Winkelregelung
 1252 
    if (IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll; // I-Anteil bei Winkelregelung
1253 
    else SummeRoll += DiffRoll; // I-Anteil bei HH
 1253 
    else SummeRoll += DiffRoll; // I-Anteil bei HH
1254 
    if (SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
 1254 
    if (SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L);
1255 
    if (SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
 1255 
    if (SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1256 
    pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
 1256 
    pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll
1257 
    tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
 1257 
    tmp_int = (long) ((long) Parameter_DynamicStability * (long) (GasMischanteil + abs(GierMischanteil) / 2)) / 64;
1258 
    if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
 1258 
    if (pd_ergebnis > tmp_int) pd_ergebnis = tmp_int;
1259 
    if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1259 
    if (pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1260 
    // Motor Links
 1260 
    // Motor Links
1261 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1261 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1262 
    motorwert /= STICK_GAIN;
 1262 
    motorwert /= STICK_GAIN;
1263 
    if ((motorwert < 0)) motorwert = 0;
 1263 
    if ((motorwert < 0)) motorwert = 0;
1264 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
 1264 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
1265 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
 1265 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1266 
    Motor_Links = motorwert;
 1266 
    Motor_Links = motorwert;
1267 
    // Motor Rechts
 1267 
    // Motor Rechts
1268 
    motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1268 
    motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
1269 
    motorwert /= STICK_GAIN;
 1269 
    motorwert /= STICK_GAIN;
1270 
    if ((motorwert < 0)) motorwert = 0;
 1270 
    if ((motorwert < 0)) motorwert = 0;
1271 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
 1271 
    else if (motorwert > MAX_GAS) motorwert = MAX_GAS;
1272 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
 1272 
    if (motorwert < MIN_GAS) motorwert = MIN_GAS;
1273 
    Motor_Rechts = motorwert;
 1273 
    Motor_Rechts = motorwert;
1274 
    // +++++++++++++++++++++++++++++++++++++++++++++++
 1274 
    // +++++++++++++++++++++++++++++++++++++++++++++++
1275 
}
 1275 
}
1276 
 
 1276 
 
1277 
 
 1277