Subversion Repositories FlightCtrl

Rev

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

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