Subversion Repositories FlightCtrl

Rev

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

Rev 613 Rev 615
1 
/*#######################################################################################
 1 
/*#######################################################################################
2 
Flight Control
 2 
Flight Control
3 
#######################################################################################*/
 3 
#######################################################################################*/
4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 4 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5 
// + Copyright (c) 04.2007 Holger Buss
 5 
// + Copyright (c) 04.2007 Holger Buss
6 
// + Nur für den privaten Gebrauch
 6 
// + Nur für den privaten Gebrauch
7 
// + www.MikroKopter.com
 7 
// + www.MikroKopter.com
8 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 8 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
9 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
9 
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
10 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
10 
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
11 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
11 
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
12 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
12 
// + bzgl. der Nutzungsbedingungen aufzunehmen.
13 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 13 
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
14 
// + Verkauf von Luftbildaufnahmen, usw.
 14 
// + Verkauf von Luftbildaufnahmen, usw.
15 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 15 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
16 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
16 
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
17 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 17 
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
18 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 18 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 19 
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
20 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 20 
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
21 
// + eindeutig als Ursprung verlinkt werden
 21 
// + eindeutig als Ursprung verlinkt werden
22 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 22 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 23 
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
24 
// + Benutzung auf eigene Gefahr
 24 
// + Benutzung auf eigene Gefahr
25 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 25 
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
26 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 26 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
27 
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
27 
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
28 
// + mit unserer Zustimmung zulässig
 28 
// + mit unserer Zustimmung zulässig
29 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 29 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 30 
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 31 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
32 
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
33 
// + this list of conditions and the following disclaimer.
 33 
// + this list of conditions and the following disclaimer.
34 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 34 
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
35 
// +     from this software without specific prior written permission.
 35 
// +     from this software without specific prior written permission.
36 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
36 
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
37 
// +     for non-commercial use (directly or indirectly)
 37 
// +     for non-commercial use (directly or indirectly)
38 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
38 
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
39 
// +     with our written permission
 39 
// +     with our written permission
40 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
40 
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
41 
// +     clearly linked as origin
41 
// +     clearly linked as origin
42 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 42 
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
43 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 43 
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
44 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 44 
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
45 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 45 
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
46 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 46 
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
47 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 47 
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
48 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 48 
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
49 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 49 
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
51 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 51 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
52 
// +  POSSIBILITY OF SUCH DAMAGE.
52 
// +  POSSIBILITY OF SUCH DAMAGE.
53 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 53 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
54 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
 54 
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 6.10.2007
55 
/*
 55 
/*
56 
Driftkompensation fuer Gyros verbessert
 56 
Driftkompensation fuer Gyros verbessert
57 
Linearsensor mit fixem Neutralwert
 57 
Linearsensor mit fixem Neutralwert
58 
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
 58 
Ersatzkompass abgeleitet aus Magnetkompass und Giergyro fuer nahezu neigungsunabhaengige Kompassfunktion
59 
*/
 59 
*/
60 
 
 60 
 
61 
#include "main.h"
 61 
#include "main.h"
62 
#include "eeprom.c"
 62 
#include "eeprom.c"
63 
 
 63 
 
64 
unsigned char h,m,s;
 64 
unsigned char h,m,s;
65 
volatile unsigned int I2CTimeout = 100;
 65 
volatile unsigned int I2CTimeout = 100;
66 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 66 
volatile int MesswertNick,MesswertRoll,MesswertGier;
67 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
 67 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
68 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 68 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
69 
volatile float NeutralAccZ = 0;
 69 
volatile float NeutralAccZ = 0;
70 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 70 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
71 
volatile long IntegralNick = 0,IntegralNick2 = 0;
 71 
volatile long IntegralNick = 0,IntegralNick2 = 0;
72 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
 72 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
73 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
 73 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
74 
volatile long Integral_Gier = 0;
 74 
volatile long Integral_Gier = 0;
75 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 75 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
76 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 76 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
77 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 77 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
78 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
 78 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
79 
volatile long Mess_Integral_Hoch = 0;
 79 
volatile long Mess_Integral_Hoch = 0;
80 
volatile int  KompassValue = 0;
 80 
volatile int  KompassValue = 0;
81 
volatile int  KompassStartwert = 0;
 81 
volatile int  KompassStartwert = 0;
82 
volatile int  KompassRichtung = 0;
 82 
volatile int  KompassRichtung = 0;
83 
unsigned char MAX_GAS,MIN_GAS;
 83 
unsigned char MAX_GAS,MIN_GAS;
84 
unsigned char Notlandung = 0;
 84 
unsigned char Notlandung = 0;
85 
unsigned char HoehenReglerAktiv = 0;
 85 
unsigned char HoehenReglerAktiv = 0;
86 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 86 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
87 
 
 87 
 
88 
//Salvo 12.10.2007
 88 
//Salvo 12.10.2007
89 
uint8_t magkompass_ok=0;
 89 
uint8_t magkompass_ok=0;
90 
uint8_t gps_cmd = GPS_CMD_STOP;
 90 
uint8_t gps_cmd = GPS_CMD_STOP;
91 
static int       ubat_cnt =0;
 91 
static int       ubat_cnt =0;
92 
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
 92 
static int gas_actual,gas_mittel; //Parameter fuer Gasreduzierung bei unterspannung
93 
int w,v;
 93 
int w,v;
94 
//Salvo End
 94 
//Salvo End
95 
 
 95 
 
96 
 //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
 96 
 //Salvo 15.12.2007 Ersatzkompass und Giergyrokompensation
97 
long GyroKomp_Int;
 97 
long GyroKomp_Int;
98 
long int GyroGier_Comp;
 98 
long int GyroGier_Comp;
99 
int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
 99 
int GyroKomp_Value; //  Der ermittelte Kompasswert aus Gyro und Magnetkompass
100 
short int cnt_stickgier_zero =0;
 100 
short int cnt_stickgier_zero =0;
101 
int gyrogier_kompass;
 101 
int gyrogier_kompass;
102 
// Salvo End
 102 
//Salvo End
- 
 
 103 
 
- 
 
 104 
 //Salvo 2.1.2008 Debugvariablen
- 
 
 105 
int debug_gp_0,debug_gp_1,debug_gp_2,debug_gp_3,debug_gp_4,debug_gp_5,debug_gp_6,debug_gp_7; //Allgemeine Debugvariablen
- 
 
 106 
//Salvo End
103 
 
 107 
 
104 
float GyroFaktor;
 108 
float GyroFaktor;
105 
float IntegralFaktor;
 109 
float IntegralFaktor;
106 
volatile int  DiffNick,DiffRoll;
 110 
volatile int  DiffNick,DiffRoll;
107 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 111 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
108 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 112 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
109 
unsigned char MotorWert[5];
 113 
unsigned char MotorWert[5];
110 
volatile unsigned char SenderOkay = 0;
 114 
volatile unsigned char SenderOkay = 0;
111 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 115 
int StickNick = 0,StickRoll = 0,StickGier = 0;
112 
char MotorenEin = 0;
 116 
char MotorenEin = 0;
113 
int HoehenWert = 0;
 117 
int HoehenWert = 0;
114 
int SollHoehe = 0;
 118 
int SollHoehe = 0;
115 
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
 119 
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
116 
float Ki =  FAKTOR_I;
 120 
float Ki =  FAKTOR_I;
117 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
 121 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
118 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
 122 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
119 
 
 123 
 
120 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 124 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
121 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 125 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
122 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 126 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
123 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 127 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
124 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
 128 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
125 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 129 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
126 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 130 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
127 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 131 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
128 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 132 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
129 
unsigned char Parameter_UserParam1 = 0;
 133 
unsigned char Parameter_UserParam1 = 0;
130 
unsigned char Parameter_UserParam2 = 0;
 134 
unsigned char Parameter_UserParam2 = 0;
131 
unsigned char Parameter_UserParam3 = 0;
 135 
unsigned char Parameter_UserParam3 = 0;
132 
unsigned char Parameter_UserParam4 = 0;
 136 
unsigned char Parameter_UserParam4 = 0;
133 
unsigned char Parameter_UserParam5 = 0;
 137 
unsigned char Parameter_UserParam5 = 0;
134 
unsigned char Parameter_UserParam6 = 0;
 138 
unsigned char Parameter_UserParam6 = 0;
135 
unsigned char Parameter_UserParam7 = 0;
 139 
unsigned char Parameter_UserParam7 = 0;
136 
unsigned char Parameter_UserParam8 = 0;
 140 
unsigned char Parameter_UserParam8 = 0;
137 
unsigned char Parameter_ServoNickControl = 100;
 141 
unsigned char Parameter_ServoNickControl = 100;
138 
unsigned char Parameter_LoopGasLimit = 70;
 142 
unsigned char Parameter_LoopGasLimit = 70;
139 
unsigned char Parameter_AchsKopplung1 = 0;
 143 
unsigned char Parameter_AchsKopplung1 = 0;
140 
unsigned char Parameter_AchsGegenKopplung1 = 0;
 144 
unsigned char Parameter_AchsGegenKopplung1 = 0;
141 
unsigned char Parameter_DynamicStability = 100;
 145 
unsigned char Parameter_DynamicStability = 100;
142 
struct mk_param_struct EE_Parameter;
 146 
struct mk_param_struct EE_Parameter;
143 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
 147 
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
144 
 
 148 
 
145 
void Piep(unsigned char Anzahl)
 149 
void Piep(unsigned char Anzahl)
146 
{
 150 
{
147 
 while(Anzahl--)
 151 
 while(Anzahl--)
148 
 {
 152 
 {
149 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 153 
  if(MotorenEin) return; //auf keinen Fall im Flug!
150 
  beeptime = 100;
 154 
  beeptime = 100;
151 
  Delay_ms(250);
 155 
  Delay_ms(250);
152 
 }
 156 
 }
153 
}
 157 
}
154 
 
 158 
 
155 
//############################################################################
 159 
//############################################################################
156 
//  Nullwerte ermitteln
 160 
//  Nullwerte ermitteln
157 
void SetNeutral(void)
 161 
void SetNeutral(void)
158 
//############################################################################
 162 
//############################################################################
159 
{
 163 
{
160 
        // Salvo 9.12.2007
 164 
        // Salvo 9.12.2007
161 
        RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
165 
        RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
162 
        // Salvo End
 166 
        // Salvo End
163 
        NeutralAccX = 0;
 167 
        NeutralAccX = 0;
164 
        NeutralAccY = 0;
 168 
        NeutralAccY = 0;
165 
        NeutralAccZ = 0;
 169 
        NeutralAccZ = 0;
166 
    AdNeutralNick = 0; 
 170 
    AdNeutralNick = 0; 
167 
        AdNeutralRoll = 0;     
 171 
        AdNeutralRoll = 0;     
168 
        AdNeutralGier = 0;
 172 
        AdNeutralGier = 0;
169 
    Parameter_AchsKopplung1 = 0;
 173 
    Parameter_AchsKopplung1 = 0;
170 
    Parameter_AchsGegenKopplung1 = 0;
 174 
    Parameter_AchsGegenKopplung1 = 0;
171 
    CalibrierMittelwert();     
 175 
    CalibrierMittelwert();     
172 
    Delay_ms_Mess(100);
 176 
    Delay_ms_Mess(100);
173 
        CalibrierMittelwert();
 177 
        CalibrierMittelwert();
174 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 178 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
175 
     {    
 179 
     {    
176 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 180 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
177 
     }
 181 
     }
178 
 
 182 
 
179 
     AdNeutralNick= AdWertNick;
 183 
     AdNeutralNick= AdWertNick;
180 
         AdNeutralRoll= AdWertRoll;    
 184 
         AdNeutralRoll= AdWertRoll;    
181 
         AdNeutralGier= AdWertGier;
 185 
         AdNeutralGier= AdWertGier;
182 
     StartNeutralRoll = AdNeutralRoll;
 186 
     StartNeutralRoll = AdNeutralRoll;
183 
     StartNeutralNick = AdNeutralNick;
 187 
     StartNeutralNick = AdNeutralNick;
184 
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
 188 
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
185 
    {
 189 
    {
186 
      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 190 
      NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
187 
          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 191 
          NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
188 
          NeutralAccZ = Aktuell_az;
 192 
          NeutralAccZ = Aktuell_az;
189 
    }
 193 
    }
190 
    else
 194 
    else
191 
    {
 195 
    {
192 
      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
 196 
      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
193 
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
 197 
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
194 
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
 198 
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
195 
    }
 199 
    }
196 
   
 200 
   
197 
        Mess_IntegralNick = 0; 
 201 
        Mess_IntegralNick = 0; 
198 
    Mess_IntegralNick2 = 0;
 202 
    Mess_IntegralNick2 = 0;
199 
    Mess_IntegralRoll = 0;     
 203 
    Mess_IntegralRoll = 0;     
200 
    Mess_IntegralRoll2 = 0;
 204 
    Mess_IntegralRoll2 = 0;
201 
    Mess_Integral_Gier = 0;    
 205 
    Mess_Integral_Gier = 0;    
202 
    MesswertNick = 0;
 206 
    MesswertNick = 0;
203 
    MesswertRoll = 0;
 207 
    MesswertRoll = 0;
204 
    MesswertGier = 0;
 208 
    MesswertGier = 0;
205 
        GyroGier_Comp =0;
 209 
        GyroGier_Comp =0;
206 
    StartLuftdruck = Luftdruck;
 210 
    StartLuftdruck = Luftdruck;
207 
    HoeheD = 0;
 211 
    HoeheD = 0;
208 
    Mess_Integral_Hoch = 0;
 212 
    Mess_Integral_Hoch = 0;
209 
    KompassStartwert = KompassValue;
 213 
    KompassStartwert = KompassValue;
210 
    GPS_Neutral();
 214 
    GPS_Neutral();
211 
    beeptime = 50;  
 215 
    beeptime = 50;  
212 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 216 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
213 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
 217 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
214 
    ExternHoehenValue = 0;
 218 
    ExternHoehenValue = 0;
215 
 
 219 
 
216 
 
 220 
 
217 
//Salvo 13.10.2007 Ersatzkompass und Gas
 221 
//Salvo 13.10.2007 Ersatzkompass und Gas
218 
        GyroKomp_Int =  KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
 222 
        GyroKomp_Int =  KompassValue * GYROKOMP_INC_GRAD_DEFAULT; //Neu ab 3.1.2007
219 
        gas_mittel      =       30;
 223 
        gas_mittel      =       30;
220 
        gas_actual      =       gas_mittel;
 224 
        gas_actual      =       gas_mittel;
221 
// Salvo End
 225 
// Salvo End
222 
}
 226 
}
223 
 
 227 
 
224 
//############################################################################
 228 
//############################################################################
225 
// Bearbeitet die Messwerte
 229 
// Bearbeitet die Messwerte
226 
void Mittelwert(void)
 230 
void Mittelwert(void)
227 
//############################################################################
 231 
//############################################################################
228 
{      
 232 
{      
229 
    static signed long tmpl,tmpl2;     
 233 
    static signed long tmpl,tmpl2;     
230 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
 234 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
231 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
 235 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
232 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
 236 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
233 
 
 237 
 
234 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
 238 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
235 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
 239 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
236 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
 240 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
237 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
 241 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
238 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
 242 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
239 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
 243 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
240 
    IntegralAccZ    += Aktuell_az - NeutralAccZ;
 244 
    IntegralAccZ    += Aktuell_az - NeutralAccZ;
241 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
 245 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
242 
//Salvo 12.11.2007
 246 
//Salvo 12.11.2007
243 
                        GyroKomp_Int   += (long)MesswertGier;
 247 
                        GyroKomp_Int   += (long)MesswertGier;
244 
                        GyroGier_Comp  += (long)MesswertGier;
 248 
                        GyroGier_Comp  += (long)MesswertGier;
245 
//Salvo End
 249 
//Salvo End
246 
            Mess_Integral_Gier +=  MesswertGier;
 250 
            Mess_Integral_Gier +=  MesswertGier;
247 
            Mess_Integral_Gier2 += MesswertGier;
 251 
            Mess_Integral_Gier2 += MesswertGier;
248 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 252 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
249 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
 253 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
250 
         {
 254 
         {
251 
            tmpl = Mess_IntegralNick / 4096L;
 255 
            tmpl = Mess_IntegralNick / 4096L;
252 
            tmpl *= MesswertGier;
 256 
            tmpl *= MesswertGier;
253 
            tmpl *= Parameter_AchsKopplung1;  //125
 257 
            tmpl *= Parameter_AchsKopplung1;  //125
254 
            tmpl /= 2048L;
 258 
            tmpl /= 2048L;
255 
            tmpl2 = Mess_IntegralRoll / 4096L;
 259 
            tmpl2 = Mess_IntegralRoll / 4096L;
256 
            tmpl2 *= MesswertGier;
 260 
            tmpl2 *= MesswertGier;
257 
            tmpl2 *= Parameter_AchsKopplung1;
 261 
            tmpl2 *= Parameter_AchsKopplung1;
258 
            tmpl2 /= 2048L;
 262 
            tmpl2 /= 2048L;
259 
         }
 263 
         }
260 
      else  tmpl = tmpl2 = 0;
 264 
      else  tmpl = tmpl2 = 0;
261 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
 265 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
262 
            MesswertRoll += tmpl;
 266 
            MesswertRoll += tmpl;
263 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
 267 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
264 
            Mess_IntegralRoll2 += MesswertRoll;
 268 
            Mess_IntegralRoll2 += MesswertRoll;
265 
            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
 269 
            Mess_IntegralRoll +=  MesswertRoll - LageKorrekturRoll;
266 
            if(Mess_IntegralRoll > Umschlag180Roll)
 270 
            if(Mess_IntegralRoll > Umschlag180Roll)
267 
            {
 271 
            {
268 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
 272 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
269 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 273 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
270 
            }
 274 
            }
271 
            if(Mess_IntegralRoll <-Umschlag180Roll)
 275 
            if(Mess_IntegralRoll <-Umschlag180Roll)
272 
            {
 276 
            {
273 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
 277 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
274 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
 278 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
275 
            }  
 279 
            }  
276 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
 280 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
277 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
 281 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
278 
            if(PlatinenVersion == 10)
 282 
            if(PlatinenVersion == 10)
279 
                         {
 283 
                         {
280 
              if(AdWertRoll > 1010) MesswertRoll = +1000;
 284 
              if(AdWertRoll > 1010) MesswertRoll = +1000;
281 
              if(AdWertRoll > 1017) MesswertRoll = +2000;
 285 
              if(AdWertRoll > 1017) MesswertRoll = +2000;
282 
                         }
 286 
                         }
283 
                         else
 287 
                         else
284 
                         {
 288 
                         {
285 
              if(AdWertRoll > 2020) MesswertRoll = +1000;
 289 
              if(AdWertRoll > 2020) MesswertRoll = +1000;
286 
              if(AdWertRoll > 2034) MesswertRoll = +2000;
 290 
              if(AdWertRoll > 2034) MesswertRoll = +2000;
287 
                         }
 291 
                         }
288 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
 292 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
289 
            MesswertNick -= tmpl2;
 293 
            MesswertNick -= tmpl2;
290 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
 294 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
291 
            Mess_IntegralNick2 += MesswertNick;
 295 
            Mess_IntegralNick2 += MesswertNick;
292 
            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
 296 
            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;
293 
            if(Mess_IntegralNick > Umschlag180Nick)
 297 
            if(Mess_IntegralNick > Umschlag180Nick)
294 
            {
 298 
            {
295 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
 299 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
296 
             Mess_IntegralNick2 = Mess_IntegralNick;
 300 
             Mess_IntegralNick2 = Mess_IntegralNick;
297 
            }
 301 
            }
298 
            if(Mess_IntegralNick <-Umschlag180Nick)
 302 
            if(Mess_IntegralNick <-Umschlag180Nick)
299 
            {
 303 
            {
300 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
 304 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
301 
             Mess_IntegralNick2 = Mess_IntegralNick;
 305 
             Mess_IntegralNick2 = Mess_IntegralNick;
302 
            }
 306 
            }
303 
            if(AdWertNick < 15)   MesswertNick = -1000;
 307 
            if(AdWertNick < 15)   MesswertNick = -1000;
304 
            if(AdWertNick <  7)   MesswertNick = -2000;
 308 
            if(AdWertNick <  7)   MesswertNick = -2000;
305 
            if(PlatinenVersion == 10)
 309 
            if(PlatinenVersion == 10)
306 
                         {
 310 
                         {
307 
              if(AdWertNick > 1010) MesswertNick = +1000;
 311 
              if(AdWertNick > 1010) MesswertNick = +1000;
308 
              if(AdWertNick > 1017) MesswertNick = +2000;
 312 
              if(AdWertNick > 1017) MesswertNick = +2000;
309 
                         }
 313 
                         }
310 
                         else
 314 
                         else
311 
                         {
 315 
                         {
312 
              if(AdWertNick > 2020) MesswertNick = +1000;
 316 
              if(AdWertNick > 2020) MesswertNick = +1000;
313 
              if(AdWertNick > 2034) MesswertNick = +2000;
 317 
              if(AdWertNick > 2034) MesswertNick = +2000;
314 
                         }
 318 
                         }
315 
//++++++++++++++++++++++++++++++++++++++++++++++++
 319 
//++++++++++++++++++++++++++++++++++++++++++++++++
316 
// ADC einschalten
 320 
// ADC einschalten
317 
    ANALOG_ON; 
 321 
    ANALOG_ON; 
318 
//++++++++++++++++++++++++++++++++++++++++++++++++
 322 
//++++++++++++++++++++++++++++++++++++++++++++++++
319 
 
 323 
 
320 
    Integral_Gier  = Mess_Integral_Gier;
 324 
    Integral_Gier  = Mess_Integral_Gier;
321 
    IntegralNick = Mess_IntegralNick;
 325 
    IntegralNick = Mess_IntegralNick;
322 
    IntegralRoll = Mess_IntegralRoll;
 326 
    IntegralRoll = Mess_IntegralRoll;
323 
    IntegralNick2 = Mess_IntegralNick2;
 327 
    IntegralNick2 = Mess_IntegralNick2;
324 
    IntegralRoll2 = Mess_IntegralRoll2;
 328 
    IntegralRoll2 = Mess_IntegralRoll2;
325 
 
 329 
 
326 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
 330 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
327 
  {
 331 
  {
328 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
 332 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
329 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 333 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
330 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
 334 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
331 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 335 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
332 
  }
 336 
  }
333 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 337 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
334 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 338 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
335 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 339 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
336 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 340 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
337 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 341 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
338 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 342 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
339 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 343 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
340 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 344 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
341 
}
 345 
}
342 
 
 346 
 
343 
//############################################################################
 347 
//############################################################################
344 
// Messwerte beim Ermitteln der Nullage
 348 
// Messwerte beim Ermitteln der Nullage
345 
void CalibrierMittelwert(void)
 349 
void CalibrierMittelwert(void)
346 
//############################################################################
 350 
//############################################################################
347 
{                
 351 
{                
348 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 352 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
349 
        ANALOG_OFF;
 353 
        ANALOG_OFF;
350 
        MesswertNick = AdWertNick;
 354 
        MesswertNick = AdWertNick;
351 
        MesswertRoll = AdWertRoll;
 355 
        MesswertRoll = AdWertRoll;
352 
        MesswertGier = AdWertGier;
 356 
        MesswertGier = AdWertGier;
353 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
 357 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
354 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
 358 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
355 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
 359 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
356 
   // ADC einschalten
 360 
   // ADC einschalten
357 
    ANALOG_ON; 
 361 
    ANALOG_ON; 
358 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 362 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
359 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 363 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
360 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 364 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
361 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 365 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
362 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 366 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
363 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 367 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
364 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 368 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
365 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 369 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
366 
 
 370 
 
367 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 371 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
368 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 372 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
369 
}
 373 
}
370 
 
 374 
 
371 
//############################################################################
 375 
//############################################################################
372 
// Senden der Motorwerte per I2C-Bus
 376 
// Senden der Motorwerte per I2C-Bus
373 
void SendMotorData(void)
 377 
void SendMotorData(void)
374 
//############################################################################
 378 
//############################################################################
375 
{
 379 
{
376 
    if(MOTOR_OFF || !MotorenEin)
 380 
    if(MOTOR_OFF || !MotorenEin)
377 
        {
 381 
        {
378 
        Motor_Hinten = 0;
 382 
        Motor_Hinten = 0;
379 
        Motor_Vorne = 0;
 383 
        Motor_Vorne = 0;
380 
        Motor_Rechts = 0;
 384 
        Motor_Rechts = 0;
381 
        Motor_Links = 0;
 385 
        Motor_Links = 0;
382 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 386 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
383 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 387 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
384 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 388 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
385 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 389 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
386 
        }
 390 
        }
387 
 
 391 
 
388 
    DebugOut.Analog[12] = Motor_Vorne;
 392 
/*    DebugOut.Analog[12] = Motor_Vorne;
389 
    DebugOut.Analog[13] = Motor_Hinten;
 393 
    DebugOut.Analog[13] = Motor_Hinten;
390 
    DebugOut.Analog[14] = Motor_Links;
 394 
    DebugOut.Analog[14] = Motor_Links;
391 
    DebugOut.Analog[15] = Motor_Rechts;  
 395 
    DebugOut.Analog[15] = Motor_Rechts;  
392 
 
 396 
*/
393 
    //Start I2C Interrupt Mode
 397 
    //Start I2C Interrupt Mode
394 
    twi_state = 0;
 398 
    twi_state = 0;
395 
    motor = 0;
 399 
    motor = 0;
396 
    i2c_start();
 400 
    i2c_start();
397 
}
 401 
}
398 
 
 402 
 
399 
 
 403 
 
400 
//############################################################################
 404 
//############################################################################
401 
// Trägt ggf. das Poti als Parameter ein
 405 
// Trägt ggf. das Poti als Parameter ein
402 
void ParameterZuordnung(void)
 406 
void ParameterZuordnung(void)
403 
//############################################################################
 407 
//############################################################################
404 
{
 408 
{
405 
 
 409 
 
406 
 #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;}
 410 
 #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 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 411 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
408 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 412 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
409 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 413 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
410 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 414 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
411 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 415 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
412 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 416 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
413 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 417 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
414 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 418 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
415 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 419 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
416 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 420 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
417 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 421 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
418 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 422 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
419 
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
 423 
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
420 
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
 424 
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
421 
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
 425 
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
422 
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
 426 
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
423 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 427 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
424 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 428 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
425 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
 429 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
426 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
 430 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
427 
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
 431 
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
428 
 
 432 
 
429 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 433 
 Ki = (float) Parameter_I_Faktor * 0.0001;
430 
 MAX_GAS = EE_Parameter.Gas_Max;
 434 
 MAX_GAS = EE_Parameter.Gas_Max;
431 
 MIN_GAS = EE_Parameter.Gas_Min;
 435 
 MIN_GAS = EE_Parameter.Gas_Min;
432 
}
 436 
}
433 
 
 437 
 
434 
 
 438 
 
435 
//############################################################################
 439 
//############################################################################
436 
//
 440 
//
437 
void MotorRegler(void)
 441 
void MotorRegler(void)
438 
//############################################################################
 442 
//############################################################################
439 
{
 443 
{
440 
         int motorwert,pd_ergebnis,h,tmp_int;
 444 
         int motorwert,pd_ergebnis,h,tmp_int;
441 
         int GierMischanteil,GasMischanteil;
 445 
         int GierMischanteil,GasMischanteil;
442 
     static long SummeNick=0,SummeRoll=0;
 446 
     static long SummeNick=0,SummeRoll=0;
443 
     static long sollGier = 0,tmp_long,tmp_long2;
 447 
     static long sollGier = 0,tmp_long,tmp_long2;
444 
     static long IntegralFehlerNick = 0;
 448 
     static long IntegralFehlerNick = 0;
445 
     static long IntegralFehlerRoll = 0;
 449 
     static long IntegralFehlerRoll = 0;
446 
         static unsigned int RcLostTimer;
 450 
         static unsigned int RcLostTimer;
447 
         static unsigned char delay_neutral = 0;
 451 
         static unsigned char delay_neutral = 0;
448 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 452 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
449 
         static unsigned int  modell_fliegt = 0;
 453 
         static unsigned int  modell_fliegt = 0;
450 
     static int hoehenregler = 0;
 454 
     static int hoehenregler = 0;
451 
     static char TimerWerteausgabe = 0;
 455 
     static char TimerWerteausgabe = 0;
452 
     static char NeueKompassRichtungMerken = 0;
 456 
     static char NeueKompassRichtungMerken = 0;
453 
     static long ausgleichNick, ausgleichRoll;
 457 
     static long ausgleichNick, ausgleichRoll;
454 
     
 458 
     
455 
        Mittelwert();
 459 
        Mittelwert();
456 
//****** GPS Daten holen ***************
 460 
//****** GPS Daten holen ***************
457 
        short int n;
 461 
        short int n;
458 
        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
 462 
        if (gps_alive_cnt > 0) gps_alive_cnt--; //Dekrementieren. Wenn 0 kommen keine ausreichenden GPS Meldungen (Timeout)
459 
        n = Get_Rel_Position();
 463 
        n = Get_Rel_Position();
460 
        if (n == 0)    
 464 
        if (n == 0)    
461 
        {
 465 
        {
462 
                ROT_ON; //led blitzen lassen
 466 
                ROT_ON; //led blitzen lassen
463 
        }
 467 
        }
464 
//******PROVISORISCH***************
 468 
//******PROVISORISCH***************
465 
    GRN_ON;
 469 
    GRN_ON;
466 
 
 470 
 
467 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
471 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
468 
// Gaswert ermitteln
 472 
// Gaswert ermitteln
469 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
473 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
470 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 474 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
471 
    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
 475 
    if(GasMischanteil > MAX_GAS - 20) GasMischanteil = MAX_GAS - 20;
472 
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
 476 
//Salvo 13.10.2007 langsame Gasreduktion bei Unterspannung. Als Ausgangswert wird der bei UBAT=k gemessen Mittelwert genommen
473 
// und dieser dann langsam zwangsweise reduziert
 477 
// und dieser dann langsam zwangsweise reduziert
474 
        if (UBat <= EE_Parameter.UnterspannungsWarnung - 4)     //Unterhalb der Piepser Schwelle aktivieren
 478 
        if (UBat <= EE_Parameter.UnterspannungsWarnung - 4)     //Unterhalb der Piepser Schwelle aktivieren
475 
        {
 479 
        {
476 
                if (ubat_cnt > 1000)
 480 
                if (ubat_cnt > 1000)
477 
                {
 481 
                {
478 
                        ubat_cnt = 0;
 482 
                        ubat_cnt = 0;
479 
                        if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
 483 
                        if (gas_actual > ((gas_mittel*12)/15)) gas_actual--;
480 
                }
 484 
                }
481 
                else ubat_cnt++;
 485 
                else ubat_cnt++;
482 
                if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
 486 
                if (GasMischanteil > gas_actual) GasMischanteil = gas_actual;
483 
        }
 487 
        }
484 
        else   //Falls UBAT wieder ok ist
 488 
        else   //Falls UBAT wieder ok ist
485 
        {
 489 
        {
486 
                if (ubat_cnt > 1000)
 490 
                if (ubat_cnt > 1000)
487 
                {
 491 
                {
488 
                        gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
 492 
                        gas_mittel = ((gas_mittel*9) + GasMischanteil)/10; //Filtern
489 
                        gas_actual = GasMischanteil;
 493 
                        gas_actual = GasMischanteil;
490 
                }
 494 
                }
491 
                else
 495 
                else
492 
                {
 496 
                {
493 
                        ubat_cnt++;
 497 
                        ubat_cnt++;
494 
                        if ((ubat_cnt % 10) == 0)
 498 
                        if ((ubat_cnt % 10) == 0)
495 
                        {
 499 
                        {
496 
                                if (gas_actual < GasMischanteil) gas_actual++;
 500 
                                if (gas_actual < GasMischanteil) gas_actual++;
497 
                                else gas_actual = GasMischanteil;
 501 
                                else gas_actual = GasMischanteil;
498 
                }
 502 
                }
499 
                }
 503 
                }
500 
                GasMischanteil = gas_actual;
 504 
                GasMischanteil = gas_actual;
501 
        }      
 505 
        }      
502 
// Salvo End
 506 
// Salvo End
503 
    if(GasMischanteil < 0) GasMischanteil = 0;
 507 
    if(GasMischanteil < 0) GasMischanteil = 0;
504 
 
 508 
 
505 
 
 509 
 
506 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
510 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
507 
// Empfang schlecht
 511 
// Empfang schlecht
508 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
512 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
509 
   if(SenderOkay < 100)
 513 
   if(SenderOkay < 100)
510 
        {
 514 
        {
511 
        if(!PcZugriff)
 515 
        if(!PcZugriff)
512 
         {
 516 
         {
513 
           if(BeepMuster == 0xffff)
 517 
           if(BeepMuster == 0xffff)
514 
            {
 518 
            {
515 
             beeptime = 15000;
 519 
             beeptime = 15000;
516 
             BeepMuster = 0x0c00;
 520 
             BeepMuster = 0x0c00;
517 
            }
 521 
            }
518 
         }
 522 
         }
519 
        if(RcLostTimer) RcLostTimer--;
 523 
        if(RcLostTimer) RcLostTimer--;
520 
        else
 524 
        else
521 
         {
 525 
         {
522 
          MotorenEin = 0;
 526 
          MotorenEin = 0;
523 
          Notlandung = 0;
 527 
          Notlandung = 0;
524 
         }
 528 
         }
525 
        ROT_ON;
 529 
        ROT_ON;
526 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 530 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
527 
            {
 531 
            {
528 
            GasMischanteil = EE_Parameter.NotGas;
 532 
            GasMischanteil = EE_Parameter.NotGas;
529 
            Notlandung = 1;
 533 
            Notlandung = 1;
530 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 534 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
531 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 535 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
532 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 536 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
533 
            }
 537 
            }
534 
         else MotorenEin = 0;
 538 
         else MotorenEin = 0;
535 
        }
 539 
        }
536 
        else
 540 
        else
537 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
541 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
538 
// Emfang gut
 542 
// Emfang gut
539 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
543 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
540 
        if(SenderOkay > 140)
 544 
        if(SenderOkay > 140)
541 
            {
 545 
            {
542 
            Notlandung = 0;
 546 
            Notlandung = 0;
543 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 547 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
544 
            if(GasMischanteil > 40)
 548 
            if(GasMischanteil > 40)
545 
                {
 549 
                {
546 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 550 
                if(modell_fliegt < 0xffff) modell_fliegt++;
547 
                }
 551 
                }
548 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 552 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
549 
                {
 553 
                {
550 
                SummeNick = 0;
 554 
                SummeNick = 0;
551 
                SummeRoll = 0;
 555 
                SummeRoll = 0;
552 
                Mess_Integral_Gier = 0;
 556 
                Mess_Integral_Gier = 0;
553 
                Mess_Integral_Gier2 = 0;
 557 
                Mess_Integral_Gier2 = 0;
554 
                }
 558 
                }
555 
            if((GasMischanteil > 200) && MotorenEin == 0)
 559 
            if((GasMischanteil > 200) && MotorenEin == 0)
556 
                {
 560 
                {
557 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
561 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
558 
// auf Nullwerte kalibrieren
 562 
// auf Nullwerte kalibrieren
559 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
563 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
560 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 564 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
561 
                    {
 565 
                    {
562 
                    if(++delay_neutral > 200)  // nicht sofort
 566 
                    if(++delay_neutral > 200)  // nicht sofort
563 
                        {
 567 
                        {
564 
                        GRN_OFF;
 568 
                        GRN_OFF;
565 
                        SetNeutral();
 569 
                        SetNeutral();
566 
                        MotorenEin = 0;
 570 
                        MotorenEin = 0;
567 
                        delay_neutral = 0;
 571 
                        delay_neutral = 0;
568 
                        modell_fliegt = 0;
 572 
                        modell_fliegt = 0;
569 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 573 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
570 
                        {
 574 
                        {
571 
                         unsigned char setting=1;
 575 
                         unsigned char setting=1;
572 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 576 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
573 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 577 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
574 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 578 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
575 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 579 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
576 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 580 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
577 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 581 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
578 
                        }
 582 
                        }
579 
 
 583 
 
580 
 
 584 
 
581 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 585 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
582 
 
 586 
 
583 
                        Piep(GetActiveParamSetNumber());
 587 
                        Piep(GetActiveParamSetNumber());
584 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 588 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
585 
                        {
 589 
                        {
586 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 590 
                          if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
587 
                        }  
 591 
                        }  
588 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
 592 
                                                GPS_Save_Home(); //Daten sind jetzt hoffentlich verfuegbar
589 
                                                if (gps_home_position.status > 0 )
 593 
                                                if (gps_home_position.status > 0 )
590 
                                                {
 594 
                                                {
591 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
 595 
                                                        Delay_ms(1000);  //akustisch verkuenden dass GPS Home Daten da sind
592 
                                                        beeptime = 1000;
 596 
                                                        beeptime = 1000;
593 
                                                        Delay_ms(500);
 597 
                                                        Delay_ms(500);
594 
                                                }
 598 
                                                }
595 
                       }
 599 
                       }
596 
                    }
 600 
                    }
597 
                 else
 601 
                 else
598 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
 602 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
599 
                    {
 603 
                    {
600 
                    if(++delay_neutral > 200)  // nicht sofort
 604 
                    if(++delay_neutral > 200)  // nicht sofort
601 
                        {
 605 
                        {
602 
                        GRN_OFF;
 606 
                        GRN_OFF;
603 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
 607 
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
604 
                        MotorenEin = 0;
 608 
                        MotorenEin = 0;
605 
                        delay_neutral = 0;
 609 
                        delay_neutral = 0;
606 
                        modell_fliegt = 0;
 610 
                        modell_fliegt = 0;
607 
                        SetNeutral();
 611 
                        SetNeutral();
608 
                                                if (ACC_FIXED > 0)
 612 
                                                if (ACC_FIXED > 0)
609 
                                                {
 613 
                                                {
610 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],ACC_NICK_NEUTRAL / 256); // ACC-NeutralWerte speichern
 614 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],ACC_NICK_NEUTRAL / 256); // ACC-NeutralWerte speichern
611 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],ACC_NICK_NEUTRAL % 256); // ACC-NeutralWerte speichern
 615 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],ACC_NICK_NEUTRAL % 256); // ACC-NeutralWerte speichern
612 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],ACC_ROLL_NEUTRAL / 256);
 616 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],ACC_ROLL_NEUTRAL / 256);
613 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],ACC_ROLL_NEUTRAL % 256);
 617 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],ACC_ROLL_NEUTRAL % 256);
614 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)ACC_Z_NEUTRAL / 256);
 618 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)ACC_Z_NEUTRAL / 256);
615 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)ACC_Z_NEUTRAL % 256);
 619 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)ACC_Z_NEUTRAL % 256);
616 
                                                }
 620 
                                                }
617 
                                                else
 621 
                                                else
618 
                                                {
 622 
                                                {
619 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
 623 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
620 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
 624 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
621 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
 625 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
622 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
 626 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
623 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
 627 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
624 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
 628 
                                eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
625 
                                                }
 629 
                                                }
626 
                        Piep(GetActiveParamSetNumber());
 630 
                        Piep(GetActiveParamSetNumber());
627 
                        }
 631 
                        }
628 
                    }
 632 
                    }
629 
 
 633 
 
630 
                 else delay_neutral = 0;
 634 
                 else delay_neutral = 0;
631 
                }
 635 
                }
632 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
636 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
633 
// Gas ist unten
 637 
// Gas ist unten
634 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
638 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
635 
            if(GasMischanteil < 35)
 639 
            if(GasMischanteil < 35)
636 
                {
 640 
                {
637 
                // Starten
 641 
                // Starten
638 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 642 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
639 
                    {
 643 
                    {
640 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
644 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
641 
// Einschalten
 645 
// Einschalten
642 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
646 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
643 
                    if(++delay_einschalten > 200)
 647 
                    if(++delay_einschalten > 200)
644 
                        {
 648 
                        {
645 
                                                int n;
 649 
                                                int n;
646 
                                                // Salvo 9.12.2007
 650 
                                                // Salvo 9.12.2007
647 
                                                RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
651 
                                                RX_SWTCH_ON; //GPS Daten auf RX eingang schalten
648 
                                                // Salvo End
 652 
                                                // Salvo End
649 
                        delay_einschalten = 200;
 653 
                        delay_einschalten = 200;
650 
                        modell_fliegt = 1;
 654 
                        modell_fliegt = 1;
651 
                        MotorenEin = 1;
 655 
                        MotorenEin = 1;
652 
                        sollGier = 0;
 656 
                        sollGier = 0;
653 
                        Mess_Integral_Gier = 0;
 657 
                        Mess_Integral_Gier = 0;
654 
                        Mess_Integral_Gier2 = 0;
 658 
                        Mess_Integral_Gier2 = 0;
655 
                        Mess_IntegralNick = 0;
 659 
                        Mess_IntegralNick = 0;
656 
                        Mess_IntegralRoll = 0;
 660 
                        Mess_IntegralRoll = 0;
657 
                        Mess_IntegralNick2 = IntegralNick;
 661 
                        Mess_IntegralNick2 = IntegralNick;
658 
                        Mess_IntegralRoll2 = IntegralRoll;
 662 
                        Mess_IntegralRoll2 = IntegralRoll;
659 
                        SummeNick = 0;
 663 
                        SummeNick = 0;
660 
                        SummeRoll = 0;
 664 
                        SummeRoll = 0;
661 
                                                n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
 665 
                                                n= GPS_CRTL(GPS_CMD_STOP); //GPS Lageregelung beenden
662 
                        }          
 666 
                        }          
663 
                    }  
 667 
                    }  
664 
                    else delay_einschalten = 0;
 668 
                    else delay_einschalten = 0;
665 
                //Auf Neutralwerte setzen
 669 
                //Auf Neutralwerte setzen
666 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
670 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
667 
// Auschalten
 671 
// Auschalten
668 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
672 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
669 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 673 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
670 
                    {
 674 
                    {
671 
                    if(++delay_ausschalten > 200)  // nicht sofort
 675 
                    if(++delay_ausschalten > 200)  // nicht sofort
672 
                        {
 676 
                        {
673 
                                                // Salvo 9.12.2007
 677 
                                                // Salvo 9.12.2007
674 
                                                RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
678 
                                                RX_SWTCH_OFF; //Bluetooth Daten auf RX eingang schalten
675 
                                                // Salvo End
 679 
                                                // Salvo End
676 
                        MotorenEin = 0;
 680 
                        MotorenEin = 0;
677 
                        delay_ausschalten = 200;
 681 
                        delay_ausschalten = 200;
678 
                        modell_fliegt = 0;
 682 
                        modell_fliegt = 0;
679 
                        }
 683 
                        }
680 
                    }
 684 
                    }
681 
                else delay_ausschalten = 0;
 685 
                else delay_ausschalten = 0;
682 
                }
 686 
                }
683 
            }
 687 
            }
684 
 
 688 
 
685 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
689 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
686 
// neue Werte von der Funke
 690 
// neue Werte von der Funke
687 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
691 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
688 
 if(!NewPpmData-- || Notlandung)  
 692 
 if(!NewPpmData-- || Notlandung)  
689 
  {
 693 
  {
690 
    int tmp_int;
 694 
    int tmp_int;
691 
    ParameterZuordnung();
 695 
    ParameterZuordnung();
692 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 696 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
693 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 697 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
694 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 698 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
695 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 699 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
696 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 700 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
697 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 701 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
698 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 702 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
699 
 
 703 
 
700 
#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
 704 
#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
701 
if(DubWiseKeys[1]) beeptime = 10;
 705 
if(DubWiseKeys[1]) beeptime = 10;
702 
    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
 706 
    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
703 
    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
 707 
    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
704 
    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
 708 
    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
705 
    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
 709 
    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
706 
    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
 710 
    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
707 
    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
 711 
    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
708 
 
 712 
 
709 
    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
 713 
    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
710 
    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
 714 
    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
711 
    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
 715 
    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
712 
    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
 716 
    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
713 
 
 717 
 
714 
    StickNick += ExternStickNick / 8;
 718 
    StickNick += ExternStickNick / 8;
715 
    StickRoll += ExternStickRoll / 8;
 719 
    StickRoll += ExternStickRoll / 8;
716 
    StickGier += ExternStickGier;
 720 
    StickGier += ExternStickGier;
717 
 
 721 
 
718 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 722 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
719 
    if(GyroFaktor < 0) GyroFaktor = 0;
 723 
    if(GyroFaktor < 0) GyroFaktor = 0;
720 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 724 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
721 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
 725 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
722 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 726 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
723 
     {
 727 
     {
724 
      if(IntegralNick >  60000)  
 728 
      if(IntegralNick >  60000)  
725 
      {
 729 
      {
726 
       StickNick -=  8 * EE_Parameter.Stick_P;      
 730 
       StickNick -=  8 * EE_Parameter.Stick_P;      
727 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 731 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
728 
      }
 732 
      }
729 
      else
 733 
      else
730 
      if(IntegralNick < -60000)  
 734 
      if(IntegralNick < -60000)  
731 
      {
 735 
      {
732 
       StickNick += 8 * EE_Parameter.Stick_P;
 736 
       StickNick += 8 * EE_Parameter.Stick_P;
733 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 737 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
734 
      }
 738 
      }
735 
      if(IntegralRoll >  60000)  
 739 
      if(IntegralRoll >  60000)  
736 
      {
 740 
      {
737 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
 741 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
738 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 742 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
739 
      }
 743 
      }
740 
      else
 744 
      else
741 
      if(IntegralRoll < -60000)  
 745 
      if(IntegralRoll < -60000)  
742 
      {
 746 
      {
743 
       StickRoll += 8 * EE_Parameter.Stick_P;
 747 
       StickRoll += 8 * EE_Parameter.Stick_P;
744 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 748 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
745 
      }
 749 
      }
746 
     }
 750 
     }
747 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
751 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
748 
// Looping?
 752 
// Looping?
749 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
753 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
750 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
 754 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
751 
  else
 755 
  else
752 
   {
 756 
   {
753 
     {
 757 
     {
754 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
 758 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
755 
     }  
 759 
     }  
756 
   }
 760 
   }
757 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
 761 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
758 
   else
 762 
   else
759 
   {
 763 
   {
760 
   if(Looping_Rechts) // Hysterese
 764 
   if(Looping_Rechts) // Hysterese
761 
     {
 765 
     {
762 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
 766 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
763 
     }
 767 
     }
764 
   }
 768 
   }
765 
 
 769 
 
766 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
 770 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
767 
  else
 771 
  else
768 
   {
 772 
   {
769 
    if(Looping_Oben)  // Hysterese
 773 
    if(Looping_Oben)  // Hysterese
770 
     {
 774 
     {
771 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
 775 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
772 
     }  
 776 
     }  
773 
   }
 777 
   }
774 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
 778 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
775 
   else
 779 
   else
776 
   {
 780 
   {
777 
    if(Looping_Unten) // Hysterese
 781 
    if(Looping_Unten) // Hysterese
778 
     {
 782 
     {
779 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
 783 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
780 
     }
 784 
     }
781 
   }
 785 
   }
782 
 
 786 
 
783 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
 787 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
784 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
 788 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
785 
  } // Ende neue Funken-Werte
 789 
  } // Ende neue Funken-Werte
786 
 
 790 
 
787 
  if(Looping_Roll) beeptime = 100;
 791 
  if(Looping_Roll) beeptime = 100;
788 
  if(Looping_Roll || Looping_Nick)
 792 
  if(Looping_Roll || Looping_Nick)
789 
   {
 793 
   {
790 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 794 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
791 
   }
 795 
   }
792 
   
 796 
   
793 
 
 797 
 
794 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
798 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
795 
// Bei Empfangsausfall im Flug
799 
// Bei Empfangsausfall im Flug
796 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
800 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
797 
   if(Notlandung)
 801 
   if(Notlandung)
798 
    {
 802 
    {
799 
     StickGier = 0;
 803 
     StickGier = 0;
800 
     StickNick = 0;
 804 
     StickNick = 0;
801 
     StickRoll = 0;
 805 
     StickRoll = 0;
802 
     GyroFaktor  = 0.1;
 806 
     GyroFaktor  = 0.1;
803 
     IntegralFaktor = 0.005;
 807 
     IntegralFaktor = 0.005;
804 
     Looping_Roll = 0;
 808 
     Looping_Roll = 0;
805 
     Looping_Nick = 0;
 809 
     Looping_Nick = 0;
806 
    }  
 810 
    }  
807 
 
 811 
 
808 
 
 812 
 
809 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
813 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
810 
// Integrale auf ACC-Signal abgleichen
 814 
// Integrale auf ACC-Signal abgleichen
811 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
815 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
812 
#define ABGLEICH_ANZAHL 256L
 816 
#define ABGLEICH_ANZAHL 256L
813 
 
 817 
 
814 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
 818 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
815 
 MittelIntegralRoll  += IntegralRoll;
 819 
 MittelIntegralRoll  += IntegralRoll;
816 
 MittelIntegralNick2 += IntegralNick2;
 820 
 MittelIntegralNick2 += IntegralNick2;
817 
 MittelIntegralRoll2 += IntegralRoll2;
 821 
 MittelIntegralRoll2 += IntegralRoll2;
818 
 
 822 
 
819 
 if(Looping_Nick || Looping_Roll)
 823 
 if(Looping_Nick || Looping_Roll)
820 
  {
 824 
  {
821 
    IntegralAccNick = 0;
 825 
    IntegralAccNick = 0;
822 
    IntegralAccRoll = 0;
 826 
    IntegralAccRoll = 0;
823 
    MittelIntegralNick = 0;
 827 
    MittelIntegralNick = 0;
824 
    MittelIntegralRoll = 0;
 828 
    MittelIntegralRoll = 0;
825 
    MittelIntegralNick2 = 0;
 829 
    MittelIntegralNick2 = 0;
826 
    MittelIntegralRoll2 = 0;
 830 
    MittelIntegralRoll2 = 0;
827 
    Mess_IntegralNick2 = Mess_IntegralNick;
 831 
    Mess_IntegralNick2 = Mess_IntegralNick;
828 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
 832 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
829 
    ZaehlMessungen = 0;
 833 
    ZaehlMessungen = 0;
830 
    LageKorrekturNick = 0;
 834 
    LageKorrekturNick = 0;
831 
    LageKorrekturRoll = 0;
 835 
    LageKorrekturRoll = 0;
832 
  }
 836 
  }
833 
 
 837 
 
834 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
838 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
835 
  if(!Looping_Nick && !Looping_Roll)
 839 
  if(!Looping_Nick && !Looping_Roll)
836 
  {
 840 
  {
837 
   long tmp_long, tmp_long2;
 841 
   long tmp_long, tmp_long2;
838 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
 842 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
839 
    tmp_long /= 16;
 843 
    tmp_long /= 16;
840 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
 844 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
841 
    tmp_long2 /= 16;
 845 
    tmp_long2 /= 16;
842 
 #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
 846 
 #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
843 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 847 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
844 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 848 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
845 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 849 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
846 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 850 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
847 
    Mess_IntegralNick -= tmp_long;
 851 
    Mess_IntegralNick -= tmp_long;
848 
    Mess_IntegralRoll -= tmp_long2;
 852 
    Mess_IntegralRoll -= tmp_long2;
849 
  }
 853 
  }
850 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
854 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
851 
 
 855 
 
852 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 856 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
853 
 {
 857 
 {
854 
  static int cnt = 0;
 858 
  static int cnt = 0;
855 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
 859 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
856 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
 860 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
857 
  if(!Looping_Nick && !Looping_Roll)
 861 
  if(!Looping_Nick && !Looping_Roll)
858 
  {
 862 
  {
859 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
 863 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
860 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
 864 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
861 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
 865 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
862 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
 866 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
863 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
 867 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
864 
#define MAX_I 0//(Poti2/10)
 868 
#define MAX_I 0//(Poti2/10)
865 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
 869 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
866 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
 870 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
867 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
 871 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
868 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 872 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
869 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
873 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
870 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
 874 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
871 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 875 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
872 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 876 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
873 
 
 877 
 
874 
//    Mess_IntegralNick -= ausgleichNick;
 878 
//    Mess_IntegralNick -= ausgleichNick;
875 
//    Mess_IntegralRoll -= ausgleichRoll;
 879 
//    Mess_IntegralRoll -= ausgleichRoll;
876 
 
 880 
 
877 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
881 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
878 
// Gyro-Drift ermitteln
 882 
// Gyro-Drift ermitteln
879 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
883 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
880 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
 884 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
881 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
 885 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
882 
//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
886 
//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
883 
//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
887 
//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
884 
    tmp_long  = IntegralNick2 - IntegralNick;
 888 
    tmp_long  = IntegralNick2 - IntegralNick;
885 
    tmp_long2 = IntegralRoll2 - IntegralRoll;
 889 
    tmp_long2 = IntegralRoll2 - IntegralRoll;
886 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
 890 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
887 
 
 891 
 
888 
    IntegralFehlerNick = tmp_long;
 892 
    IntegralFehlerNick = tmp_long;
889 
    IntegralFehlerRoll = tmp_long2;
 893 
    IntegralFehlerRoll = tmp_long2;
890 
    Mess_IntegralNick2 -= IntegralFehlerNick;
 894 
    Mess_IntegralNick2 -= IntegralFehlerNick;
891 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
 895 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
892 
 
 896 
 
893 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
 897 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
894 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 898 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
895 
 
 899 
 
896 
//Salvo Ersatzkompass Ueberlauf korrigieren
 900 
//Salvo Ersatzkompass Ueberlauf korrigieren
897 
                if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
 901 
                if (GyroKomp_Int >= ((long)360 * GYROKOMP_INC_GRAD_DEFAULT)) GyroKomp_Int = GyroKomp_Int - (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
898 
                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
 902 
                if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + (GYROKOMP_INC_GRAD_DEFAULT *(long)360); //neu ab 3.11.2007
899 
                ROT_OFF;
 903 
                ROT_OFF;
900 
// Salvo End
 904 
// Salvo End
901 
 
 905 
 
902 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
 906 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
903 
DebugOut.Analog[18] = IntegralAccRoll / 26;
 907 
DebugOut.Analog[18] = IntegralAccRoll / 26;
904 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 908 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
905 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 909 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
906 
DebugOut.Analog[21] = MittelIntegralNick / 26;
 910 
DebugOut.Analog[21] = MittelIntegralNick / 26;
907 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
 911 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
908 
DebugOut.Analog[28] = ausgleichNick;
 912 
DebugOut.Analog[28] = ausgleichNick;
909 
DebugOut.Analog[29] = ausgleichRoll;
 913 
DebugOut.Analog[29] = ausgleichRoll;
910 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
 914 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
911 
*/
 915 
*/
912 
 
 916 
 
913 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 917 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
914 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 918 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
915 
#define BEWEGUNGS_LIMIT 20000
 919 
#define BEWEGUNGS_LIMIT 20000
916 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
 920 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
917 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 921 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
918 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
 922 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
919 
        {
 923 
        {
920 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
 924 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
921 
         {
 925 
         {
922 
           if(last_n_p)
 926 
           if(last_n_p)
923 
           {
 927 
           {
924 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 928 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
925 
            ausgleichNick = IntegralFehlerNick / 8;
 929 
            ausgleichNick = IntegralFehlerNick / 8;
926 
            if(ausgleichNick > 5000) ausgleichNick = 5000;
 930 
            if(ausgleichNick > 5000) ausgleichNick = 5000;
927 
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 931 
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
928 
           }
 932 
           }
929 
           else last_n_p = 1;
 933 
           else last_n_p = 1;
930 
         } else  last_n_p = 0;
 934 
         } else  last_n_p = 0;
931 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
 935 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
932 
         {
 936 
         {
933 
           if(last_n_n)
 937 
           if(last_n_n)
934 
            {
 938 
            {
935 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
 939 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
936 
             ausgleichNick = IntegralFehlerNick / 8;
 940 
             ausgleichNick = IntegralFehlerNick / 8;
937 
             if(ausgleichNick < -5000) ausgleichNick = -5000;
 941 
             if(ausgleichNick < -5000) ausgleichNick = -5000;
938 
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
 942 
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
939 
            }
 943 
            }
940 
           else last_n_n = 1;
 944 
           else last_n_n = 1;
941 
         } else  last_n_n = 0;
 945 
         } else  last_n_n = 0;
942 
        } else cnt = 0;
 946 
        } else cnt = 0;
943 
//Salvo 26.12.2007
 947 
//Salvo 26.12.2007
944 
                w = (abs(Mittelwert_AccNick));
 948 
                w = (abs(Mittelwert_AccNick));
945 
                v = (abs(Mittelwert_AccRoll));
 949 
                v = (abs(Mittelwert_AccRoll));
946 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in annaehend waagrechter Lage nachtrimmen
 950 
                if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) // Gyro nur in annaehend waagrechter Lage nachtrimmen
947 
                {
 951 
                {
948 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 952 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
949 
                if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
 953 
                if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
950 
                if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 954 
                if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
951 
                }
 955 
                }
952 
//Salvo End
 956 
//Salvo End
953 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 957 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
954 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 958 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
955 
 
 959 
 
956 
        ausgleichRoll = 0;
 960 
        ausgleichRoll = 0;
957 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
 961 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
958 
        {
 962 
        {
959 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
 963 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
960 
         {
 964 
         {
961 
           if(last_r_p)
 965 
           if(last_r_p)
962 
           {
 966 
           {
963 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 967 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
964 
            ausgleichRoll = IntegralFehlerRoll / 8;
 968 
            ausgleichRoll = IntegralFehlerRoll / 8;
965 
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
 969 
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
966 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 970 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
967 
           }
 971 
           }
968 
           else last_r_p = 1;
 972 
           else last_r_p = 1;
969 
         } else  last_r_p = 0;
 973 
         } else  last_r_p = 0;
970 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
 974 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
971 
         {
 975 
         {
972 
           if(last_r_n)
 976 
           if(last_r_n)
973 
           {
 977 
           {
974 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
 978 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
975 
            ausgleichRoll = IntegralFehlerRoll / 8;
 979 
            ausgleichRoll = IntegralFehlerRoll / 8;
976 
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
 980 
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
977 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
 981 
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
978 
           }
 982 
           }
979 
           else last_r_n = 1;
 983 
           else last_r_n = 1;
980 
         } else  last_r_n = 0;
 984 
         } else  last_r_n = 0;
981 
        } else
 985 
        } else
982 
        {
 986 
        {
983 
         cnt = 0;
 987 
         cnt = 0;
984 
        }
 988 
        }
985 
//Salvo 26.12.2007
 989 
//Salvo 26.12.2007
986 
                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur inannaehernd waagrechter Lage nachtrimmen
 990 
                if  ((w  < ACC_WAAGRECHT_LIMIT*2) && (v < ACC_WAAGRECHT_LIMIT*2)) // Gyro nur inannaehernd waagrechter Lage nachtrimmen
987 
                {
 991 
                {
988 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 992 
                if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
989 
                if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 993 
                if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
990 
                if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 994 
                if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
991 
            }
 995 
            }
992 
//Salvo End
 996 
//Salvo End
993 
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 997 
//DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
994 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 998 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
995 
  }
 999 
  }
996 
  else
 1000 
  else
997 
  {
 1001 
  {
998 
   LageKorrekturRoll = 0;
 1002 
   LageKorrekturRoll = 0;
999 
   LageKorrekturNick = 0;
 1003 
   LageKorrekturNick = 0;
1000 
  }
 1004 
  }
1001 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 1005 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
1002 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1006 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1003 
   MittelIntegralNick_Alt = MittelIntegralNick;      
 1007 
   MittelIntegralNick_Alt = MittelIntegralNick;      
1004 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
 1008 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
1005 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1009 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
1006 
   IntegralAccNick = 0;
 1010 
   IntegralAccNick = 0;
1007 
   IntegralAccRoll = 0;
 1011 
   IntegralAccRoll = 0;
1008 
   IntegralAccZ = 0;
 1012 
   IntegralAccZ = 0;
1009 
   MittelIntegralNick = 0;
 1013 
   MittelIntegralNick = 0;
1010 
   MittelIntegralRoll = 0;
 1014 
   MittelIntegralRoll = 0;
1011 
   MittelIntegralNick2 = 0;
 1015 
   MittelIntegralNick2 = 0;
1012 
   MittelIntegralRoll2 = 0;
 1016 
   MittelIntegralRoll2 = 0;
1013 
   ZaehlMessungen = 0;
 1017 
   ZaehlMessungen = 0;
1014 
 } // Ende Abgleich
 1018 
 } // Ende Abgleich
1015 
 
 1019 
 
1016 
// Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
 1020 
// Salvo Ersatzkompass  und Giergyrokompensation 15.12.2007 **********************
1017 
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
 1021 
if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat
1018 
{
 1022 
{
1019 
           Kompass_Neuer_Wert = 0;
 1023 
           Kompass_Neuer_Wert = 0;
1020 
   w = (abs(Mittelwert_AccNick));
 1024 
   w = (abs(Mittelwert_AccNick));
1021 
   v = (abs(Mittelwert_AccRoll));
 1025 
   v = (abs(Mittelwert_AccRoll));
1022 
   if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro  nur mit Magnetkompass aktualisieren wenn alles ok
 1026 
   if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro  nur mit Magnetkompass aktualisieren wenn alles ok
1023 
   {
 1027 
   {
1024 
        if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
 1028 
        if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
1025 
        {
 1029 
        {
1026 
 
 1030 
 
1027 
          if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
 1031 
          if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass
1028 
          {
 1032 
          {
1029 
                        if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
 1033 
                        if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1;
1030 
                        if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
 1034 
                        if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da
1031 
                        {
 1035 
                        {
1032 
                                w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
 1036 
                                w = (int) (GyroGier_Comp/(long)GYROKOMP_INC_GRAD_DEFAULT);
1033 
                                v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
 1037 
                                v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen
1034 
                                if (v <-180) v +=360; // Uberlaufkorrektur
 1038 
                                if (v <-180) v +=360; // Uberlaufkorrektur
1035 
                                if (v > 180) v -=360; // Uberlaufkorrektur
 1039 
                                if (v > 180) v -=360; // Uberlaufkorrektur
1036 
 
 1040 
 
1037 
                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
 1041 
                                v = w-v;  //Differenz Gyro zu Kompass ist der Driftfehler
1038 
 
 1042 
 
1039 
                                #define GIER_COMP_MAX 8
 1043 
                                #define GIER_COMP_MAX 8
1040 
                                if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
 1044 
                                if (v > GIER_COMP_MAX)  v= GIER_COMP_MAX;
1041 
                                if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
 1045 
                                if (v < -GIER_COMP_MAX) v= - GIER_COMP_MAX;
1042 
                                if (abs(w) > 1)
 1046 
                                if (abs(w) > 1)
1043 
                                {      
 1047 
                                {      
1044 
                                        GyroGier_Comp           = 0;
 1048 
                                        GyroGier_Comp           = 0;
1045 
                                        gyrogier_kompass        = KompassValue; // Kompasswert merken
 1049 
                                        gyrogier_kompass        = KompassValue; // Kompasswert merken
1046 
                                        AdNeutralGier           -= v;
 1050 
                                        AdNeutralGier           -= v;
1047 
                                }
 1051 
                                }
1048 
                        }
 1052 
                        }
1049 
          }
 1053 
          }
1050 
          else
 1054 
          else
1051 
          {
 1055 
          {
1052 
                gyrogier_kompass        = KompassValue; // Kompasswert merken
 1056 
                gyrogier_kompass        = KompassValue; // Kompasswert merken
1053 
                cnt_stickgier_zero      = 0;
 1057 
                cnt_stickgier_zero      = 0;
1054 
                GyroGier_Comp = 0;
 1058 
                GyroGier_Comp = 0;
1055 
          }
 1059 
          }
1056 
 
 1060 
 
1057 
          magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
1061 
          magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
1058 
          GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
 1062 
          GyroKomp_Int = (GyroKomp_Int )/(long)GYROKOMP_INC_GRAD_DEFAULT;
1059 
 
 1063 
 
1060 
          w = KompassValue - GyroKomp_Int;
 1064 
          w = KompassValue - GyroKomp_Int;
1061 
          if ((w > 0) && (w < 180))
 1065 
          if ((w > 0) && (w < 180))
1062 
          {
 1066 
          {
1063 
           ++GyroKomp_Int;
 1067 
           ++GyroKomp_Int;
1064 
          }
 1068 
          }
1065 
          else if ((w > 0) && (w >= 180))
 1069 
          else if ((w > 0) && (w >= 180))
1066 
          {
 1070 
          {
1067 
           --GyroKomp_Int;
 1071 
           --GyroKomp_Int;
1068 
          }
 1072 
          }
1069 
          else if ((w < 0) && (w >= -180))
 1073 
          else if ((w < 0) && (w >= -180))
1070 
          {
 1074 
          {
1071 
           --GyroKomp_Int;
 1075 
           --GyroKomp_Int;
1072 
          }
 1076 
          }
1073 
          else if ((w < 0) && (w < -180))
 1077 
          else if ((w < 0) && (w < -180))
1074 
          {
 1078 
          {
1075 
           ++GyroKomp_Int;
 1079 
           ++GyroKomp_Int;
1076 
          }
 1080 
          }
1077 
          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
 1081 
          if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360L;
1078 
          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
 1082 
          GyroKomp_Int = (GyroKomp_Int%360L) * (long)GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
1079 
        }
 1083 
        }
1080 
   }
 1084 
   }
1081 
   else //Kompassfehler
 1085 
   else //Kompassfehler
1082 
   {
 1086 
   {
1083 
    magkompass_ok = 0;
 1087 
    magkompass_ok = 0;
1084 
        GyroGier_Comp = 0;
 1088 
        GyroGier_Comp = 0;
1085 
   }
 1089 
   }
1086 
   Kompass_Value_Old    =       KompassValue;
 1090 
   Kompass_Value_Old    =       KompassValue;
1087 
}
 1091 
}
1088 
// Salvo End *************************
 1092 
// Salvo End *************************
1089 
 
 1093 
 
1090 
// Salvo 6.10.2007
1094 
// Salvo 6.10.2007
1091 
        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
 1095 
        // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist
1092 
        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
 1096 
        //GPS Hold Aktiveren wenn Knueppel in Ruhelage sind
1093 
        if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
 1097 
        if ((EE_Parameter.GlobalConfig & CFG_GPS_AKTIV) && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) < GPS_STICK_HOLDOFF)
1094 
        && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
 1098 
        && (abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < GPS_STICK_HOLDOFF) && (gps_alive_cnt > 0))
1095 
        {
 1099 
        {
1096 
                if (Parameter_MaxHoehe > 200)  
 1100 
                if (Parameter_MaxHoehe > 200)  
1097 
                {      
 1101 
                {      
1098 
                        if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
 1102 
                        if ( gps_cmd == GPS_CMD_REQ_HOLD) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
1099 
                        else gps_cmd = GPS_CMD_REQ_HOME;
 1103 
                        else gps_cmd = GPS_CMD_REQ_HOME;
1100 
                        n = GPS_CRTL(gps_cmd);
 1104 
                        n = GPS_CRTL(gps_cmd);
1101 
                }
 1105 
                }
1102 
                else
 1106 
                else
1103 
                {
 1107 
                {
1104 
                        if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
 1108 
                        if ( gps_cmd == GPS_CMD_REQ_HOME) gps_cmd = GPS_CMD_STOP; // erst mal stoppen, denn altes Kommando wurde noch nicht beendet
1105 
                        else gps_cmd = GPS_CMD_REQ_HOLD;
 1109 
                        else gps_cmd = GPS_CMD_REQ_HOLD;
1106 
                        n= GPS_CRTL(gps_cmd);
 1110 
                        n= GPS_CRTL(gps_cmd);
1107 
                }
 1111 
                }
1108 
        }
 1112 
        }
1109 
        else
 1113 
        else
1110 
        {
 1114 
        {
1111 
                if (gps_cmd != GPS_CMD_STOP)
 1115 
                if (gps_cmd != GPS_CMD_STOP)
1112 
                {
 1116 
                {
1113 
                        gps_cmd = GPS_CMD_STOP;
 1117 
                        gps_cmd = GPS_CMD_STOP;
1114 
                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
 1118 
                        n= GPS_CRTL(gps_cmd); //GPS Lageregelung beenden
1115 
                }
 1119 
                }
1116 
        }
 1120 
        }
1117 
        if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12)  LED_J16_OFF; //led im GPS Mode schnell blinken lassen
 1121 
        if (gps_state != GPS_CRTL_IDLE) if (TimerWerteausgabe == 12)  LED_J16_OFF; //led im GPS Mode schnell blinken lassen
1118 
 
 1122 
 
1119 
 
 1123 
 
1120 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1124 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1121 
//  Gieren
 1125 
//  Gieren
1122 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1126 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1123 
    if(abs(StickGier) > 20) // war 35
1127 
    if(abs(StickGier) > 20) // war 35
1124 
     {
 1128 
     {
1125 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 1129 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
1126 
     }
 1130 
     }
1127 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
 1131 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1128 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 1132 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
1129 
    sollGier = tmp_int;
 1133 
    sollGier = tmp_int;
1130 
    Mess_Integral_Gier -= tmp_int;  
 1134 
    Mess_Integral_Gier -= tmp_int;  
1131 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
 1135 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
1132 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
 1136 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1133 
 
 1137 
 
1134 
 
 1138 
 
1135 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1139 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1136 
//  Kompass
 1140 
//  Kompass
1137 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1141 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1138 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
 1142 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (Kompass_present > 0))
1139 
     {
 1143 
     {
1140 
           if(v > w) w = v; // grösste Neigung ermitteln
 1144 
           if(v > w) w = v; // grösste Neigung ermitteln
1141 
 
 1145 
 
1142 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
 1146 
// Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert
1143 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
 1147 
                if ((magkompass_ok > 0)  &&  NeueKompassRichtungMerken)  
1144 
        {
 1148 
        {
1145 
                 KompassStartwert = KompassValue;
 1149 
                 KompassStartwert = KompassValue;
1146 
         NeueKompassRichtungMerken = 0;
 1150 
         NeueKompassRichtungMerken = 0;
1147 
        }
 1151 
        }
1148 
// Salvo 13.9.2007
 1152 
// Salvo 13.9.2007
1149 
       w=0;
 1153 
       w=0;
1150 
// Salvo End
 1154 
// Salvo End
1151 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 1155 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
1152 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 1156 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
1153 
       if(w > 0)
 1157 
       if(w > 0)
1154 
        {
 1158 
        {
1155 
// Salvo Kompasssteuerung **********************       
1159 
// Salvo Kompasssteuerung **********************       
1156 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 1160 
                 if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
1157 
// Salvo End
 1161 
// Salvo End
1158 
        }
 1162 
        }
1159 
     }
 1163 
     }
1160 
 
 1164 
 
1161 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1165 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1162 
 
 1166 
 
1163 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1167 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1164 
//  Debugwerte zuordnen
 1168 
//  Debugwerte zuordnen
1165 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1169 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1166 
  if(!TimerWerteausgabe--)
 1170 
  if(!TimerWerteausgabe--)
1167 
   {
 1171 
   {
1168 
    TimerWerteausgabe = 24;
 1172 
    TimerWerteausgabe = 24;
1169 
 
 1173 
 
1170 
        // Salvo 13.12.2007 Beleuchtung steuern
 1174 
        // Salvo 13.12.2007 Beleuchtung steuern
1171 
        if (!(beeptime & BeepMuster)) LED_J16_FLASH;
 1175 
        if (!(beeptime & BeepMuster)) LED_J16_FLASH;
1172 
        else if (MotorenEin) LED_J16_ON;
 1176 
        else if (MotorenEin) LED_J16_ON;
1173 
        else LED_J16_OFF;
 1177 
        else LED_J16_OFF;
1174 
        // Salvo End
 1178 
        // Salvo End
1175 
 
 1179 
 
1176 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 1180 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
1177 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 1181 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
1178 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 1182 
    DebugOut.Analog[2] = Mittelwert_AccNick;
1179 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 1183 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
1180 
    DebugOut.Analog[4] = MesswertGier;
 1184 
    DebugOut.Analog[4] = MesswertGier;
1181 
    DebugOut.Analog[5] = HoehenWert;
 1185 
    DebugOut.Analog[5] = HoehenWert;
1182 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
 1186 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
1183 
    DebugOut.Analog[8] = KompassValue;
 1187 
    DebugOut.Analog[8] = KompassValue;
1184 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
 1188 
        DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT;
1185 
 
 1189 
 
1186 
// Diverse parameter Debugging
 1190 
// Diverse parameter Debugging
1187 
        DebugOut.Analog[16] =  dataset_cnt;
 1191 
        DebugOut.Analog[16] =  dataset_cnt;
1188 
        DebugOut.Analog[17] =  UBat;
 1192 
        DebugOut.Analog[17] =  UBat;
1189 
        DebugOut.Analog[18] =  MesswertNick;
 1193 
        DebugOut.Analog[18] =  MesswertNick;
1190 
        DebugOut.Analog[19] =  MesswertRoll;
 1194 
        DebugOut.Analog[19] =  MesswertRoll;
1191 
        DebugOut.Analog[20] =  MesswertGier;
 1195 
        DebugOut.Analog[20] =  MesswertGier;
1192 
        DebugOut.Analog[21] =  StickNick;
 1196 
        DebugOut.Analog[21] =  StickNick;
1193 
        DebugOut.Analog[22] =  StickRoll;
 1197 
        DebugOut.Analog[22] =  StickRoll;
1194 
        DebugOut.Analog[23] =  StickGier;
 1198 
        DebugOut.Analog[23] =  StickGier;
1195 
// GPS Debugging
 1199 
// GPS Debugging
1196 
//    DebugOut.Analog[10] = GPS_dist_2trgt/10;
 1200 
//    DebugOut.Analog[10] = GPS_dist_2trgt/10;
1197 
//      DebugOut.Analog[11] = GPS_hdng_rel_2trgt;
 1201 
//      DebugOut.Analog[11] = GPS_hdng_rel_2trgt;
1198 
    DebugOut.Analog[10] = Parameter_UserParam1;
 1202 
    DebugOut.Analog[10] = Parameter_UserParam1;
1199 
        DebugOut.Analog[11] = Parameter_UserParam3;
 1203 
        DebugOut.Analog[11] = Parameter_UserParam3;
1200 
        DebugOut.Analog[24] =  GPS_Nick;
 1204 
        DebugOut.Analog[24] =  GPS_Nick;
1201 
        DebugOut.Analog[25] =  GPS_Roll;
 1205 
        DebugOut.Analog[25] =  GPS_Roll;
1202 
        DebugOut.Analog[26] =  gps_rel_act_position.utm_east; //in 10cm ausgeben
 1206 
        DebugOut.Analog[26] =  gps_rel_act_position.utm_east; //in 10cm ausgeben
1203 
        DebugOut.Analog[27] =  gps_rel_act_position.utm_north;
 1207 
        DebugOut.Analog[27] =  gps_rel_act_position.utm_north;
1204 
        DebugOut.Analog[28] =  gps_rel_act_position.utm_alt;
 1208 
        DebugOut.Analog[28] =  gps_rel_act_position.utm_alt;
1205 
        DebugOut.Analog[29] =  gps_state + (gps_sub_state*10)+(50*gps_cmd);
 1209 
        DebugOut.Analog[29] =  gps_state + (gps_sub_state*10)+(50*gps_cmd);
1206 
        DebugOut.Analog[30] =  gps_reg_x;
 1210 
        DebugOut.Analog[30] =  debug_gp_0;
1207 
        DebugOut.Analog[31] =  gps_reg_y;
 1211 
        DebugOut.Analog[31] =  debug_gp_1;
- 
 
 1212 
        DebugOut.Analog[12] = debug_gp_2;
- 
 
 1213 
    DebugOut.Analog[13] = debug_gp_3;
- 
 
 1214 
    DebugOut.Analog[14] = debug_gp_4;
- 
 
 1215 
    DebugOut.Analog[15] = debug_gp_5;
- 
 
 1216 
 
1208 
//      DebugOut.Analog[30] =  dist_flown;
 1217 
//      DebugOut.Analog[30] =  dist_flown;
1209 
//      DebugOut.Analog[31] =  (int) dist_2home;
 1218 
//      DebugOut.Analog[31] =  (int) dist_2home;
1210 
//      DebugOut.Analog[31] =  (int) GPS_hdng_abs_2trgt;
 1219 
//      DebugOut.Analog[31] =  (int) GPS_hdng_abs_2trgt;
1211 
//      DebugOut.Analog[31] =  (int) GyroGier_Comp;
 1220 
//      DebugOut.Analog[31] =  (int) GyroGier_Comp;
- 
 
 1221 
/*
1212 
/*    DebugOut.Analog[16] = motor_rx[0];
 1222 
    DebugOut.Analog[16] = motor_rx[0];
1213 
    DebugOut.Analog[17] = motor_rx[1];
 1223 
    DebugOut.Analog[17] = motor_rx[1];
1214 
    DebugOut.Analog[18] = motor_rx[2];
 1224 
    DebugOut.Analog[18] = motor_rx[2];
1215 
    DebugOut.Analog[19] = motor_rx[3];
 1225 
    DebugOut.Analog[19] = motor_rx[3];
1216 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
 1226 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1217 
    DebugOut.Analog[20] /= 14;
 1227 
    DebugOut.Analog[20] /= 14;
1218 
    DebugOut.Analog[21] = motor_rx[4];
 1228 
    DebugOut.Analog[21] = motor_rx[4];
1219 
    DebugOut.Analog[22] = motor_rx[5];
 1229 
    DebugOut.Analog[22] = motor_rx[5];
1220 
    DebugOut.Analog[23] = motor_rx[6];
 1230 
    DebugOut.Analog[23] = motor_rx[6];
1221 
    DebugOut.Analog[24] = motor_rx[7];
 1231 
    DebugOut.Analog[24] = motor_rx[7];
1222 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 1232 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1223 
*/
 1233 
*/
1224 
//    DebugOut.Analog[9] = MesswertNick;
 1234 
//    DebugOut.Analog[9] = MesswertNick;
1225 
//    DebugOut.Analog[9] = SollHoehe;
 1235 
//    DebugOut.Analog[9] = SollHoehe;
1226 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 1236 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
1227 
//    DebugOut.Analog[11] = KompassStartwert;
 1237 
//    DebugOut.Analog[11] = KompassStartwert;
1228 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 1238 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
1229 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 1239 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
1230 
//    DebugOut.Analog[9] = KompassRichtung;    
 1240 
//    DebugOut.Analog[9] = KompassRichtung;    
1231 
//    DebugOut.Analog[10] = GasMischanteil;
 1241 
//    DebugOut.Analog[10] = GasMischanteil;
1232 
//    DebugOut.Analog[3] = HoeheD * 32;
 1242 
//    DebugOut.Analog[3] = HoeheD * 32;
1233 
//    DebugOut.Analog[4] = hoehenregler;
 1243 
//    DebugOut.Analog[4] = hoehenregler;
1234 
  }
 1244 
  }
1235 
 
 1245 
 
1236 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1246 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1237 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 1247 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1238 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1248 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1239 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 1249 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
1240 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 1250 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
1241 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 1251 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
1242 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 1252 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
1243 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 1253 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
1244 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 1254 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
1245 
 
 1255 
 
1246 
//DebugOut.Analog[28] = MesswertRoll;
 1256 
//DebugOut.Analog[28] = MesswertRoll;
1247 
//DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
 1257 
//DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
1248 
//DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
 1258 
//DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
1249 
 
 1259 
 
1250 
    // Maximalwerte abfangen
 1260 
    // Maximalwerte abfangen
1251 
    #define MAX_SENSOR  2048
 1261 
    #define MAX_SENSOR  2048
1252 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 1262 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1253 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 1263 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1254 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 1264 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1255 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 1265 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1256 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 1266 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
1257 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 1267 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1258 
 
 1268 
 
1259 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1269 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1260 
// Höhenregelung
 1270 
// Höhenregelung
1261 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 1271 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1262 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1272 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1263 
//OCR0B = 180 - (Poti1 + 120) / 4;
 1273 
//OCR0B = 180 - (Poti1 + 120) / 4;
1264 
//DruckOffsetSetting = OCR0B;
 1274 
//DruckOffsetSetting = OCR0B;
1265 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 1275 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
1266 
  {
 1276 
  {
1267 
    int tmp_int;
 1277 
    int tmp_int;
1268 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 1278 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1269 
    {
 1279 
    {
1270 
     if(Parameter_MaxHoehe < 50)
 1280 
     if(Parameter_MaxHoehe < 50)
1271 
      {
 1281 
      {
1272 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 1282 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
1273 
       HoehenReglerAktiv = 0;
 1283 
       HoehenReglerAktiv = 0;
1274 
      }
 1284 
      }
1275 
      else  
 1285 
      else  
1276 
        HoehenReglerAktiv = 1;
 1286 
        HoehenReglerAktiv = 1;
1277 
    }
 1287 
    }
1278 
    else
 1288 
    else
1279 
    {
 1289 
    {
1280 
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
 1290 
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
1281 
     HoehenReglerAktiv = 1;
 1291 
     HoehenReglerAktiv = 1;
1282 
    }
 1292 
    }
1283 
 
 1293 
 
1284 
    if(Notlandung) SollHoehe = 0;
 1294 
    if(Notlandung) SollHoehe = 0;
1285 
    h = HoehenWert;
 1295 
    h = HoehenWert;
1286 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 1296 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
1287 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 1297 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
1288 
      h = GasMischanteil - h;         // vom Gas abziehen
 1298 
      h = GasMischanteil - h;         // vom Gas abziehen
1289 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 1299 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
1290 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 1300 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
1291 
      if(tmp_int > 50) tmp_int = 50;
 1301 
      if(tmp_int > 50) tmp_int = 50;
1292 
      else if(tmp_int < -50) tmp_int = -50;
 1302 
      else if(tmp_int < -50) tmp_int = -50;
1293 
      h -= tmp_int;
 1303 
      h -= tmp_int;
1294 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 1304 
      hoehenregler = (hoehenregler*15 + h) / 16;      
1295 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 1305 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
1296 
       {
 1306 
       {
1297 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 1307 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
1298 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 1308 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
1299 
       }  
 1309 
       }  
1300 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 1310 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
1301 
      GasMischanteil = hoehenregler;
 1311 
      GasMischanteil = hoehenregler;
1302 
     }
 1312 
     }
1303 
  }
 1313 
  }
1304 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1314 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1305 
// + Mischer und PI-Regler
 1315 
// + Mischer und PI-Regler
1306 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1316 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1307 
  DebugOut.Analog[7] = GasMischanteil;
 1317 
  DebugOut.Analog[7] = GasMischanteil;
1308 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1318 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1309 
// Gier-Anteil
 1319 
// Gier-Anteil
1310 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1320 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1311 
#define MUL_G  1.0
 1321 
#define MUL_G  1.0
1312 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 1322 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
1313 
//GierMischanteil = 0;
 1323 
//GierMischanteil = 0;
1314 
 
 1324 
 
1315 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
 1325 
    if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
1316 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
 1326 
    if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
1317 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
 1327 
    if(GierMischanteil > ((MAX_GAS - GasMischanteil))) GierMischanteil = ((MAX_GAS - GasMischanteil));
1318 
    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
 1328 
    if(GierMischanteil < -((MAX_GAS - GasMischanteil))) GierMischanteil = -((MAX_GAS - GasMischanteil));
1319 
 
 1329 
 
1320 
    if(GasMischanteil < 20) GierMischanteil = 0;
 1330 
    if(GasMischanteil < 20) GierMischanteil = 0;
1321 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1331 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1322 
// Nick-Achse
 1332 
// Nick-Achse
1323 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1333 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1324 
    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
 1334 
    DiffNick = MesswertNick - (StickNick - GPS_Nick);   // Differenz bestimmen
1325 
    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
 1335 
    if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung
1326 
    else  SummeNick += DiffNick; // I-Anteil bei HH
1336 
    else  SummeNick += DiffNick; // I-Anteil bei HH
1327 
    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
 1337 
    if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ;
1328 
    if(SummeNick >  16000) SummeNick =  16000;
 1338 
    if(SummeNick >  16000) SummeNick =  16000;
1329 
    if(SummeNick < -16000) SummeNick = -16000;
 1339 
    if(SummeNick < -16000) SummeNick = -16000;
1330 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1340 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
1331 
    // Motor Vorn
 1341 
    // Motor Vorn
1332 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1342 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1333 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
 1343 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1334 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1344 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1335 
 
 1345 
 
1336 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 1346 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
1337 
        if ((motorwert < 0)) motorwert = 0;
 1347 
        if ((motorwert < 0)) motorwert = 0;
1338 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1348 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
1339 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1349 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1340 
        Motor_Vorne = motorwert;           
 1350 
        Motor_Vorne = motorwert;           
1341 
    // Motor Heck
 1351 
    // Motor Heck
1342 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 1352 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
1343 
        if ((motorwert < 0)) motorwert = 0;
 1353 
        if ((motorwert < 0)) motorwert = 0;
1344 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1354 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
1345 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1355 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1346 
        Motor_Hinten = motorwert;              
 1356 
        Motor_Hinten = motorwert;              
1347 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1357 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1348 
// Roll-Achse
 1358 
// Roll-Achse
1349 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1359 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1350 
        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
 1360 
        DiffRoll = MesswertRoll - (StickRoll  - GPS_Roll);      // Differenz bestimmen
1351 
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
 1361 
    if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll  - GPS_Roll);// I-Anteil bei Winkelregelung
1352 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
 1362 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1353 
    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
 1363 
    if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ;
1354 
    if(SummeRoll >  16000) SummeRoll =  16000;
 1364 
    if(SummeRoll >  16000) SummeRoll =  16000;
1355 
    if(SummeRoll < -16000) SummeRoll = -16000;
 1365 
    if(SummeRoll < -16000) SummeRoll = -16000;
1356 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 1366 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
1357 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1367 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1358 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
 1368 
    if(pd_ergebnis >  tmp_int) pd_ergebnis =  tmp_int;
1359 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
 1369 
    if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int;
1360 
    // Motor Links
 1370 
    // Motor Links
1361 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1371 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
1362 
        if ((motorwert < 0)) motorwert = 0;
 1372 
        if ((motorwert < 0)) motorwert = 0;
1363 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1373 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1364 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1374 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
1365 
        Motor_Links = motorwert;               
 1375 
        Motor_Links = motorwert;               
1366 
    // Motor Rechts
 1376 
    // Motor Rechts
1367 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1377 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
1368 
        if ((motorwert < 0)) motorwert = 0;
 1378 
        if ((motorwert < 0)) motorwert = 0;
1369 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1379 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
1370 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1380 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
1371 
        Motor_Rechts = motorwert;
 1381 
        Motor_Rechts = motorwert;
1372 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1382 
   // +++++++++++++++++++++++++++++++++++++++++++++++
1373 
}
 1383 
}
1374 
 
 1384 
 
1375 
 
 1385