Subversion Repositories FlightCtrl

Rev

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

Rev 304 Rev 395
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 
#include "eeprom.c"
57 
 
 58 
 
58 
unsigned char h,m,s;
 59 
unsigned char h,m,s;
59 
volatile unsigned int I2CTimeout = 100;
 60 
volatile unsigned int I2CTimeout = 100;
60 
volatile int MesswertNick,MesswertRoll,MesswertGier;
 61 
volatile int MesswertNick,MesswertRoll,MesswertGier;
61 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
 62 
volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0;
62 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
 63 
volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
63 
volatile float NeutralAccZ = 0;
 64 
volatile float NeutralAccZ = 0;
64 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
 65 
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
65 
volatile long IntegralNick = 0,IntegralNick2 = 0;
 66 
volatile long IntegralNick = 0,IntegralNick2 = 0;
66 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
 67 
volatile long IntegralRoll = 0,IntegralRoll2 = 0;
- 
 
 68 
volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
67 
volatile long Integral_Gier = 0;
 69 
volatile long Integral_Gier = 0;
68 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
 70 
volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
69 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
 71 
volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
70 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
 72 
volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
- 
 
 73 
volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
71 
volatile long Mess_Integral_Hoch = 0;
 74 
volatile long Mess_Integral_Hoch = 0;
72 
volatile int  KompassValue = 0;
 75 
volatile int  KompassValue = 0;
73 
volatile int  KompassStartwert = 0;
 76 
volatile int  KompassStartwert = 0;
74 
volatile int  KompassRichtung = 0;
 77 
volatile int  KompassRichtung = 0;
75 
unsigned char MAX_GAS,MIN_GAS;
 78 
unsigned char MAX_GAS,MIN_GAS;
76 
unsigned char Notlandung = 0;
 79 
unsigned char Notlandung = 0;
77 
unsigned char HoehenReglerAktiv = 0;
 80 
unsigned char HoehenReglerAktiv = 0;
- 
 
 81 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
78 
 
 82 
 
79 
float GyroFaktor;
 83 
float GyroFaktor;
80 
float IntegralFaktor;
 84 
float IntegralFaktor;
81 
 
 85 
 
82 
volatile int  DiffNick,DiffRoll;
 86 
volatile int  DiffNick,DiffRoll;
83 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
 87 
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
84 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
 88 
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
85 
unsigned char MotorWert[5];
 89 
unsigned char MotorWert[5];
86 
volatile unsigned char SenderOkay = 0;
 90 
volatile unsigned char SenderOkay = 0;
87 
int StickNick = 0,StickRoll = 0,StickGier = 0;
 91 
int StickNick = 0,StickRoll = 0,StickGier = 0;
88 
char MotorenEin = 0;
 92 
char MotorenEin = 0;
89 
int HoehenWert = 0;
 93 
int HoehenWert = 0;
90 
int SollHoehe = 0;
 94 
int SollHoehe = 0;
91 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
 95 
int I_LageRoll = 0,I_LageNick = 0;
92 
 
 - 
 
93 
float Kp =  FAKTOR_P;
 96 
float Kp =  FAKTOR_P;
94 
float Ki =  FAKTOR_I;
 97 
float Ki =  FAKTOR_I;
- 
 
 98 
unsigned char Looping_Nick = 0,Looping_Roll = 0;
- 
 
 99 
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
95 
 
 100 
 
96 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
 101 
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
97 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
 102 
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
98 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
 103 
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
99 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
 104 
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
100 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
 105 
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
101 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
 106 
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
102 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
 107 
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
103 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
 108 
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
104 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
 109 
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
105 
unsigned char Parameter_UserParam1 = 0;
 110 
unsigned char Parameter_UserParam1 = 0;
106 
unsigned char Parameter_UserParam2 = 0;
 111 
unsigned char Parameter_UserParam2 = 0;
107 
unsigned char Parameter_UserParam3 = 0;
 112 
unsigned char Parameter_UserParam3 = 0;
108 
unsigned char Parameter_UserParam4 = 0;
 113 
unsigned char Parameter_UserParam4 = 0;
109 
unsigned char Parameter_ServoNickControl = 100;
 114 
unsigned char Parameter_ServoNickControl = 100;
110 
unsigned char Parameter_LoopGasLimit = 70;
 115 
unsigned char Parameter_LoopGasLimit = 70;
- 
 
 116 
unsigned char Parameter_AchsKopplung1 = 0;
- 
 
 117 
unsigned char Parameter_AchsGegenKopplung1 = 0;
111 
struct mk_param_struct EE_Parameter;
 118 
struct mk_param_struct EE_Parameter;
112 
 
 119 
 
113 
void Piep(unsigned char Anzahl)
 120 
void Piep(unsigned char Anzahl)
114 
{
 121 
{
115 
 while(Anzahl--)
 122 
 while(Anzahl--)
116 
 {
 123 
 {
117 
  if(MotorenEin) return; //auf keinen Fall im Flug!
 124 
  if(MotorenEin) return; //auf keinen Fall im Flug!
118 
  beeptime = 100;
 125 
  beeptime = 100;
119 
  Delay_ms(250);
 126 
  Delay_ms(250);
120 
 }
 127 
 }
121 
}
 128 
}
122 
 
 129 
 
123 
//############################################################################
 130 
//############################################################################
124 
//  Nullwerte ermitteln
 131 
//  Nullwerte ermitteln
125 
void SetNeutral(void)
 132 
void SetNeutral(void)
126 
//############################################################################
 133 
//############################################################################
127 
{
 134 
{
128 
    unsigned int timer;
 - 
 
129 
        NeutralAccX = 0;
 135 
        NeutralAccX = 0;
130 
        NeutralAccY = 0;
 136 
        NeutralAccY = 0;
131 
        NeutralAccZ = 0;
 137 
        NeutralAccZ = 0;
132 
    AdNeutralNick = 0; 
 138 
    AdNeutralNick = 0; 
133 
        AdNeutralRoll = 0;     
 139 
        AdNeutralRoll = 0;     
134 
        AdNeutralGier = 0;
 140 
        AdNeutralGier = 0;
- 
 
 141 
    Parameter_AchsKopplung1 = 0;
- 
 
 142 
    Parameter_AchsGegenKopplung1 = 0;
135 
    CalibrierMittelwert();     
 143 
    CalibrierMittelwert();     
136 
    timer = SetDelay(5);    
 144 
    Delay_ms_Mess(100);
137 
        while (!CheckDelay(timer));
 - 
 
138 
        CalibrierMittelwert();
 145 
        CalibrierMittelwert();
139 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 146 
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
140 
     {    
 147 
     {    
141 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 148 
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
142 
     }
 149 
     }
143 
 
 - 
 
144 
    if(PlatinenVersion == 10)
 - 
 
145 
    {
 150 
 
146 
     AdNeutralNick= abs(MesswertNick); 
 151 
     AdNeutralNick= AdWertNick;
147 
         AdNeutralRoll= abs(MesswertRoll);     
 152 
         AdNeutralRoll= AdWertRoll;    
148 
         AdNeutralGier= abs(MesswertGier);
 - 
 
149 
    }
 - 
 
150 
    else
 - 
 
151 
    {
 - 
 
152 
     AdNeutralNick= abs(MesswertNick) / 2;     
 - 
 
153 
         AdNeutralRoll= abs(MesswertRoll) / 2; 
 - 
 
154 
         AdNeutralGier= abs(MesswertGier) / 2;
 153 
         AdNeutralGier= AdWertGier;
155 
    }
 154 
 
156 
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
 155 
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
157 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
 156 
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
158 
        NeutralAccZ = Aktuell_az;
 157 
        NeutralAccZ = Aktuell_az;
159 
   
 158 
   
160 
        Mess_IntegralNick = 0; 
 159 
        Mess_IntegralNick = 0; 
161 
    Mess_IntegralNick2 = 0;
 160 
    Mess_IntegralNick2 = 0;
162 
    Mess_IntegralRoll = 0;     
 161 
    Mess_IntegralRoll = 0;     
163 
    Mess_IntegralRoll2 = 0;
 162 
    Mess_IntegralRoll2 = 0;
164 
    Mess_Integral_Gier = 0;    
 163 
    Mess_Integral_Gier = 0;    
165 
    MesswertNick = 0;
 164 
    MesswertNick = 0;
166 
    MesswertRoll = 0;
 165 
    MesswertRoll = 0;
167 
    MesswertGier = 0;
 166 
    MesswertGier = 0;
168 
    StartLuftdruck = Luftdruck;
 167 
    StartLuftdruck = Luftdruck;
169 
    HoeheD = 0;
 168 
    HoeheD = 0;
170 
    Mess_Integral_Hoch = 0;
 169 
    Mess_Integral_Hoch = 0;
171 
    KompassStartwert = KompassValue;
 170 
    KompassStartwert = KompassValue;
172 
    GPS_Neutral();
 171 
    GPS_Neutral();
173 
    beeptime = 50;  
 172 
    beeptime = 50;  
- 
 
 173 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
- 
 
 174 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
174 
}
 175 
}
175 
 
 176 
 
176 
//############################################################################
 177 
//############################################################################
177 
// Bildet den Mittelwert aus den Messwerten
 178 
// Bearbeitet die Messwerte
178 
void Mittelwert(void)
 179 
void Mittelwert(void)
179 
//############################################################################
 180 
//############################################################################
180 
{      
 181 
{      
- 
 
 182 
    static signed long tmpl,tmpl2;     
181 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 183 
    MesswertGier = (signed int) AdNeutralGier - AdWertGier;
182 
    ANALOG_OFF;
 184 
    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;
183 
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 185 
    MesswertNick = (signed int) AdWertNick - AdNeutralNick;
- 
 
 186 
    if(PlatinenVersion != 10)
- 
 
 187 
     {
- 
 
 188 
      MesswertGier *= 2;
184 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 189 
          MesswertRoll *= 2;
- 
 
 190 
      MesswertNick *= 2;
- 
 
 191 
     }
185 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 192 
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
186 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L;
 193 
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;
187 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L;
 194 
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;
188 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L;
 195 
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;
- 
 
 196 
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
- 
 
 197 
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
- 
 
 198 
    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;
- 
 
 199 
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 200 
            Mess_Integral_Gier +=  MesswertGier;
- 
 
 201 
            Mess_Integral_Gier2 += MesswertGier;
- 
 
 202 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
- 
 
 203 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
- 
 
 204 
         {
189 
    AccumulateNick = 0;   MessanzahlNick = 0;
 205 
            tmpl = Mess_IntegralNick / 4096L;
- 
 
 206 
            tmpl *= MesswertGier;
- 
 
 207 
            tmpl *= Parameter_AchsKopplung1;  //125
- 
 
 208 
            tmpl /= 2048L;
- 
 
 209 
            tmpl2 = Mess_IntegralRoll / 4096L;
- 
 
 210 
            tmpl2 *= MesswertGier;
- 
 
 211 
            tmpl2 *= Parameter_AchsKopplung1;
- 
 
 212 
            tmpl2 /= 2048L;
- 
 
 213 
         }
- 
 
 214 
      else  tmpl = tmpl2 = 0;
- 
 
 215 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 216 
            MesswertRoll += tmpl;
- 
 
 217 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
- 
 
 218 
            Mess_IntegralRoll2 += MesswertRoll;
- 
 
 219 
            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;
- 
 
 220 
            if(Mess_IntegralRoll > Umschlag180Roll)
- 
 
 221 
            {
- 
 
 222 
             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);
- 
 
 223 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
- 
 
 224 
            }
- 
 
 225 
            if(Mess_IntegralRoll <-Umschlag180Roll)
- 
 
 226 
            {
- 
 
 227 
             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);
- 
 
 228 
             Mess_IntegralRoll2 = Mess_IntegralRoll;
- 
 
 229 
            }  
190 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 230 
            if(AdWertRoll < 15)   MesswertRoll = -1000;
191 
    AccumulateGier = 0;   MessanzahlGier = 0;
 231 
            if(AdWertRoll <  7)   MesswertRoll = -2000;
- 
 
 232 
            if(AdWertRoll > 1010) MesswertRoll = +1000;
192 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 233 
            if(AdWertRoll > 1017) MesswertRoll = +2000;
- 
 
 234 
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 235 
            MesswertNick -= tmpl2;
- 
 
 236 
            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;
- 
 
 237 
            Mess_IntegralNick2 += MesswertNick;
- 
 
 238 
            Mess_IntegralNick  += MesswertNick - I_LageNick;
- 
 
 239 
            if(Mess_IntegralNick > Umschlag180Nick)
- 
 
 240 
            {
- 
 
 241 
             Mess_IntegralNick = -(Umschlag180Nick - 10000L);
- 
 
 242 
             Mess_IntegralNick2 = Mess_IntegralNick;
- 
 
 243 
            }
- 
 
 244 
            if(Mess_IntegralNick <-Umschlag180Nick)
- 
 
 245 
            {
- 
 
 246 
             Mess_IntegralNick =  (Umschlag180Nick - 10000L);
- 
 
 247 
             Mess_IntegralNick2 = Mess_IntegralNick;
- 
 
 248 
            }
- 
 
 249 
            if(AdWertNick < 15)   MesswertNick = -1000;
- 
 
 250 
            if(AdWertNick <  7)   MesswertNick = -2000;
193 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 251 
            if(AdWertNick > 1010) MesswertNick = +1000;
194 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 252 
            if(AdWertNick > 1017) MesswertNick = +2000;
- 
 
 253 
//++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 254 
// ADC einschalten
- 
 
 255 
    ANALOG_ON; 
- 
 
 256 
//++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 257 
 
195 
    Integral_Gier  = Mess_Integral_Gier;
 258 
    Integral_Gier  = Mess_Integral_Gier;
196 
//    Integral_Gier2 = Mess_Integral_Gier2;
 - 
 
197 
    IntegralNick = Mess_IntegralNick;
 259 
    IntegralNick = Mess_IntegralNick;
198 
    IntegralRoll = Mess_IntegralRoll;
 260 
    IntegralRoll = Mess_IntegralRoll;
199 
    IntegralNick2 = Mess_IntegralNick2;
 261 
    IntegralNick2 = Mess_IntegralNick2;
200 
    IntegralRoll2 = Mess_IntegralRoll2;
 262 
    IntegralRoll2 = Mess_IntegralRoll2;
201 
    // ADC einschalten
 - 
 
202 
    ANALOG_ON; 
 - 
 
- 
 
 263 
 
203 
 
 264 
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
204 
/*
 - 
 
205 
//------------------------------------------------------------------------------
 265 
  {
206 
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
 - 
 
207 
    else                                         
 266 
    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);
208 
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
 267 
    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
209 
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
 - 
 
210 
    else                                         
 268 
    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);
211 
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
 - 
 
212 
//------------------------------------------------------------------------------
 269 
    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
213 
*/
 270 
  }
214 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 271 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
215 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 272 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
216 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 273 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
217 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 274 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
218 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 275 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
219 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 276 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
220 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 277 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
221 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 278 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
222 
}
 279 
}
223 
 
 280 
 
224 
//############################################################################
 281 
//############################################################################
225 
// Messwerte beim Ermitteln der Nullage
 282 
// Messwerte beim Ermitteln der Nullage
226 
void CalibrierMittelwert(void)
 283 
void CalibrierMittelwert(void)
227 
//############################################################################
 284 
//############################################################################
228 
{                
 285 
{                
229 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
 286 
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
230 
        ANALOG_OFF;
 287 
        ANALOG_OFF;
231 
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
 - 
 
232 
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
 - 
 
233 
        if(MessanzahlGier)    (MesswertGier = AccumulateGier / MessanzahlGier);
 - 
 
234 
        if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick);
 - 
 
235 
        if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll;
 - 
 
236 
        if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch;
 - 
 
237 
    AccumulateNick = 0;   MessanzahlNick = 0;
 288 
        MesswertNick = AdWertNick;
238 
    AccumulateRoll = 0;   MessanzahlRoll = 0;
 289 
        MesswertRoll = AdWertRoll;
239 
    AccumulateGier = 0;   MessanzahlGier = 0;
 290 
        MesswertGier = AdWertGier;
240 
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
 291 
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
241 
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
 292 
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
242 
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
 293 
        Mittelwert_AccHoch = (long)AdWertAccHoch;
243 
    // ADC einschalten
 294 
   // ADC einschalten
244 
    ANALOG_ON; 
 295 
    ANALOG_ON; 
245 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
 296 
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
246 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
 297 
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
247 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
 298 
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
248 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
 299 
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
249 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
 300 
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
250 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 301 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
251 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 302 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
252 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 303 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
- 
 
 304 
 
- 
 
 305 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
- 
 
 306 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
253 
}
 307 
}
254 
 
 308 
 
255 
//############################################################################
 309 
//############################################################################
256 
// Senden der Motorwerte per I2C-Bus
 310 
// Senden der Motorwerte per I2C-Bus
257 
void SendMotorData(void)
 311 
void SendMotorData(void)
258 
//############################################################################
 312 
//############################################################################
259 
{
 313 
{
260 
    if(MOTOR_OFF || !MotorenEin)
 314 
    if(MOTOR_OFF || !MotorenEin)
261 
        {
 315 
        {
262 
        Motor_Hinten = 0;
 316 
        Motor_Hinten = 0;
263 
        Motor_Vorne = 0;
 317 
        Motor_Vorne = 0;
264 
        Motor_Rechts = 0;
 318 
        Motor_Rechts = 0;
265 
        Motor_Links = 0;
 319 
        Motor_Links = 0;
266 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
 320 
        if(MotorTest[0]) Motor_Vorne = MotorTest[0];
267 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
 321 
        if(MotorTest[1]) Motor_Hinten = MotorTest[1];
268 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
 322 
        if(MotorTest[2]) Motor_Links = MotorTest[2];
269 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
 323 
        if(MotorTest[3]) Motor_Rechts = MotorTest[3];
270 
        }
 324 
        }
271 
 
 325 
 
272 
    DebugOut.Analog[12] = Motor_Vorne;
 326 
    DebugOut.Analog[12] = Motor_Vorne;
273 
    DebugOut.Analog[13] = Motor_Hinten;
 327 
    DebugOut.Analog[13] = Motor_Hinten;
274 
    DebugOut.Analog[14] = Motor_Links;
 328 
    DebugOut.Analog[14] = Motor_Links;
275 
    DebugOut.Analog[15] = Motor_Rechts;  
 329 
    DebugOut.Analog[15] = Motor_Rechts;  
276 
 
 330 
 
277 
    //Start I2C Interrupt Mode
 331 
    //Start I2C Interrupt Mode
278 
    twi_state = 0;
 332 
    twi_state = 0;
279 
    motor = 0;
 333 
    motor = 0;
280 
    i2c_start();
 334 
    i2c_start();
281 
}
 335 
}
282 
 
 - 
 
283 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
284 
// + Konstanten
- 
 
285 
// + 0-250 -> normale Werte
 - 
 
286 
// + 251 -> Poti1
 - 
 
287 
// + 252 -> Poti2
 - 
 
288 
// + 253 -> Poti3
 - 
 
289 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 - 
 
290 
void DefaultKonstanten1(void)
 - 
 
291 
{
 - 
 
292 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 - 
 
293 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 - 
 
294 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 - 
 
295 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 - 
 
296 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 - 
 
297 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 - 
 
298 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 - 
 
299 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
 - 
 
300 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
 - 
 
301 
 EE_Parameter.Hoehe_MinGas = 30;
 - 
 
302 
 EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
 - 
 
303 
 EE_Parameter.Hoehe_P      = 10;       // Wert : 0-32
 - 
 
304 
 EE_Parameter.Luftdruck_D  = 50;       // Wert : 0-250
 - 
 
305 
 EE_Parameter.Hoehe_ACC_Wirkung = 50;  // Wert : 0-250
 - 
 
306 
 EE_Parameter.Hoehe_Verstaerkung = 4;  // Wert : 0-50
 - 
 
307 
 EE_Parameter.Stick_P = 4; //2         // Wert : 1-6
 - 
 
308 
 EE_Parameter.Stick_D = 8; //8         // Wert : 0-64
 - 
 
309 
 EE_Parameter.Gier_P = 16;             // Wert : 1-20
 - 
 
310 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 - 
 
311 
 EE_Parameter.Gas_Max = 250;           // Wert : 33-250
 - 
 
312 
 EE_Parameter.GyroAccFaktor = 26;      // Wert : 1-64
 - 
 
313 
 EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
 - 
 
314 
 EE_Parameter.Gyro_P = 120; //80          // Wert : 0-250
 - 
 
315 
 EE_Parameter.Gyro_I = 150;               // Wert : 0-250
 - 
 
316 
 EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250
 - 
 
317 
 EE_Parameter.NotGas = 35;                // Wert : 0-250     // Gaswert bei Empangsverlust
 - 
 
318 
 EE_Parameter.NotGasZeit = 20;            // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 - 
 
319 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 - 
 
320 
 EE_Parameter.I_Faktor = 0;
 - 
 
321 
 EE_Parameter.UserParam1 = 0;             //zur freien Verwendung
 - 
 
322 
 EE_Parameter.UserParam2 = 0;             //zur freien Verwendung
 - 
 
323 
 EE_Parameter.UserParam3 = 0;             //zur freien Verwendung
 - 
 
324 
 EE_Parameter.UserParam4 = 0;             //zur freien Verwendung
 - 
 
325 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 - 
 
326 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 - 
 
327 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 - 
 
328 
 EE_Parameter.ServoNickMin = 50;          // Wert : 0-250     // Anschlag
 - 
 
329 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 - 
 
330 
 EE_Parameter.ServoNickRefresh = 5;
 - 
 
331 
 EE_Parameter.LoopGasLimit = 50;
 - 
 
332 
 EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
 - 
 
333 
 EE_Parameter.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
 - 
 
334 
 memcpy(EE_Parameter.Name, "Sport\0", 12);  
 - 
 
335 
}
 - 
 
336 
 
 - 
 
337 
void DefaultKonstanten2(void)
 - 
 
338 
{
 - 
 
339 
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
 - 
 
340 
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
 - 
 
341 
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
 - 
 
342 
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
 - 
 
343 
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
 - 
 
344 
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
 - 
 
345 
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
 - 
 
346 
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
 - 
 
347 
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
 - 
 
348 
 EE_Parameter.Hoehe_MinGas = 30;
 - 
 
349 
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
 - 
 
350 
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
 - 
 
351 
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
 - 
 
352 
 EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250
 - 
 
353 
 EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50
 - 
 
354 
 EE_Parameter.Stick_P = 4; //2           // Wert : 1-6
 - 
 
355 
 EE_Parameter.Stick_D = 0; //8           // Wert : 0-64
 - 
 
356 
 EE_Parameter.Gier_P = 16;            // Wert : 1-20
 - 
 
357 
 EE_Parameter.Gas_Min = 15;            // Wert : 0-32
 - 
 
358 
 EE_Parameter.Gas_Max = 250;          // Wert : 33-250
 - 
 
359 
 EE_Parameter.GyroAccFaktor = 26;     // Wert : 1-64
 - 
 
360 
 EE_Parameter.KompassWirkung = 128;    // Wert : 0-250
 - 
 
361 
 EE_Parameter.Gyro_P = 175; //80           // Wert : 0-250
 - 
 
362 
 EE_Parameter.Gyro_I = 175;           // Wert : 0-250
 - 
 
363 
 EE_Parameter.UnterspannungsWarnung = 94;  // Wert : 0-250
 - 
 
364 
 EE_Parameter.NotGas = 35;                 // Wert : 0-250     // Gaswert bei Empangsverlust
 - 
 
365 
 EE_Parameter.NotGasZeit = 20;             // Wert : 0-250     // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen
 - 
 
366 
 EE_Parameter.UfoAusrichtung = 0;         // X oder + Formation
 - 
 
367 
 EE_Parameter.I_Faktor = 0;
 - 
 
368 
 EE_Parameter.UserParam1 = 0;   //zur freien Verwendung
 - 
 
369 
 EE_Parameter.UserParam2 = 0;   //zur freien Verwendung
 - 
 
370 
 EE_Parameter.UserParam3 = 0;   //zur freien Verwendung
 - 
 
371 
 EE_Parameter.UserParam4 = 0;   //zur freien Verwendung
 - 
 
372 
 EE_Parameter.ServoNickControl = 100;     // Wert : 0-250     // Stellung des Servos
 - 
 
373 
 EE_Parameter.ServoNickComp = 40;         // Wert : 0-250     // Einfluss Gyro/Servo
 - 
 
374 
 EE_Parameter.ServoNickCompInvert = 0;    // Wert : 0-250     // Richtung Einfluss Gyro/Servo
 - 
 
375 
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
 - 
 
376 
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
 - 
 
377 
 EE_Parameter.ServoNickRefresh = 5;
 - 
 
378 
 EE_Parameter.LoopGasLimit = 50;
 - 
 
379 
 EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
 - 
 
380 
 EE_Parameter.LoopConfig = 0;              // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
- 
 
381 
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
 - 
 
382 
}
 336 
 
383 
 
 337 
 
384 
 
 338 
 
385 
//############################################################################
 339 
//############################################################################
386 
// Trägt ggf. das Poti als Parameter ein
 340 
// Trägt ggf. das Poti als Parameter ein
387 
void ParameterZuordnung(void)
 341 
void ParameterZuordnung(void)
388 
//############################################################################
 342 
//############################################################################
389 
{
 343 
{
390 
 
 344 
 
391 
 #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;}
 345 
 #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;}
392 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
 346 
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
393 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
 347 
 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
394 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
 348 
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
395 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
 349 
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
396 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
 350 
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
397 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
 351 
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
398 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
 352 
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
399 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
 353 
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
400 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
 354 
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
401 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
 355 
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
402 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
 356 
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
403 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
 357 
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
404 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 358 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
405 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 - 
 
406 
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
 - 
 
407 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
 359 
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
- 
 
 360 
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
- 
 
 361 
 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
408 
 
 362 
 
409 
 Ki = (float) Parameter_I_Faktor * 0.0001;
 363 
 Ki = (float) Parameter_I_Faktor * 0.0001;
410 
 MAX_GAS = EE_Parameter.Gas_Max;
 364 
 MAX_GAS = EE_Parameter.Gas_Max;
411 
 MIN_GAS = EE_Parameter.Gas_Min;
 365 
 MIN_GAS = EE_Parameter.Gas_Min;
412 
}
 366 
}
413 
 
 367 
 
414 
 
 368 
 
415 
//############################################################################
 369 
//############################################################################
416 
//
 370 
//
417 
void MotorRegler(void)
 371 
void MotorRegler(void)
418 
//############################################################################
 372 
//############################################################################
419 
{
 373 
{
420 
         int motorwert,pd_ergebnis,h,tmp_int;
 374 
         int motorwert,pd_ergebnis,h,tmp_int;
421 
         int GierMischanteil,GasMischanteil;
 375 
         int GierMischanteil,GasMischanteil;
422 
     static long SummeNick=0,SummeRoll=0;
 376 
     static long SummeNick=0,SummeRoll=0;
423 
     static long sollGier = 0,tmp_long,tmp_long2;
 377 
     static long sollGier = 0,tmp_long,tmp_long2;
424 
     long IntegralFehlerNick = 0;
 378 
     static long IntegralFehlerNick = 0;
425 
     long IntegralFehlerRoll = 0;
 379 
     static long IntegralFehlerRoll = 0;
426 
         static unsigned int RcLostTimer;
 380 
         static unsigned int RcLostTimer;
427 
         static unsigned char delay_neutral = 0;
 381 
         static unsigned char delay_neutral = 0;
428 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
 382 
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
429 
         static unsigned int  modell_fliegt = 0;
 383 
         static unsigned int  modell_fliegt = 0;
430 
     static int hoehenregler = 0;
 384 
     static int hoehenregler = 0;
431 
     static char TimerWerteausgabe = 0;
 385 
     static char TimerWerteausgabe = 0;
432 
     static char NeueKompassRichtungMerken = 0;
 386 
     static char NeueKompassRichtungMerken = 0;
- 
 
 387 
     static long ausgleichNick, ausgleichRoll;
- 
 
 388 
     
433 
        Mittelwert();
 389 
        Mittelwert();
434 
 
 390 
 
435 
    GRN_ON;
 391 
    GRN_ON;
436 
 
 - 
 
437 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
392 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
438 
// Gaswert ermitteln
 393 
// Gaswert ermitteln
439 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
394 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
440 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
 395 
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
441 
    if(GasMischanteil < 0) GasMischanteil = 0;
 396 
    if(GasMischanteil < 0) GasMischanteil = 0;
442 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
397 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
443 
// Emfang schlecht
 398 
// Emfang schlecht
444 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
399 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
445 
   if(SenderOkay < 100)
 400 
   if(SenderOkay < 100)
446 
        {
 401 
        {
447 
        if(!PcZugriff)
 402 
        if(!PcZugriff)
448 
         {
 403 
         {
449 
           if(BeepMuster == 0xffff)
 404 
           if(BeepMuster == 0xffff)
450 
            {
 405 
            {
451 
             beeptime = 15000;
 406 
             beeptime = 15000;
452 
             BeepMuster = 0x0c00;
 407 
             BeepMuster = 0x0c00;
453 
            }
 408 
            }
454 
         }
 409 
         }
455 
        if(RcLostTimer) RcLostTimer--;
 410 
        if(RcLostTimer) RcLostTimer--;
456 
        else
 411 
        else
457 
         {
 412 
         {
458 
          MotorenEin = 0;
 413 
          MotorenEin = 0;
459 
          Notlandung = 0;
 414 
          Notlandung = 0;
460 
         }
 415 
         }
461 
        ROT_ON;
 416 
        ROT_ON;
462 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
 417 
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
463 
            {
 418 
            {
464 
            GasMischanteil = EE_Parameter.NotGas;
 419 
            GasMischanteil = EE_Parameter.NotGas;
465 
            Notlandung = 1;
 420 
            Notlandung = 1;
466 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
 421 
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
467 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
 422 
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
468 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
 423 
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
469 
            }
 424 
            }
470 
         else MotorenEin = 0;
 425 
         else MotorenEin = 0;
471 
        }
 426 
        }
472 
        else
 427 
        else
473 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
428 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
474 
// Emfang gut
 429 
// Emfang gut
475 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
430 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
476 
        if(SenderOkay > 140)
 431 
        if(SenderOkay > 140)
477 
            {
 432 
            {
478 
            Notlandung = 0;
 433 
            Notlandung = 0;
479 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
 434 
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
480 
            if(GasMischanteil > 40)
 435 
            if(GasMischanteil > 40)
481 
                {
 436 
                {
482 
                if(modell_fliegt < 0xffff) modell_fliegt++;
 437 
                if(modell_fliegt < 0xffff) modell_fliegt++;
483 
                }
 438 
                }
484 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
 439 
            if((modell_fliegt < 200) || (GasMischanteil < 40))
485 
                {
 440 
                {
486 
                SummeNick = 0;
 441 
                SummeNick = 0;
487 
                SummeRoll = 0;
 442 
                SummeRoll = 0;
488 
                Mess_Integral_Gier = 0;
 443 
                Mess_Integral_Gier = 0;
489 
                Mess_Integral_Gier2 = 0;
 444 
                Mess_Integral_Gier2 = 0;
490 
                }
 445 
                }
491 
            if((GasMischanteil > 200) && MotorenEin == 0)
 446 
            if((GasMischanteil > 200) && MotorenEin == 0)
492 
                {
 447 
                {
493 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
448 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
494 
// auf Nullwerte kalibrieren
 449 
// auf Nullwerte kalibrieren
495 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
450 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
496 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
 451 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
497 
                    {
 452 
                    {
- 
 
 453 
                    unsigned char setting = 2;
498 
                    if(++delay_neutral > 200)  // nicht sofort
 454 
                    if(++delay_neutral > 200)  // nicht sofort
499 
                        {
 455 
                        {
500 
                        GRN_OFF;
 456 
                        GRN_OFF;
501 
                        MotorenEin = 0;
 457 
                        MotorenEin = 0;
502 
                        delay_neutral = 0;
 458 
                        delay_neutral = 0;
503 
                        modell_fliegt = 0;
 459 
                        modell_fliegt = 0;
504 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
 460 
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
505 
                        {
 461 
                        {
506 
                         unsigned char setting=1;
 462 
                         unsigned char setting=1;
507 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
 463 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
508 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
 464 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
509 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
 465 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
510 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
 466 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
511 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
 467 
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
512 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
 468 
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
513 
                        }
 469 
                        }
514 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
 470 
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
515 
                          {
 471 
                          {
516 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
 472 
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
517 
                          }  
 473 
                          }  
518 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
 474 
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
519 
                        SetNeutral();
 475 
                        SetNeutral();
520 
                        Piep(GetActiveParamSetNumber());
 476 
                        Piep(GetActiveParamSetNumber());
521 
                        }
 477 
                        }
522 
                    }
 478 
                    }
523 
                 else delay_neutral = 0;
 479 
                 else delay_neutral = 0;
524 
                }
 480 
                }
525 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
481 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
526 
// Gas ist unten
 482 
// Gas ist unten
527 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
483 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
528 
            if(GasMischanteil < 35)
 484 
            if(GasMischanteil < 35)
529 
                {
 485 
                {
530 
                // Starten
 486 
                // Starten
531 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
 487 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
532 
                    {
 488 
                    {
533 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
489 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
534 
// Einschalten
 490 
// Einschalten
535 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
491 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
536 
                    if(++delay_einschalten > 200)
 492 
                    if(++delay_einschalten > 200)
537 
                        {
 493 
                        {
538 
                        delay_einschalten = 200;
 494 
                        delay_einschalten = 200;
539 
                        modell_fliegt = 1;
 495 
                        modell_fliegt = 1;
540 
                        MotorenEin = 1;
 496 
                        MotorenEin = 1;
541 
                        sollGier = 0;
 497 
                        sollGier = 0;
542 
                        Mess_Integral_Gier = 0;
 498 
                        Mess_Integral_Gier = 0;
543 
                        Mess_Integral_Gier2 = 0;
 499 
                        Mess_Integral_Gier2 = 0;
544 
                        Mess_IntegralNick = 0;
 500 
                        Mess_IntegralNick = 0;
545 
                        Mess_IntegralRoll = 0;
 501 
                        Mess_IntegralRoll = 0;
546 
                        Mess_IntegralNick2 = IntegralNick;
 502 
                        Mess_IntegralNick2 = IntegralNick;
547 
                        Mess_IntegralRoll2 = IntegralRoll;
 503 
                        Mess_IntegralRoll2 = IntegralRoll;
548 
                        SummeNick = 0;
 504 
                        SummeNick = 0;
549 
                        SummeRoll = 0;
 505 
                        SummeRoll = 0;
550 
                        }          
 506 
                        }          
551 
                    }  
 507 
                    }  
552 
                    else delay_einschalten = 0;
 508 
                    else delay_einschalten = 0;
553 
                //Auf Neutralwerte setzen
 509 
                //Auf Neutralwerte setzen
554 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
510 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
555 
// Auschalten
 511 
// Auschalten
556 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
512 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
557 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
 513 
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
558 
                    {
 514 
                    {
559 
                    if(++delay_ausschalten > 200)  // nicht sofort
 515 
                    if(++delay_ausschalten > 200)  // nicht sofort
560 
                        {
 516 
                        {
561 
                        MotorenEin = 0;
 517 
                        MotorenEin = 0;
562 
                        delay_ausschalten = 200;
 518 
                        delay_ausschalten = 200;
563 
                        modell_fliegt = 0;
 519 
                        modell_fliegt = 0;
564 
                        }
 520 
                        }
565 
                    }
 521 
                    }
566 
                else delay_ausschalten = 0;
 522 
                else delay_ausschalten = 0;
567 
                }
 523 
                }
568 
            }
 524 
            }
569 
 
 525 
 
570 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
526 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
571 
// neue Werte von der Funke
 527 
// neue Werte von der Funke
572 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
528 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
573 
 if(!NewPpmData-- || Notlandung)  
 529 
 if(!NewPpmData-- || Notlandung)  
574 
  {
 530 
  {
575 
    ParameterZuordnung();
 531 
    ParameterZuordnung();
576 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
 532 
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
577 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
 533 
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
578 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
 534 
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
579 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
 535 
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
580 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
 536 
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
581 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
 537 
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
582 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
 538 
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
583 
 
 539 
 
584 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
 540 
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
585 
    if(GyroFaktor < 0) GyroFaktor = 0;
 541 
    if(GyroFaktor < 0) GyroFaktor = 0;
586 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
 542 
    if(IntegralFaktor < 0) IntegralFaktor = 0;
587 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
 543 
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
588 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 544 
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
589 
     {
 545 
     {
590 
      if(IntegralNick >  60000)  
 546 
      if(IntegralNick >  60000)  
591 
      {
 547 
      {
592 
       StickNick -=  8 * EE_Parameter.Stick_P;      
 548 
       StickNick -=  8 * EE_Parameter.Stick_P;      
593 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 549 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
594 
      }
 550 
      }
595 
      else
 551 
      else
596 
      if(IntegralNick < -60000)  
 552 
      if(IntegralNick < -60000)  
597 
      {
 553 
      {
598 
       StickNick += 8 * EE_Parameter.Stick_P;
 554 
       StickNick += 8 * EE_Parameter.Stick_P;
599 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
 555 
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
600 
      }
 556 
      }
601 
      if(IntegralRoll >  60000)  
 557 
      if(IntegralRoll >  60000)  
602 
      {
 558 
      {
603 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
 559 
       StickRoll -=  8 * EE_Parameter.Stick_P;      
604 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 560 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
605 
      }
 561 
      }
606 
      else
 562 
      else
607 
      if(IntegralRoll < -60000)  
 563 
      if(IntegralRoll < -60000)  
608 
      {
 564 
      {
609 
       StickRoll += 8 * EE_Parameter.Stick_P;
 565 
       StickRoll += 8 * EE_Parameter.Stick_P;
610 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
 566 
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
611 
      }
 567 
      }
612 
     }
 568 
     }
613 
 
 - 
 
614 
  }
 - 
 
615 
 
 - 
 
616 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
569 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
617 
// Looping?
 570 
// Looping?
618 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
571 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
619 
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
 572 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
620 
     ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
 - 
 
- 
 
 573 
  else
621 
   {
 574 
   {
622 
    Looping_Roll = 1;
 575 
     {
623 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 576 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  
- 
 
 577 
     }  
624 
   }
 578 
   }
- 
 
 579 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
- 
 
 580 
   else
- 
 
 581 
   {
625 
   else Looping_Roll = 0;  
 582 
   if(Looping_Rechts) // Hysterese
- 
 
 583 
     {
- 
 
 584 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
- 
 
 585 
     }
- 
 
 586 
   }
626 
 
 587 
 
627 
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
 - 
 
- 
 
 588 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
628 
     ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
 589 
  else
629 
   {
 590 
   {
- 
 
 591 
    if(Looping_Oben)  // Hysterese
- 
 
 592 
     {
- 
 
 593 
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  
- 
 
 594 
     }  
- 
 
 595 
   }
- 
 
 596 
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
- 
 
 597 
   else
- 
 
 598 
   {
- 
 
 599 
    if(Looping_Unten) // Hysterese
- 
 
 600 
     {
- 
 
 601 
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
- 
 
 602 
     }
- 
 
 603 
   }
- 
 
 604 
 
- 
 
 605 
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
- 
 
 606 
   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
- 
 
 607 
  } // Ende neue Funken-Werte
630 
    Looping_Nick = 1;
 608 
 
- 
 
 609 
  if(Looping_Roll) beeptime = 100;
- 
 
 610 
  if(Looping_Roll || Looping_Nick)
631 
    Looping_Roll = 0;
 611 
   {
632 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
 612 
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
633 
   }
 - 
 
634 
   else Looping_Nick = 0;  
 613 
   }
635 
   
 614 
   
636 
 
 615 
 
637 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
616 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
638 
// Bei Empfangsausfall im Flug
617 
// Bei Empfangsausfall im Flug
639 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
618 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
640 
   if(Notlandung)
 619 
   if(Notlandung)
641 
    {
 620 
    {
642 
     StickGier = 0;
 621 
     StickGier = 0;
643 
     StickNick = 0;
 622 
     StickNick = 0;
644 
     StickRoll = 0;
 623 
     StickRoll = 0;
645 
     GyroFaktor  = 0.1;
 624 
     GyroFaktor  = 0.1;
646 
     IntegralFaktor = 0.005;
 625 
     IntegralFaktor = 0.005;
647 
     Looping_Roll = 0;
 626 
     Looping_Roll = 0;
648 
     Looping_Nick = 0;
 627 
     Looping_Nick = 0;
649 
    }  
 628 
    }  
650 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
651 
// Gyro-Drift kompensieren
 - 
 
652 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
653 
#define DRIFT_FAKTOR 3
 - 
 
654 
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
 - 
 
655 
        {
 629 
 
656 
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
 - 
 
657 
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
 - 
 
658 
        ZaehlMessungen = 0;
 - 
 
659 
        if(IntegralFehlerNick > 800/DRIFT_FAKTOR)   AdNeutralNick++;
 - 
 
660 
        if(IntegralFehlerNick < -800/DRIFT_FAKTOR)  AdNeutralNick--;
 - 
 
661 
        if(IntegralFehlerRoll > 800/DRIFT_FAKTOR)   AdNeutralRoll++;
 - 
 
662 
        if(IntegralFehlerRoll < -800/DRIFT_FAKTOR)  AdNeutralRoll--;
 - 
 
663 
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
 - 
 
664 
//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)      
 - 
 
665 
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 - 
 
666 
        Mess_IntegralNick2 = IntegralNick;
 - 
 
667 
        Mess_IntegralRoll2 = IntegralRoll;
 - 
 
668 
        Mess_Integral_Gier2 = Integral_Gier;
 - 
 
669 
    ANALOG_ON;  // ADC einschalten
 - 
 
670 
        }
 630 
 
671 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
631 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
672 
// Integrale auf ACC-Signal abgleichen
 632 
// Integrale auf ACC-Signal abgleichen
673 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
633 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
 634 
#define ABGLEICH_ANZAHL 256L
- 
 
 635 
 
- 
 
 636 
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
- 
 
 637 
 MittelIntegralRoll  += IntegralRoll;
- 
 
 638 
 MittelIntegralNick2 += IntegralNick2;
- 
 
 639 
 MittelIntegralRoll2 += IntegralRoll2;
- 
 
 640 
 
674 
  if(!Looping_Nick && !Looping_Roll)
 641 
 if(Looping_Nick || Looping_Roll)
675 
  {
 642 
  {
676 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
 643 
    IntegralAccNick = 0;
677 
    tmp_long /= 32;
 644 
    IntegralAccRoll = 0;
678 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
 645 
    MittelIntegralNick = 0;
679 
    tmp_long2 /= 32;
 646 
    MittelIntegralRoll = 0;
680 
 
 - 
 
681 
 #define AUSGLEICH 100
 647 
    MittelIntegralNick2 = 0;
682 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 648 
    MittelIntegralRoll2 = 0;
683 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 649 
    Mess_IntegralNick2 = Mess_IntegralNick;
684 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 650 
    Mess_IntegralRoll2 = Mess_IntegralRoll;
685 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 651 
    ZaehlMessungen = 0;
- 
 
 652 
    I_LageNick = 0;
- 
 
 653 
    I_LageRoll = 0;
686 
  }
 654 
  }
- 
 
 655 
 
- 
 
 656 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
687 
  else
 657 
 {
- 
 
 658 
  static int cnt = 0;
- 
 
 659 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
- 
 
 660 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
- 
 
 661 
  if(!Looping_Nick && !Looping_Roll)
688 
  {
 662 
  {
- 
 
 663 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
- 
 
 664 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
- 
 
 665 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
- 
 
 666 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
- 
 
 667 
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
- 
 
 668 
 
- 
 
 669 
//DebugOut.Analog[6] = labs(MittelIntegralNick_Alt - MittelIntegralNick) / 10;
- 
 
 670 
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 671 
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
- 
 
 672 
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
- 
 
 673 
    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
- 
 
 674 
     {
- 
 
 675 
       tmp_long = IntegralFehlerNick / 700L;
- 
 
 676 
       if(tmp_long > 40) tmp_long = 40;
- 
 
 677 
       if(tmp_long <-40) tmp_long =-40;
- 
 
 678 
       I_LageNick = tmp_long;    
- 
 
 679 
     }
- 
 
 680 
     else
- 
 
 681 
     {
- 
 
 682 
      I_LageNick /= 2;
- 
 
 683 
     }
- 
 
 684 
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++    
- 
 
 685 
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
- 
 
 686 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
- 
 
 687 
    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
- 
 
 688 
     {
- 
 
 689 
      tmp_long2 = IntegralFehlerRoll / 700L;
- 
 
 690 
      if(tmp_long2 > 40) tmp_long2 = 40;
- 
 
 691 
      if(tmp_long2 <-40) tmp_long2 =-40;
- 
 
 692 
      I_LageRoll = tmp_long2;    
- 
 
 693 
     }
- 
 
 694 
     else
- 
 
 695 
     {
- 
 
 696 
      I_LageRoll /=2;
- 
 
 697 
     }
- 
 
 698 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
- 
 
 699 
   MittelIntegralNick_Alt = MittelIntegralNick;      
- 
 
 700 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
- 
 
 701 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
- 
 
 702 
    Mess_IntegralNick -= ausgleichNick;
- 
 
 703 
    Mess_IntegralRoll -= ausgleichRoll;
- 
 
 704 
 
- 
 
 705 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
 706 
// Gyro-Drift ermitteln
- 
 
 707 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
 708 
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
- 
 
 709 
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
- 
 
 710 
    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
- 
 
 711 
    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
- 
 
 712 
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
- 
 
 713 
 
- 
 
 714 
    IntegralFehlerNick = tmp_long;
- 
 
 715 
    IntegralFehlerRoll = tmp_long2;
- 
 
 716 
    Mess_IntegralNick2 -= IntegralFehlerNick;
- 
 
 717 
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
- 
 
 718 
 
- 
 
 719 
    if(PlatinenVersion != 10)
- 
 
 720 
    {
689 
   tmp_long = 0;
 721 
     tmp_long /= 2;
690 
   tmp_long2 = 0;
 722 
     tmp_long2 /= 2;
- 
 
 723 
    }
- 
 
 724 
 
- 
 
 725 
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
- 
 
 726 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
- 
 
 727 
 
- 
 
 728 
 
- 
 
 729 
DebugOut.Analog[30] = I_LageRoll;
- 
 
 730 
DebugOut.Analog[17] = IntegralAccNick / 26;
- 
 
 731 
DebugOut.Analog[18] = IntegralAccRoll / 26;
- 
 
 732 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
- 
 
 733 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
- 
 
 734 
DebugOut.Analog[21] = MittelIntegralNick / 26;
- 
 
 735 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
- 
 
 736 
DebugOut.Analog[28] = ausgleichNick;
- 
 
 737 
DebugOut.Analog[29] = ausgleichRoll;
- 
 
 738 
 
- 
 
 739 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
- 
 
 740 
#define FEHLER_LIMIT2  4096
- 
 
 741 
 
- 
 
 742 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 743 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
- 
 
 744 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
- 
 
 745 
         {
- 
 
 746 
           if(last_n_p)
- 
 
 747 
           {
- 
 
 748 
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
- 
 
 749 
            ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
- 
 
 750 
            if(ausgleichNick > 10000) ausgleichNick = 10000;
- 
 
 751 
            Mess_IntegralNick += ausgleichNick;
- 
 
 752 
           }
- 
 
 753 
           else last_n_p = 1;
- 
 
 754 
         } else  last_n_p = 0;
- 
 
 755 
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
- 
 
 756 
         {
- 
 
 757 
           if(last_n_n)
- 
 
 758 
            {
- 
 
 759 
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
- 
 
 760 
             ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
- 
 
 761 
             if(ausgleichNick < -10000) ausgleichNick = -10000;
- 
 
 762 
             Mess_IntegralNick += ausgleichNick;
- 
 
 763 
            }
- 
 
 764 
           else last_n_n = 1;
- 
 
 765 
         } else  last_n_n = 0;
- 
 
 766 
 
- 
 
 767 
        if(cnt > 2) cnt = 2;
- 
 
 768 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
- 
 
 769 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
- 
 
 770 
//        if(IntegralFehlerNick >  FEHLER_LIMIT)  { AdNeutralNick += (cnt + last_n_p/20);  if(last_n_p++ == 4) {last_n_p = 4;}} else last_n_p = 0;
- 
 
 771 
//        if(IntegralFehlerNick < -FEHLER_LIMIT)  { AdNeutralNick -= (cnt + last_n_n/20);  if(last_n_n++ == 4) {last_n_n = 4;}} else last_n_n = 0;
- 
 
 772 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
- 
 
 773 
        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
- 
 
 774 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
- 
 
 775 
         {
- 
 
 776 
           if(last_r_p)
- 
 
 777 
           {
- 
 
 778 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
- 
 
 779 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
- 
 
 780 
            if(ausgleichRoll > 10000) ausgleichRoll = 10000;
- 
 
 781 
            Mess_IntegralRoll += ausgleichRoll;
- 
 
 782 
           }
- 
 
 783 
           else last_r_p = 1;
- 
 
 784 
         } else  last_r_p = 0;
- 
 
 785 
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
- 
 
 786 
         {
- 
 
 787 
           if(last_r_n)
- 
 
 788 
           {
- 
 
 789 
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
- 
 
 790 
            ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
- 
 
 791 
            if(ausgleichRoll < -10000) ausgleichRoll = -10000;
- 
 
 792 
            Mess_IntegralRoll += ausgleichRoll;
- 
 
 793 
           }
- 
 
 794 
           else last_r_n = 1;
- 
 
 795 
         } else  last_r_n = 0;
- 
 
 796 
 
- 
 
 797 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
- 
 
 798 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
- 
 
 799 
        if(cnt > 2) cnt = 2;
- 
 
 800 
//        if(IntegralFehlerRoll >  FEHLER_LIMIT)  { AdNeutralRoll += (cnt + last_r_p/20);  if(last_r_p++ == 4) {beeptime =500;last_r_p = 4;}} else last_r_p = 0;
- 
 
 801 
//        if(IntegralFehlerRoll < -FEHLER_LIMIT)  { AdNeutralRoll -= (cnt + last_r_n/20);  if(last_r_n++ == 4) {beeptime = 60;last_r_n = 4;}} else last_r_n = 0;
- 
 
 802 
 
- 
 
 803 
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
- 
 
 804 
//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)      
- 
 
 805 
        cnt = 2;
- 
 
 806 
//DebugOut.Analog[24] = 10*(AdNeutralRoll - 550);
691 
  }
 807 
  }
- 
 
 808 
    IntegralAccNick = 0;
- 
 
 809 
    IntegralAccRoll = 0;
- 
 
 810 
    IntegralAccZ = 0;
692 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 811 
    MittelIntegralNick = 0;
- 
 
 812 
    MittelIntegralRoll = 0;
693 
    Mess_IntegralNick -= tmp_long;
 813 
    MittelIntegralNick2 = 0;
694 
    Mess_IntegralRoll -= tmp_long2;
 814 
    MittelIntegralRoll2 = 0;
- 
 
 815 
    ZaehlMessungen = 0;
- 
 
 816 
 }
- 
 
 817 
 
695 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
818 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
696 
//  Gieren
 819 
//  Gieren
697 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
820 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
698 
    if(abs(StickGier) > 20) // war 35
821 
    if(abs(StickGier) > 20) // war 35
699 
     {
 822 
     {
700 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
 823 
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
701 
     }
 824 
     }
702 
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
 825 
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
703 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
 826 
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
704 
    sollGier = tmp_int;
 827 
    sollGier = tmp_int;
705 
    Mess_Integral_Gier -= tmp_int;  
 828 
    Mess_Integral_Gier -= tmp_int;  
706 
    if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
 829 
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
707 
    if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
 830 
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
708 
   
 - 
 
709 
 ANALOG_ON;     // ADC einschalten
 - 
 
710 
 
 831 
 
711 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
832 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
712 
//  Kompass
 833 
//  Kompass
713 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
834 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
714 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
 835 
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
715 
     {
 836 
     {
716 
       int w,v;
 837 
       int w,v;
717 
       static int SignalSchlecht = 0;
 838 
       static int SignalSchlecht = 0;
718 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
 839 
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
719 
       v = abs(IntegralRoll /512);
 840 
       v = abs(IntegralRoll /512);
720 
       if(v > w) w = v; // grösste Neigung ermitteln
 841 
       if(v > w) w = v; // grösste Neigung ermitteln
721 
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
 842 
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
722 
        {
 843 
        {
723 
         KompassStartwert = KompassValue;
 844 
         KompassStartwert = KompassValue;
724 
         NeueKompassRichtungMerken = 0;
 845 
         NeueKompassRichtungMerken = 0;
725 
        }
 846 
        }
726 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
 847 
       w = (w * Parameter_KompassWirkung) / 64;           // auf die Wirkung normieren
727 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
 848 
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
728 
       if(w > 0)
 849 
       if(w > 0)
729 
        {
 850 
        {
730 
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 - 
 
731 
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
 851 
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
732 
          ANALOG_ON;  // ADC einschalten
 - 
 
733 
          if(SignalSchlecht) SignalSchlecht--;
 852 
          if(SignalSchlecht) SignalSchlecht--;
734 
        }  
 853 
        }  
735 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
 854 
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
736 
     }
 855 
     }
737 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
856 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
738 
 
 857 
 
739 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
858 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
740 
//  Debugwerte zuordnen
 859 
//  Debugwerte zuordnen
741 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
860 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
742 
  if(!TimerWerteausgabe--)
 861 
  if(!TimerWerteausgabe--)
743 
   {
 862 
   {
744 
    TimerWerteausgabe = 49;
 863 
    TimerWerteausgabe = 24;
745 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
 864 
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
746 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
 865 
    DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor;
747 
    DebugOut.Analog[2] = Mittelwert_AccNick;
 866 
    DebugOut.Analog[2] = Mittelwert_AccNick;
748 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
 867 
    DebugOut.Analog[3] = Mittelwert_AccRoll;
749 
    DebugOut.Analog[4] = MesswertGier;
 868 
    DebugOut.Analog[4] = MesswertGier;
750 
    DebugOut.Analog[5] = HoehenWert;
 869 
    DebugOut.Analog[5] = HoehenWert;
751 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
 870 
    DebugOut.Analog[6] =(Mess_Integral_Hoch / 512);
752 
    DebugOut.Analog[8] = KompassValue;
 871 
    DebugOut.Analog[8] = KompassValue;
753 
    DebugOut.Analog[9] = UBat;
 872 
    DebugOut.Analog[9] = UBat;
754 
    DebugOut.Analog[10] = SenderOkay;
 873 
    DebugOut.Analog[10] = SenderOkay;
- 
 
 874 
    DebugOut.Analog[16] = Mittelwert_AccHoch;
755 
 
 875 
 
756 
/*    DebugOut.Analog[16] = motor_rx[0];
 876 
/*    DebugOut.Analog[16] = motor_rx[0];
757 
    DebugOut.Analog[17] = motor_rx[1];
 877 
    DebugOut.Analog[17] = motor_rx[1];
758 
    DebugOut.Analog[18] = motor_rx[2];
 878 
    DebugOut.Analog[18] = motor_rx[2];
759 
    DebugOut.Analog[19] = motor_rx[3];
 879 
    DebugOut.Analog[19] = motor_rx[3];
760 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
 880 
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
761 
    DebugOut.Analog[20] /= 14;
 881 
    DebugOut.Analog[20] /= 14;
762 
    DebugOut.Analog[21] = motor_rx[4];
 882 
    DebugOut.Analog[21] = motor_rx[4];
763 
    DebugOut.Analog[22] = motor_rx[5];
 883 
    DebugOut.Analog[22] = motor_rx[5];
764 
    DebugOut.Analog[23] = motor_rx[6];
 884 
    DebugOut.Analog[23] = motor_rx[6];
765 
    DebugOut.Analog[24] = motor_rx[7];
 885 
    DebugOut.Analog[24] = motor_rx[7];
766 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
 886 
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
767 
*/
 887 
*/
768 
//    DebugOut.Analog[9] = MesswertNick;
 888 
//    DebugOut.Analog[9] = MesswertNick;
769 
//    DebugOut.Analog[9] = SollHoehe;
 889 
//    DebugOut.Analog[9] = SollHoehe;
770 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
 890 
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
771 
//    DebugOut.Analog[11] = KompassStartwert;
 891 
//    DebugOut.Analog[11] = KompassStartwert;
772 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
 892 
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
773 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
 893 
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
774 
//    DebugOut.Analog[9] = KompassRichtung;    
 894 
//    DebugOut.Analog[9] = KompassRichtung;    
775 
//    DebugOut.Analog[10] = GasMischanteil;
 895 
//    DebugOut.Analog[10] = GasMischanteil;
776 
//    DebugOut.Analog[3] = HoeheD * 32;
 896 
//    DebugOut.Analog[3] = HoeheD * 32;
777 
//    DebugOut.Analog[4] = hoehenregler;
 897 
//    DebugOut.Analog[4] = hoehenregler;
778 
  }
 898 
  }
779 
 
 899 
 
780 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
900 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
781 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
 901 
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
782 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
902 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
783 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
 903 
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
784 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 904 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
785 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 905 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
786 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 906 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
787 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
 907 
//    MesswertGier = MesswertGier * (GyroFaktor/2) + Integral_Gier * IntegralFaktor;
788 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
 908 
    MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2;
789 
 
 909 
 
790 
    // Maximalwerte abfangen
 910 
    // Maximalwerte abfangen
791 
    #define MAX_SENSOR  2048
 911 
    #define MAX_SENSOR  2048
792 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 912 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
793 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 913 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
794 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
 914 
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
795 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
 915 
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
796 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
 916 
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
797 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
 917 
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
798 
 
 918 
 
799 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
919 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
800 
// Höhenregelung
 920 
// Höhenregelung
801 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
 921 
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
802 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
922 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
803 
//OCR0B = 180 - (Poti1 + 120) / 4;
 923 
//OCR0B = 180 - (Poti1 + 120) / 4;
804 
//DruckOffsetSetting = OCR0B;
 924 
//DruckOffsetSetting = OCR0B;
805 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
 925 
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
806 
  {
 926 
  {
807 
    int tmp_int;
 927 
    int tmp_int;
808 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
 928 
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
809 
    {
 929 
    {
810 
     if(Parameter_MaxHoehe < 50)
 930 
     if(Parameter_MaxHoehe < 50)
811 
      {
 931 
      {
812 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
 932 
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
813 
       HoehenReglerAktiv = 0;
 933 
       HoehenReglerAktiv = 0;
814 
      }
 934 
      }
815 
      else  
 935 
      else  
816 
        HoehenReglerAktiv = 1;
 936 
        HoehenReglerAktiv = 1;
817 
    }
 937 
    }
818 
    else
 938 
    else
819 
    {
 939 
    {
820 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
 940 
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
821 
     HoehenReglerAktiv = 1;
 941 
     HoehenReglerAktiv = 1;
822 
    }
 942 
    }
823 
 
 943 
 
824 
    if(Notlandung) SollHoehe = 0;
 944 
    if(Notlandung) SollHoehe = 0;
825 
    h = HoehenWert;
 945 
    h = HoehenWert;
826 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
 946 
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
827 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
 947 
     {      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil
828 
      h = GasMischanteil - h;         // vom Gas abziehen
 948 
      h = GasMischanteil - h;         // vom Gas abziehen
829 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
 949 
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
830 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
 950 
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
831 
      if(tmp_int > 50) tmp_int = 50;
 951 
      if(tmp_int > 50) tmp_int = 50;
832 
      else if(tmp_int < -50) tmp_int = -50;
 952 
      else if(tmp_int < -50) tmp_int = -50;
833 
      h -= tmp_int;
 953 
      h -= tmp_int;
834 
      hoehenregler = (hoehenregler*15 + h) / 16;      
 954 
      hoehenregler = (hoehenregler*15 + h) / 16;      
835 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
 955 
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
836 
       {
 956 
       {
837 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
 957 
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
838 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
 958 
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
839 
       }  
 959 
       }  
840 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
 960 
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
841 
      GasMischanteil = hoehenregler;
 961 
      GasMischanteil = hoehenregler;
842 
     }
 962 
     }
843 
  }
 963 
  }
844 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
964 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
845 
// + Mischer und PI-Regler
 965 
// + Mischer und PI-Regler
846 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
966 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
847 
  DebugOut.Analog[7] = GasMischanteil;
 967 
  DebugOut.Analog[7] = GasMischanteil;
848 
 
 968 
 
849 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
969 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
850 
// Gier-Anteil
 970 
// Gier-Anteil
851 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
971 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
852 
#define MUL_G  1.0
 972 
#define MUL_G  1.0
853 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
 973 
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
854 
//GierMischanteil = 0;
 974 
//GierMischanteil = 0;
855 
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
 975 
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
856 
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
 976 
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
857 
    if(GierMischanteil > 100)  GierMischanteil = 100;
 977 
    if(GierMischanteil > 100)  GierMischanteil = 100;
858 
    if(GierMischanteil < -100) GierMischanteil = -100;
 978 
    if(GierMischanteil < -100) GierMischanteil = -100;
859 
 
 979 
 
860 
    if(GasMischanteil < 20) GierMischanteil = 0;
 980 
    if(GasMischanteil < 20) GierMischanteil = 0;
861 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
981 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
862 
// Nick-Achse
 982 
// Nick-Achse
863 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
983 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
864 
 
 - 
 
865 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
 984 
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
866 
    SummeNick += DiffNick;                                   // I-Anteil
 985 
    SummeNick += DiffNick;                                   // I-Anteil
867 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
 986 
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
868 
    if(SummeNick >  16000) SummeNick =  16000;
 987 
    if(SummeNick >  16000) SummeNick =  16000;
869 
    if(SummeNick < -16000) SummeNick = -16000;
 988 
    if(SummeNick < -16000) SummeNick = -16000;
870 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
989 
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                     
871 
    // Motor Vorn
 990 
    // Motor Vorn
872 
#define MUL  2
 991 
#define MUL  2
873 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 992 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
874 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 993 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
875 
 
 994 
 
876 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
 995 
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
877 
        if ((motorwert < 0)) motorwert = 0;
 996 
        if ((motorwert < 0)) motorwert = 0;
878 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 997 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
879 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 998 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
880 
        Motor_Vorne = motorwert;           
 999 
        Motor_Vorne = motorwert;           
881 
    // Motor Heck
 1000 
    // Motor Heck
882 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
 1001 
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
883 
        if ((motorwert < 0)) motorwert = 0;
 1002 
        if ((motorwert < 0)) motorwert = 0;
884 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
 1003 
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
885 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1004 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
886 
        Motor_Hinten = motorwert;              
 1005 
        Motor_Hinten = motorwert;              
887 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1006 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
888 
// Roll-Achse
 1007 
// Roll-Achse
889 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
1008 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
890 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
 1009 
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
891 
        SummeRoll += DiffRoll;                                                              // I-Anteil
 1010 
        SummeRoll += DiffRoll;                                                              // I-Anteil
892 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
 1011 
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
893 
    if(SummeRoll >  16000) SummeRoll =  16000;
 1012 
    if(SummeRoll >  16000) SummeRoll =  16000;
894 
    if(SummeRoll < -16000) SummeRoll = -16000;
 1013 
    if(SummeRoll < -16000) SummeRoll = -16000;
895 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
 1014 
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
896 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
 1015 
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
897 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
 1016 
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
898 
    // Motor Links
 1017 
    // Motor Links
899 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
 1018 
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
900 
        if ((motorwert < 0)) motorwert = 0;
 1019 
        if ((motorwert < 0)) motorwert = 0;
901 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1020 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
902 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
 1021 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
903 
        Motor_Links = motorwert;               
 1022 
        Motor_Links = motorwert;               
904 
    // Motor Rechts
 1023 
    // Motor Rechts
905 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
 1024 
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
906 
        if ((motorwert < 0)) motorwert = 0;
 1025 
        if ((motorwert < 0)) motorwert = 0;
907 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
 1026 
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
908 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
 1027 
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
909 
        Motor_Rechts = motorwert;
 1028 
        Motor_Rechts = motorwert;
910 
   // +++++++++++++++++++++++++++++++++++++++++++++++
 1029 
   // +++++++++++++++++++++++++++++++++++++++++++++++
911 
}
 1030 
}
912 
 
 1031 
 
913 
 
 1032