Subversion Repositories FlightCtrl

Rev

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

Rev 477 Rev 496
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
 50 
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
51 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 51 
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
52 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 52 
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
53 
// +  POSSIBILITY OF SUCH DAMAGE.
53 
// +  POSSIBILITY OF SUCH DAMAGE.
54 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 54 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
55 
 
 55 
 
56 
#include "main.h"
 56 
#include "main.h"
57 
 
 57 
 
58 
unsigned char h,m,s;
 58 
unsigned char h,m,s;
59 
volatile unsigned char Timeout = 0;
 59 
volatile unsigned char Timeout = 0;
60 
int MesswertNick,MesswertRoll,MesswertGier;
 60 
int MesswertNick,MesswertRoll,MesswertGier;
61 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 61 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
62 
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
 62 
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch;
63 
 
 63 
 
64 
long IntegralNick = 0,IntegralNick2 = 0;
 64 
long IntegralNick = 0,IntegralNick2 = 0;
65 
long IntegralRoll = 0,IntegralRoll2 = 0;
 65 
long IntegralRoll = 0,IntegralRoll2 = 0;
66 
long Integral_Gier = 0;
 66 
long Integral_Gier = 0;
67 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 67 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
68 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 68 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
69 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 69 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
70 
volatile long Mess_Integral_Hoch = 0;
 70 
volatile long Mess_Integral_Hoch = 0;
71 
 
 71 
 
72 
int KompassValue = 0;
 72 
int KompassValue = 0;
73 
int KompassStartwert = 0;
 73 
int KompassStartwert = 0;
74 
int KompassRichtung = 0;
 74 
int KompassRichtung = 0;
75 
uint8_t updKompass = 0;
 75 
uint8_t updKompass = 0;
76 
 
 76 
 
77 
unsigned char MAX_GAS,MIN_GAS;
 77 
unsigned char MAX_GAS,MIN_GAS;
78 
unsigned char Notlandung = 0;
 78 
unsigned char Notlandung = 0;
79 
unsigned char HoehenReglerAktiv = 0;
 79 
unsigned char HoehenReglerAktiv = 0;
80 
 
 80 
 
81 
float GyroFaktor;
 81 
float GyroFaktor;
82 
float IntegralFaktor;
 82 
float IntegralFaktor;
83 
 
 83 
 
84 
int DiffNick,DiffRoll;
 84 
int DiffNick,DiffRoll;
85 
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 85 
int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
86 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links;
 86 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links;
87 
unsigned char MotorWert[5];
 87 
unsigned char MotorWert[5];
88 
volatile unsigned char SenderOkay = 0;
 88 
volatile unsigned char SenderOkay = 0;
89 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 89 
int StickNick = 0,StickRoll = 0,StickGier = 0;
90 
char MotorenEin = 0;
 90 
char MotorenEin = 0;
91 
int HoehenWert = 0;
 91 
int HoehenWert = 0;
92 
int SollHoehe = 0;
 92 
int SollHoehe = 0;
93 
 
 93 
 
94 
float Kp =  FAKTOR_P;
 94 
float Kp =  FAKTOR_P;
95 
float Ki =  FAKTOR_I;
 95 
float Ki =  FAKTOR_I;
96 
 
 96 
 
97 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 97 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
98 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 98 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
99 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 99 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
100 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 100 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
101 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
 101 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
102 
unsigned char Parameter_Gyro_P = 50;            // Wert : 10-250
 102 
unsigned char Parameter_Gyro_P = 50;            // Wert : 10-250
103 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 103 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
104 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 104 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
105 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 105 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
106 
unsigned char Parameter_UserParam1 = 0;
 106 
unsigned char Parameter_UserParam1 = 0;
107 
unsigned char Parameter_UserParam2 = 0;
 107 
unsigned char Parameter_UserParam2 = 0;
108 
unsigned char Parameter_UserParam3 = 0;
 108 
unsigned char Parameter_UserParam3 = 0;
109 
unsigned char Parameter_UserParam4 = 0;
 109 
unsigned char Parameter_UserParam4 = 0;
110 
unsigned char Parameter_ServoNickControl = 100;
 110 
unsigned char Parameter_ServoNickControl = 100;
111 
struct mk_param_struct EE_Parameter;
 111 
struct mk_param_struct EE_Parameter;
112 
 
 112 
 
113 
struct acc_neutral_struct ee_acc_neutral EEMEM;         // Reservierung im EEPROM
 113 
struct acc_neutral_struct ee_acc_neutral EEMEM;         // Reservierung im EEPROM
114 
struct acc_neutral_struct acc_neutral;
 114 
struct acc_neutral_struct acc_neutral;
115 
 
 115 
 
116 
void Piep(unsigned char Anzahl)
 116 
void Piep(unsigned char Anzahl)
117 
{
 117 
{
118 
 while(Anzahl--)
 118 
 while(Anzahl--)
119 
 {
 119 
 {
120 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 120 
  if(MotorenEin) return; //auf keinen Fall im Flug!
121 
  beeptime = 100;
 121 
  beeptime = 100;
122 
  Delay_ms(250);
 122 
  Delay_ms(250);
123 
 }
 123 
 }
124 
}
 124 
}
125 
 
 125 
 
126 
 
 126 
 
127 
//############################################################################
 127 
//############################################################################
128 
//  Neutrallage kalibrieren und fest im EEPROM abspeichern
 128 
//  Neutrallage kalibrieren und fest im EEPROM abspeichern
129 
void calib_acc(void)
 129 
void calib_acc(void)
130 
//############################################################################
 130 
//############################################################################
131 
{
 131 
{
132 
    unsigned int timer;
 132 
    unsigned int timer;
133 
        acc_neutral.X = 0;
 133 
        acc_neutral.X = 0;
134 
        acc_neutral.Y = 0;
 134 
        acc_neutral.Y = 0;
135 
        acc_neutral.Z = 0;
 135 
        acc_neutral.Z = 0;
136 
    CalibrierMittelwert();     
 136 
    CalibrierMittelwert();     
137 
    timer = SetDelay(5);    
 137 
    timer = SetDelay(5);    
138 
        while (!CheckDelay(timer));
 138 
        while (!CheckDelay(timer));
139 
        CalibrierMittelwert(); 
 139 
        CalibrierMittelwert(); 
140 
               
 140 
               
141 
        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 141 
        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
142 
        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 142 
        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
143 
        acc_neutral.Z = Aktuell_az;
 143 
        acc_neutral.Z = Aktuell_az;
144 
       
 144 
       
145 
        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
 145 
        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
146 
}
 146 
}
147 
 
 147 
 
148 
//############################################################################
 148 
//############################################################################
149 
//  Nullwerte ermitteln
 149 
//  Nullwerte ermitteln
150 
void SetNeutral(void)
 150 
void SetNeutral(void)
151 
//############################################################################
 151 
//############################################################################
152 
{
 152 
{
153 
    unsigned int timer;
 153 
    unsigned int timer;
154 
        acc_neutral.X = 0;
 154 
        acc_neutral.X = 0;
155 
        acc_neutral.Y = 0;
 155 
        acc_neutral.Y = 0;
156 
        acc_neutral.Z = 0;
 156 
        acc_neutral.Z = 0;
157 
    AdNeutralNick = 0; 
 157 
    AdNeutralNick = 0; 
158 
        AdNeutralRoll = 0;     
 158 
        AdNeutralRoll = 0;     
159 
        AdNeutralGier = 0;
 159 
        AdNeutralGier = 0;
160 
    CalibrierMittelwert();     
 160 
    CalibrierMittelwert();     
161 
    timer = SetDelay(5);    
 161 
    timer = SetDelay(5);    
162 
        while (!CheckDelay(timer));
 162 
        while (!CheckDelay(timer));
163 
        CalibrierMittelwert();
 163 
        CalibrierMittelwert();
164 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 164 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
165 
     {    
 165 
     {    
166 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 166 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
167 
     }
 167 
     }
168 
    AdNeutralNick= abs(MesswertNick);  
 168 
    AdNeutralNick= abs(MesswertNick);  
169 
        AdNeutralRoll= abs(MesswertRoll);      
 169 
        AdNeutralRoll= abs(MesswertRoll);      
170 
        AdNeutralGier= abs(MesswertGier);
 170 
        AdNeutralGier= abs(MesswertGier);
171 
       
 171 
       
172 
        eeprom_read_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
 172 
        eeprom_read_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));
173 
   
 173 
   
174 
        Mess_IntegralNick = 0; 
 174 
        Mess_IntegralNick = 0; 
175 
    Mess_IntegralNick2 = 0;
 175 
    Mess_IntegralNick2 = 0;
176 
    Mess_IntegralRoll = 0;     
 176 
    Mess_IntegralRoll = 0;     
177 
    Mess_IntegralRoll2 = 0;
 177 
    Mess_IntegralRoll2 = 0;
178 
    Mess_Integral_Gier = 0;    
 178 
    Mess_Integral_Gier = 0;    
179 
    MesswertNick = 0;
 179 
    MesswertNick = 0;
180 
    MesswertRoll = 0;
 180 
    MesswertRoll = 0;
181 
    MesswertGier = 0;
 181 
    MesswertGier = 0;
182 
    StartLuftdruck = Luftdruck;
 182 
    StartLuftdruck = Luftdruck;
183 
    HoeheD = 0;
 183 
    HoeheD = 0;
184 
    Mess_Integral_Hoch = 0;
 184 
    Mess_Integral_Hoch = 0;
185 
    KompassStartwert = KompassValue;
 185 
    KompassStartwert = KompassValue;
186 
    GPS_Neutral();
 186 
    GPS_Neutral();
187 
    beeptime = 50;  
 187 
    beeptime = 50;  
188 
}
 188 
}
189 
 
 189 
 
190 
//############################################################################
 190 
//############################################################################
191 
// Bildet den Mittelwert aus den Messwerten
 191 
// Bildet den Mittelwert aus den Messwerten
192 
void Mittelwert(void)
 192 
void Mittelwert(void)
193 
//############################################################################
 193 
//############################################################################
194 
{      
 194 
{      
195 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 195 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
196 
    ANALOG_OFF;
 196 
    ANALOG_OFF;
197 
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 197 
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
198 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 198 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
199 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 199 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
200 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
 200 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
201 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
 201 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
202 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
 202 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
203 
    AccumulateNick = 0;   MessanzahlNick = 0;
 203 
    AccumulateNick = 0;   MessanzahlNick = 0;
204 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 204 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
205 
    AccumulateGier = 0;   MessanzahlGier = 0;
 205 
    AccumulateGier = 0;   MessanzahlGier = 0;
206 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 206 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
207 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 207 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
208 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 208 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
209 
    Integral_Gier  = Mess_Integral_Gier;
 209 
    Integral_Gier  = Mess_Integral_Gier;
210 
//    Integral_Gier2 = Mess_Integral_Gier2;
 210 
//    Integral_Gier2 = Mess_Integral_Gier2;
211 
    IntegralNick = Mess_IntegralNick;
 211 
    IntegralNick = Mess_IntegralNick;
212 
    IntegralRoll = Mess_IntegralRoll;
 212 
    IntegralRoll = Mess_IntegralRoll;
213 
    IntegralNick2 = Mess_IntegralNick2;
 213 
    IntegralNick2 = Mess_IntegralNick2;
214 
    IntegralRoll2 = Mess_IntegralRoll2;
 214 
    IntegralRoll2 = Mess_IntegralRoll2;
215 
    // ADC einschalten
 215 
    // ADC einschalten
216 
    ANALOG_ON; 
 216 
    ANALOG_ON; 
217 
 
 217 
 
218 
/*
 218 
/*
219 
//------------------------------------------------------------------------------
 219 
//------------------------------------------------------------------------------
220 
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
 220 
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
221 
    else                                         
 221 
    else                                         
222 
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 222 
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
223 
 
 223 
 
224 
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
 224 
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
225 
    else                                         
 225 
    else                                         
226 
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 226 
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
227 
//------------------------------------------------------------------------------
 227 
//------------------------------------------------------------------------------
228 
*/
 228 
*/
229 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 229 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
230 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 230 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
231 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 231 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
232 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 232 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
233 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 233 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
234 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 234 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
235 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 235 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
236 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 236 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
237 
}
 237 
}
238 
 
 238 
 
239 
//############################################################################
 239 
//############################################################################
240 
// Messwerte beim Ermitteln der Nullage
 240 
// Messwerte beim Ermitteln der Nullage
241 
void CalibrierMittelwert(void)
 241 
void CalibrierMittelwert(void)
242 
//############################################################################
 242 
//############################################################################
243 
{                
 243 
{                
244 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 244 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
245 
        ANALOG_OFF;
 245 
        ANALOG_OFF;
246 
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 246 
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
247 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 247 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
248 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 248 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
249 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
 249 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
250 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
 250 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
251 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
 251 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
252 
    AccumulateNick = 0;   MessanzahlNick = 0;
 252 
    AccumulateNick = 0;   MessanzahlNick = 0;
253 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 253 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
254 
    AccumulateGier = 0;   MessanzahlGier = 0;
 254 
    AccumulateGier = 0;   MessanzahlGier = 0;
255 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 255 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
256 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 256 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
257 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 257 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
258 
    // ADC einschalten
 258 
    // ADC einschalten
259 
    ANALOG_ON; 
 259 
    ANALOG_ON; 
260 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 260 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
261 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 261 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
262 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 262 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
263 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 263 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
264 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 264 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
265 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 265 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
266 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 266 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
267 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 267 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
268 
}
 268 
}
269 
 
 269 
 
270 
//############################################################################
 270 
//############################################################################
271 
// Senden der Motorwerte per I2C-Bus
 271 
// Senden der Motorwerte per I2C-Bus
272 
void SendMotorData(void)
 272 
void SendMotorData(void)
273 
//############################################################################
 273 
//############################################################################
274 
{
 274 
{
275 
    if(MOTOR_OFF || !MotorenEin)
 275 
    if(MOTOR_OFF || !MotorenEin)
276 
        {
 276 
        {
277 
        Motor_Hinten = 0;
 277 
        Motor_Hinten = 0;
278 
        Motor_Vorne = 0;
 278 
        Motor_Vorne = 0;
279 
        Motor_Rechts = 0;
 279 
        Motor_Rechts = 0;
280 
        Motor_Links = 0;
 280 
        Motor_Links = 0;
281 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 281 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
282 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 282 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
283 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 283 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
284 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 284 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
285 
        }
 285 
        }
286 
 
 286 
 
287 
    DebugOut.Analog[12] = Motor_Vorne;
 287 
    DebugOut.Analog[12] = Motor_Vorne;
288 
    DebugOut.Analog[13] = Motor_Hinten;
 288 
    DebugOut.Analog[13] = Motor_Hinten;
289 
    DebugOut.Analog[14] = Motor_Links;
 289 
    DebugOut.Analog[14] = Motor_Links;
290 
    DebugOut.Analog[15] = Motor_Rechts;  
 290 
    DebugOut.Analog[15] = Motor_Rechts;  
291 
 
 291 
 
292 
    //Start I2C Interrupt Mode
 292 
    //Start I2C Interrupt Mode
293 
    twi_state = 0;
 293 
    twi_state = 0;
294 
    motor = 0;
 294 
    motor = 0;
295 
    i2c_start();
 295 
    i2c_start();
296 
}
 296 
}
297 
 
 297 
 
298 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 298 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
299 
// + Konstanten
299 
// + Konstanten
300 
// + 0-250 -> normale Werte
 300 
// + 0-250 -> normale Werte
301 
// + 251 -> Poti1
 301 
// + 251 -> Poti1
302 
// + 252 -> Poti2
 302 
// + 252 -> Poti2
303 
// + 253 -> Poti3
 303 
// + 253 -> Poti3
304 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 304 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
305 
void DefaultKonstanten1(void)
 305 
void DefaultKonstanten1(void)
306 
{
 306 
{
307 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 307 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
308 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 308 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
309 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 309 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
310 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 310 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
311 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 311 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
312 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 312 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
313 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 313 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
314 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
 314 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
315 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
 315 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
316 
 EE_Parameter.Hoehe_MinGas = 30;
 316 
 EE_Parameter.Hoehe_MinGas = 30;
317 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
 317 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
318 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
 318 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
319 
 EE_Parameter.Luftdruck_D  = 70;      // Wert : 0-250
 319 
 EE_Parameter.Luftdruck_D  = 70;      // Wert : 0-250
320 
 EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
 320 
 EE_Parameter.Hoehe_ACC_Wirkung = 30; // Wert : 0-250
321 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
 321 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
322 
 EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
 322 
 EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
323 
 EE_Parameter.Stick_D = 8; //8           // Wert : 0-64
 323 
 EE_Parameter.Stick_D = 8; //8           // Wert : 0-64
324 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
 324 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
325 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 325 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
326 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
 326 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
327 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
 327 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
328 
 EE_Parameter.KompassWirkung = 16;    // Wert : 0-250
 328 
 EE_Parameter.KompassWirkung = 16;    // Wert : 0-250
329 
 EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
 329 
 EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
330 
 EE_Parameter.Gyro_I = 150;               // Wert : 0-250
 330 
 EE_Parameter.Gyro_I = 150;               // Wert : 0-250
331 
 EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250
 331 
 EE_Parameter.UnterspannungsWarnung = 90; // Wert : 0-250
332 
 EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
 332 
 EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
333 
 EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 333 
 EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
334 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 334 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
335 
 EE_Parameter.I_Faktor = 5;
 335 
 EE_Parameter.I_Faktor = 5;
336 
 EE_Parameter.UserParam1 = 178;             //zur freien Verwendung
 336 
 EE_Parameter.UserParam1 = 178;             //zur freien Verwendung
337 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
 337 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
338 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
 338 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
339 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
 339 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
340 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 340 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
341 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 341 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
342 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 342 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
343 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
 343 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
344 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 344 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
345 
 EE_Parameter.ServoNickRefresh = 5;
 345 
 EE_Parameter.ServoNickRefresh = 5;
346 
 memcpy(EE_Parameter.Name, "Normal\0", 12);  
 346 
 memcpy(EE_Parameter.Name, "Normal\0", 12);  
347 
}
 347 
}
348 
 
 348 
 
349 
void DefaultKonstanten2(void)
 349 
void DefaultKonstanten2(void)
350 
{
 350 
{
351 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 351 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
352 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 352 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
353 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 353 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
354 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 354 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
355 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 355 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
356 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 356 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
357 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 357 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
358 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
 358 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
359 
 EE_Parameter.Hoehe_MinGas = 30;
 359 
 EE_Parameter.Hoehe_MinGas = 30;
360 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
 360 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
361 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
 361 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
362 
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
 362 
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
363 
 EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
 363 
 EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
364 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
 364 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
365 
 EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
 365 
 EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
366 
 EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
 366 
 EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
367 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
 367 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
368 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 368 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
369 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
 369 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
370 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
 370 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
371 
 EE_Parameter.KompassWirkung = 16;    // Wert : 0-250
 371 
 EE_Parameter.KompassWirkung = 16;    // Wert : 0-250
372 
 EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
 372 
 EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
373 
 EE_Parameter.Gyro_I = 175;           // Wert : 0-250
 373 
 EE_Parameter.Gyro_I = 175;           // Wert : 0-250
374 
 EE_Parameter.UnterspannungsWarnung = 90;  // Wert : 0-250
 374 
 EE_Parameter.UnterspannungsWarnung = 90;  // Wert : 0-250
375 
 EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
 375 
 EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
376 
 EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 376 
 EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
377 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 377 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
378 
 EE_Parameter.I_Faktor = 5;
 378 
 EE_Parameter.I_Faktor = 5;
379 
 EE_Parameter.UserParam1 = 178;   //zur freien Verwendung
 379 
 EE_Parameter.UserParam1 = 178;   //zur freien Verwendung
380 
 EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
 380 
 EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
381 
 EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
 381 
 EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
382 
 EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
 382 
 EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
383 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
 383 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
384 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
 384 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
385 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 385 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
386 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 386 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
387 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 387 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
388 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
 388 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
389 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 389 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
390 
 EE_Parameter.ServoNickRefresh = 5;
 390 
 EE_Parameter.ServoNickRefresh = 5;
391 
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
 391 
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
392 
}
 392 
}
393 
 
 393 
 
394 
 
 394 
 
395 
//############################################################################
 395 
//############################################################################
396 
// Trägt ggf. das Poti als Parameter ein
 396 
// Trägt ggf. das Poti als Parameter ein
397 
void ParameterZuordnung(void)
 397 
void ParameterZuordnung(void)
398 
//############################################################################
 398 
//############################################################################
399 
{
 399 
{
400 
 
 400 
 
401 
 #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;}
 401 
 #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;}
402 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 402 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
403 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 403 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
404 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 404 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
405 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 405 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
406 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 406 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
407 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 407 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
408 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 408 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
409 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 409 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
410 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 410 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
411 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 411 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
412 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 412 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
413 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 413 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
414 
 
 414 
 
415 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 415 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
416 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 416 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
417 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 417 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
418 
 
 418 
 
419 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 419 
 Ki = (float) Parameter_I_Faktor * 0.0001;
420 
 MAX_GAS = EE_Parameter.Gas_Max;
 420 
 MAX_GAS = EE_Parameter.Gas_Max;
421 
 MIN_GAS = EE_Parameter.Gas_Min;
 421 
 MIN_GAS = EE_Parameter.Gas_Min;
422 
}
 422 
}
423 
 
 423 
 
424 
 
 424 
 
425 
//############################################################################
 425 
//############################################################################
426 
//
 426 
//
427 
void MotorRegler(void)
 427 
void MotorRegler(void)
428 
//############################################################################
 428 
//############################################################################
429 
{
 429 
{
430 
         int motorwert,pd_ergebnis,h,tmp_int;
 430 
         int motorwert,pd_ergebnis,h,tmp_int;
431 
         int GierMischanteil,GasMischanteil;
 431 
         int GierMischanteil,GasMischanteil;
432 
     static long SummeNick=0,SummeRoll=0;
 432 
     static long SummeNick=0,SummeRoll=0;
433 
     static long sollGier = 0,tmp_long,tmp_long2;
 433 
     static long sollGier = 0,tmp_long,tmp_long2;
434 
     static int IntegralFehlerNick = 0;
 434 
     static int IntegralFehlerNick = 0;
435 
     static int IntegralFehlerRoll = 0;
 435 
     static int IntegralFehlerRoll = 0;
436 
         static unsigned int RcLostTimer;
 436 
         static unsigned int RcLostTimer;
437 
         static unsigned char delay_neutral = 0;
 437 
         static unsigned char delay_neutral = 0;
438 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 438 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
439 
         static unsigned int  modell_fliegt = 0;
 439 
         static unsigned int  modell_fliegt = 0;
440 
     static int hoehenregler = 0;
 440 
     static int hoehenregler = 0;
441 
     static char TimerWerteausgabe = 0;
 441 
     static char TimerWerteausgabe = 0;
442 
     static char NeueKompassRichtungMerken = 1;
 442 
     static char NeueKompassRichtungMerken = 1;
443 
        Mittelwert();
 443 
        Mittelwert();
444 
 
 444 
 
445 
    GRN_ON;
 445 
    GRN_ON;
446 
 
 446 
 
447 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
447 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
448 
// Gaswert ermitteln
 448 
// Gaswert ermitteln
449 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
449 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
450 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 450 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
451 
    if(GasMischanteil < 0) GasMischanteil = 0;
 451 
    if(GasMischanteil < 0) GasMischanteil = 0;
452 
   
 452 
   
453 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
453 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
454 
// Emfang schlecht
 454 
// Emfang schlecht
455 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
455 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
456 
   if(SenderOkay < 100)
 456 
   if(SenderOkay < 100)
457 
        {
 457 
        {
458 
        if(!PcZugriff)  beeptime = 500;
 458 
        if(!PcZugriff)  beeptime = 500;
459 
        if(RcLostTimer) RcLostTimer--;
 459 
        if(RcLostTimer) RcLostTimer--;
460 
        else
 460 
        else
461 
         {
 461 
         {
462 
          MotorenEin = 0;
 462 
          MotorenEin = 0;
463 
          Notlandung = 0;
 463 
          Notlandung = 0;
464 
         }
 464 
         }
465 
        ROT_ON;
 465 
        ROT_ON;
466 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 466 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
467 
            {
 467 
            {
468 
            GasMischanteil = EE_Parameter.NotGas;
 468 
            GasMischanteil = EE_Parameter.NotGas;
469 
            Notlandung = 1;
 469 
            Notlandung = 1;
470 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 470 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
471 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 471 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
472 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 472 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
473 
/*          Poti1 = 65;
 473 
/*          Poti1 = 65;
474 
            Poti2 = 48;
 474 
            Poti2 = 48;
475 
            Poti3 = 0;
 475 
            Poti3 = 0;
476 
*/          }
 476 
*/          }
477 
         else MotorenEin = 0;
 477 
         else MotorenEin = 0;
478 
        }
 478 
        }
479 
        else
 479 
        else
480 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
480 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
481 
// Emfang gut
 481 
// Emfang gut
482 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
482 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
483 
        if(SenderOkay > 140)
 483 
        if(SenderOkay > 140)
484 
            {
 484 
            {
485 
            Notlandung = 0;
 485 
            Notlandung = 0;
486 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 486 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
487 
            if(GasMischanteil > 40)
 487 
            if(GasMischanteil > 40)
488 
                {
 488 
                {
489 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 489 
                if(modell_fliegt < 0xffff) modell_fliegt++;
490 
                }
 490 
                }
491 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 491 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
492 
                {
 492 
                {
493 
                SummeNick = 0;
 493 
                SummeNick = 0;
494 
                SummeRoll = 0;
 494 
                SummeRoll = 0;
495 
                Mess_Integral_Gier = 0;
 495 
                Mess_Integral_Gier = 0;
496 
                Mess_Integral_Gier2 = 0;
 496 
                Mess_Integral_Gier2 = 0;
497 
                }
 497 
                }
498 
            if((GasMischanteil > 200) && MotorenEin == 0)
 498 
            if((GasMischanteil > 200) && MotorenEin == 0)
499 
                {
 499 
                {
500 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
500 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
501 
// auf Nullwerte kalibrieren
 501 
// auf Nullwerte kalibrieren
502 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
502 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
503 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 503 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
504 
                    {
 504 
                    {
505 
                    unsigned char setting;
 505 
                    unsigned char setting;
506 
                    if(++delay_neutral > 200)  // nicht sofort
 506 
                    if(++delay_neutral > 200)  // nicht sofort
507 
                        {
 507 
                        {
508 
                        GRN_OFF;
 508 
                        GRN_OFF;
509 
                        SetNeutral();
 509 
                        SetNeutral();
510 
                        MotorenEin = 0;
 510 
                        MotorenEin = 0;
511 
                        delay_neutral = 0;
 511 
                        delay_neutral = 0;
512 
                        modell_fliegt = 0;
 512 
                        modell_fliegt = 0;
513 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 513 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
514 
                        {
 514 
                        {
515 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 515 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
516 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 516 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
517 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 517 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
518 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 518 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
519 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 519 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
520 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 520 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
521 
                        }
 521 
                        }
522 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
 522 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], sizeof(struct mk_param_struct));
523 
                        Piep(GetActiveParamSetNumber());
 523 
                        Piep(GetActiveParamSetNumber());
524 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 524 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
525 
                          {
 525 
                          {
526 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 526 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
527 
                          }  
 527 
                          }  
528 
                        }
 528 
                        }
529 
                    }
 529 
                    }
530 
                 else delay_neutral = 0;
 530 
                 else delay_neutral = 0;
531 
                }
 531 
                }
532 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
532 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
533 
// Gas ist unten
 533 
// Gas ist unten
534 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
534 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
535 
            if(GasMischanteil < 35)
 535 
            if(GasMischanteil < 35)
536 
                {
 536 
                {
537 
                // Starten
 537 
                // Starten
538 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 538 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
539 
                    {
 539 
                    {
540 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
540 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
541 
// Einschalten
 541 
// Einschalten
542 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
542 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
543 
                    if(++delay_einschalten > 200)
 543 
                    if(++delay_einschalten > 200)
544 
                        {
 544 
                        {
545 
                        delay_einschalten = 200;
 545 
                        delay_einschalten = 200;
546 
                        modell_fliegt = 1;
 546 
                        modell_fliegt = 1;
547 
                        MotorenEin = 1;
 547 
                        MotorenEin = 1;
548 
                        sollGier = 0;
 548 
                        sollGier = 0;
549 
                        Mess_Integral_Gier = 0;
 549 
                        Mess_Integral_Gier = 0;
550 
                        Mess_Integral_Gier2 = 0;
 550 
                        Mess_Integral_Gier2 = 0;
551 
                        Mess_IntegralNick = 0;
 551 
                        Mess_IntegralNick = 0;
552 
                        Mess_IntegralRoll = 0;
 552 
                        Mess_IntegralRoll = 0;
553 
                        Mess_IntegralNick2 = IntegralNick;
 553 
                        Mess_IntegralNick2 = IntegralNick;
554 
                        Mess_IntegralRoll2 = IntegralRoll;
 554 
                        Mess_IntegralRoll2 = IntegralRoll;
555 
                        SummeNick = 0;
 555 
                        SummeNick = 0;
556 
                        SummeRoll = 0;
 556 
                        SummeRoll = 0;
557 
                        }          
 557 
                        }          
558 
                    }  
 558 
                    }  
559 
                    else delay_einschalten = 0;
 559 
                    else delay_einschalten = 0;
560 
                //Auf Neutralwerte setzen
 560 
                //Auf Neutralwerte setzen
561 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
561 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
562 
// Auschalten
 562 
// Auschalten
563 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
563 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
564 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 564 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
565 
                    {
 565 
                    {
566 
                    if(++delay_ausschalten > 200)  // nicht sofort
 566 
                    if(++delay_ausschalten > 200)  // nicht sofort
567 
                        {
 567 
                        {
568 
                        MotorenEin = 0;
 568 
                        MotorenEin = 0;
569 
                        delay_ausschalten = 200;
 569 
                        delay_ausschalten = 200;
570 
                        modell_fliegt = 0;
 570 
                        modell_fliegt = 0;
571 
                        }
 571 
                        }
572 
                    }
 572 
                    }
573 
                else delay_ausschalten = 0;
 573 
                else delay_ausschalten = 0;
574 
                }
 574 
                }
575 
            }
 575 
            }
576 
 
 576 
 
577 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
577 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
578 
// neue Werte von der Funke
 578 
// neue Werte von der Funke
579 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
579 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
580 
 if(!NewPpmData-- || Notlandung)  
 580 
 if(!NewPpmData-- || Notlandung)  
581 
  {
 581 
  {
582 
    ParameterZuordnung();
 582 
    ParameterZuordnung();
583 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 583 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
584 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 584 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
585 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 585 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
586 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 586 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
587 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 587 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
588 
 
 588 
 
589 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 589 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
590 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 590 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
591 
 
 591 
 
592 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 592 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
593 
    if(GyroFaktor < 0) GyroFaktor = 0;
 593 
    if(GyroFaktor < 0) GyroFaktor = 0;
594 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 594 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
595 
  }
 595 
  }
596 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
596 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
597 
// Bei Empfangsausfall im Flug
597 
// Bei Empfangsausfall im Flug
598 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
598 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
599 
   if(Notlandung)
 599 
   if(Notlandung)
600 
    {
 600 
    {
601 
     StickGier = 0;
 601 
     StickGier = 0;
602 
     StickNick = 0;
 602 
     StickNick = 0;
603 
     StickRoll = 0;
 603 
     StickRoll = 0;
604 
     GyroFaktor  = 0.1;
 604 
     GyroFaktor  = 0.1;
605 
     IntegralFaktor = 0.005;
 605 
     IntegralFaktor = 0.005;
606 
    }  
 606 
    }  
607 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
607 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
608 
// Gyro-Drift kompensieren
 608 
// Gyro-Drift kompensieren
609 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
609 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
610 
#define DRIFT_FAKTOR 3
 610 
#define DRIFT_FAKTOR 3
611 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
 611 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
612 
        {
 612 
        {
613 
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
 613 
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
614 
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
 614 
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
615 
        ZaehlMessungen = 0;
 615 
        ZaehlMessungen = 0;
616 
        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
 616 
        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
617 
        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
 617 
        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
618 
        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
 618 
        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
619 
        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
 619 
        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
620 
        //if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;        
620 
        //if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;        
621 
        //if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;        
621 
        //if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;        
622 
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 622 
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
623 
        Mess_IntegralNick2 = IntegralNick;
 623 
        Mess_IntegralNick2 = IntegralNick;
624 
        Mess_IntegralRoll2 = IntegralRoll;
 624 
        Mess_IntegralRoll2 = IntegralRoll;
625 
        Mess_Integral_Gier2 = Integral_Gier;
 625 
        Mess_Integral_Gier2 = Integral_Gier;
626 
    ANALOG_ON;  // ADC einschalten
 626 
    ANALOG_ON;  // ADC einschalten
627 
        }
 627 
        }
628 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
628 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
629 
// Integrale auf ACC-Signal abgleichen
 629 
// Integrale auf ACC-Signal abgleichen
630 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
630 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
631 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
 631 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16;  
632 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
 632 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16;  
633 
#define AUSGLEICH 500
 633 
#define AUSGLEICH 500
634 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 634 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
635 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 635 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
636 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 636 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
637 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 637 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
638 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 638 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
639 
    Mess_IntegralNick -= tmp_long;
 639 
    Mess_IntegralNick -= tmp_long;
640 
    Mess_IntegralRoll -= tmp_long2;
 640 
    Mess_IntegralRoll -= tmp_long2;
- 
 
 641 
 
641 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
642 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
642 
//  Gieren
 643 
//  Gieren wie in .66c
643 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
644 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
644 
    sollGier = StickGier;
 - 
 
645 
    if(abs(StickGier) > 35)
 645 
    if(abs(StickGier) > 20) // war 35
646 
     {
 646 
     {
647 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 647 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
648 
     }
 648 
     }
649 
    tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo
 649 
    tmp_int  = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
- 
 
 650 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
- 
 
 651 
    sollGier = tmp_int;
650 
    Mess_Integral_Gier -= tmp_int;  
 652 
    Mess_Integral_Gier -= tmp_int;  
651 
    if(Mess_Integral_Gier > 30000) Mess_Integral_Gier = 30000;  // begrenzen
 653 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
652 
    if(Mess_Integral_Gier <-30000) Mess_Integral_Gier =-30000;
 654 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
653 
   
 655 
   
654 
 ANALOG_ON;     // ADC einschalten
 656 
 ANALOG_ON;     // ADC einschalten
655 
 
 657 
 
656 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
658 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
657 
//  Kompass
 659 
//  Kompass
658 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
660 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
659 
    if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
 661 
    if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
660 
     {
 662 
     {
661 
       int w,v;
 663 
       int w,v;
662 
       static uint8_t SignalSchlecht = 0;
 - 
 
663 
 
 664 
 
664 
                if (!updKompass--)              // Aufruf mit ~10 Hz
 665 
                if (!updKompass--)              // Aufruf mit ~10 Hz
665 
                {
 666 
                {
666 
                        KompassValue = heading_MM3();
 667 
                        KompassValue = heading_MM3();
667 
                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
 668 
                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
668 
                        updKompass = 50;
 669 
                        updKompass = 50;
669 
                }
 670 
                }
670 
 
 671 
 
671 
       w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln
 672 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
672 
       v = abs(IntegralRoll /1024);
 673 
       v = abs(IntegralRoll /512);
673 
       if(v > w) w = v; // grösste Neigung ermitteln
 674 
       if(v > w) w = v; // grösste Neigung ermitteln
674 
       if(w < 35 && NeueKompassRichtungMerken && !SignalSchlecht)    
 675 
       if(w < 35 && NeueKompassRichtungMerken)    
675 
        {
 676 
        {
676 
         KompassStartwert = KompassValue;
 677 
         KompassStartwert = KompassValue;
677 
         NeueKompassRichtungMerken = 0;
 678 
         NeueKompassRichtungMerken = 0;
678 
        }
 679 
        }
679 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 680 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
680 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 681 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
681 
       if(w > 0)
 682 
       if(w > 0)
682 
       {
 683 
       {
683 
                if(!SignalSchlecht)
 - 
 
684 
                {
 - 
 
685 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 684 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
686 
                        Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 685 
                        Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
687 
                        ANALOG_ON;  // ADC einschalten
 686 
                        ANALOG_ON;  // ADC einschalten
688 
                }
 687 
           }
689 
                else SignalSchlecht--;
 688 
           else beeptime = 100;    
690 
               
 - 
 
691 
       }  
 - 
 
692 
       else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek
 - 
 
693 
     }
 689 
     }
- 
 
 690 
 
694 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
691 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
695 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
692 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
696 
 
 693 
 
697 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
694 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
698 
//  Debugwerte zuordnen
 695 
//  Debugwerte zuordnen
699 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
696 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
700 
DebugOut.Sekunden++;
 697 
DebugOut.Sekunden++;
701 
  if(!TimerWerteausgabe--)
 698 
  if(!TimerWerteausgabe--)
702 
   {
 699 
   {
703 
    TimerWerteausgabe = 49;
 700 
    TimerWerteausgabe = 49;
704 
//    DebugOut.Analog[0] = MesswertNick;
 701 
//    DebugOut.Analog[0] = MesswertNick;
705 
//    DebugOut.Analog[1] = MesswertRoll;
 702 
//    DebugOut.Analog[1] = MesswertRoll;
706 
//    DebugOut.Analog[2] = MesswertGier;
 703 
//    DebugOut.Analog[2] = MesswertGier;
707 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 704 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
708 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 705 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
709 
 
 706 
 
710 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 707 
    DebugOut.Analog[2] = Mittelwert_AccNick;
711 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 708 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
712 
    DebugOut.Analog[4] = MesswertGier;
 709 
    DebugOut.Analog[4] = MesswertGier;
713 
    DebugOut.Analog[5] = HoehenWert;
 710 
    DebugOut.Analog[5] = HoehenWert;
714 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 711 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
715 
    DebugOut.Analog[7] = GasMischanteil;
 712 
    DebugOut.Analog[7] = GasMischanteil;
716 
    DebugOut.Analog[8] = KompassValue;
 713 
    DebugOut.Analog[8] = KompassValue;
717 
 
 714 
 
718 
// Kanäle 9 bis 11 in compass.c
 715 
// Kanäle 9 bis 11 in compass.c
719 
 
 716 
 
720 
//    DebugOut.Analog[9] = SollHoehe;
 717 
//    DebugOut.Analog[9] = SollHoehe;
721 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 718 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
722 
//    DebugOut.Analog[11] = KompassStartwert;
 719 
//    DebugOut.Analog[11] = KompassStartwert;
723 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 720 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
724 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 721 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
725 
//    DebugOut.Analog[9] = KompassRichtung;    
 722 
//    DebugOut.Analog[9] = KompassRichtung;    
726 
//    DebugOut.Analog[10] = GasMischanteil;
 723 
//    DebugOut.Analog[10] = GasMischanteil;
727 
//    DebugOut.Analog[3] = HoeheD * 32;
 724 
//    DebugOut.Analog[3] = HoeheD * 32;
728 
//    DebugOut.Analog[4] = hoehenregler;
 725 
//    DebugOut.Analog[4] = hoehenregler;
729 
  }
 726 
  }
730 
 
 727 
 
731 
 
 728 
 
732 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
729 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
733 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 730 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
734 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
731 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
735 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 732 
    MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
736 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 733 
    MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
737 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;    
 734 
    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;    
738 
    // Maximalwerte abfangen
 735 
    // Maximalwerte abfangen
739 
    #define MAX_SENSOR  2048
 736 
    #define MAX_SENSOR  2048
740 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 737 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
741 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 738 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
742 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 739 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
743 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 740 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
744 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 741 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
745 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 742 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
746 
 
 743 
 
747 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
744 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
748 
// Höhenregelung
 745 
// Höhenregelung
749 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 746 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
750 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
747 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
751 
//OCR0B = 180 - (Poti1 + 120) / 4;
 748 
//OCR0B = 180 - (Poti1 + 120) / 4;
752 
//DruckOffsetSetting = OCR0B;
 749 
//DruckOffsetSetting = OCR0B;
753 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 750 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
754 
  {
 751 
  {
755 
    int tmp_int;
 752 
    int tmp_int;
756 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 753 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
757 
    {
 754 
    {
758 
     if(Parameter_MaxHoehe < 50)
 755 
     if(Parameter_MaxHoehe < 50)
759 
      {
 756 
      {
760 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 757 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
761 
       HoehenReglerAktiv = 0;
 758 
       HoehenReglerAktiv = 0;
762 
      }
 759 
      }
763 
      else  
 760 
      else  
764 
        HoehenReglerAktiv = 1;
 761 
        HoehenReglerAktiv = 1;
765 
    }
 762 
    }
766 
    else
 763 
    else
767 
    {
 764 
    {
768 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
 765 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
769 
     HoehenReglerAktiv = 1;
 766 
     HoehenReglerAktiv = 1;
770 
    }
 767 
    }
771 
 
 768 
 
772 
    if(Notlandung) SollHoehe = 0;
 769 
    if(Notlandung) SollHoehe = 0;
773 
    h = HoehenWert;
 770 
    h = HoehenWert;
774 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 771 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
775 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 772 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
776 
      h = GasMischanteil - h;         // vom Gas abziehen
 773 
      h = GasMischanteil - h;         // vom Gas abziehen
777 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 774 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
778 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 775 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
779 
      if(tmp_int > 50) tmp_int = 50;
 776 
      if(tmp_int > 50) tmp_int = 50;
780 
      else if(tmp_int < -50) tmp_int = -50;
 777 
      else if(tmp_int < -50) tmp_int = -50;
781 
      h -= tmp_int;
 778 
      h -= tmp_int;
782 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 779 
      hoehenregler = (hoehenregler*15 + h) / 16;      
783 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 780 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
784 
       {
 781 
       {
785 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 782 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
786 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 783 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
787 
       }  
 784 
       }  
788 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 785 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
789 
      GasMischanteil = hoehenregler;
 786 
      GasMischanteil = hoehenregler;
790 
     }
 787 
     }
791 
  }
 788 
  }
792 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
789 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
793 
// + Mischer und PI-Regler
 790 
// + Mischer und PI-Regler
794 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
791 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
795 
 
 792 
 
796 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
793 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
797 
// Gier-Anteil
 794 
// Gier-Anteil
798 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
795 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
799 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 796 
        GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
800 
    if(GierMischanteil > 100)  GierMischanteil = 100;
 797 
    if(GierMischanteil > 100)  GierMischanteil = 100;
801 
    if(GierMischanteil < -100) GierMischanteil = -100;
 798 
    if(GierMischanteil < -100) GierMischanteil = -100;
802 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
799 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
803 
// Nick-Achse
 800 
// Nick-Achse
804 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
801 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
805 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
 802 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
806 
    SummeNick += DiffNick;                                   // I-Anteil
 803 
    SummeNick += DiffNick;                                   // I-Anteil
807 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 804 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
808 
    if(SummeNick >  16000) SummeNick =  16000;
 805 
    if(SummeNick >  16000) SummeNick =  16000;
809 
    if(SummeNick < -16000) SummeNick = -16000;
 806 
    if(SummeNick < -16000) SummeNick = -16000;
810 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
807 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
811 
    // Motor Vorn
 808 
    // Motor Vorn
812 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 809 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
813 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 810 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
814 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 811 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
815 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 812 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
816 
        Motor_Vorne = motorwert;           
 813 
        Motor_Vorne = motorwert;           
817 
    // Motor Heck
 814 
    // Motor Heck
818 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 815 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
819 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 816 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
820 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 817 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
821 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 818 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
822 
        Motor_Hinten = motorwert;              
 819 
        Motor_Hinten = motorwert;              
823 
 
 820 
 
824 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
821 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
825 
// Roll-Achse
 822 
// Roll-Achse
826 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
823 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
827 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
 824 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
828 
        SummeRoll += DiffRoll;                                                              // I-Anteil
 825 
        SummeRoll += DiffRoll;                                                              // I-Anteil
829 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 826 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
830 
    if(SummeRoll >  16000) SummeRoll =  16000;
 827 
    if(SummeRoll >  16000) SummeRoll =  16000;
831 
    if(SummeRoll < -16000) SummeRoll = -16000;
 828 
    if(SummeRoll < -16000) SummeRoll = -16000;
832 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 829 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
833 
    // Motor Links
 830 
    // Motor Links
834 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 831 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
835 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
 832 
        if ((motorwert < 0) | (GasMischanteil < 10)) motorwert = 0;
836 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 833 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
837 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 834 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
838 
        Motor_Links = motorwert;               
 835 
        Motor_Links = motorwert;               
839 
    // Motor Rechts
 836 
    // Motor Rechts
840 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 837 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
841 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
 838 
        if ((motorwert < 0) | (GasMischanteil < 10))    motorwert = 0;
842 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 839 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
843 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 840 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
844 
        Motor_Rechts = motorwert;
 841 
        Motor_Rechts = motorwert;
845 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 842 
   // +++++++++++++++++++++++++++++++++++++++++++++++
846 
 
 843 
 
847 
}
 844 
}
848 
 
 845 
 
849 
 
 846