Subversion Repositories FlightCtrl

Rev

Rev 211 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 211 Rev 212
Line 4... Line 4...
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
Line 48... Line 48...
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
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 55... Line 55...
55
 
55
 
Line 56... Line 56...
56
#include "main.h"
56
#include "main.h"
Line 73... Line 73...
73
volatile int  KompassStartwert = 0;
73
volatile int  KompassStartwert = 0;
74
volatile int  KompassRichtung = 0;
74
volatile int  KompassRichtung = 0;
75
unsigned char MAX_GAS,MIN_GAS;
75
unsigned char MAX_GAS,MIN_GAS;
76
unsigned char Notlandung = 0;
76
unsigned char Notlandung = 0;
77
unsigned char HoehenReglerAktiv = 0;
77
unsigned char HoehenReglerAktiv = 0;
-
 
78
//-------HF-------
-
 
79
unsigned char LED_Switch = 0;
-
 
80
unsigned char LED_flash  = 0;
-
 
81
unsigned char LED_1  = 0;
-
 
82
unsigned char LED_2  = 0;
-
 
83
unsigned char LED_2on  = 0;
-
 
84
//-------HF-------
Line 78... Line 85...
78
 
85
 
79
float GyroFaktor;
86
float GyroFaktor;
Line 80... Line 87...
80
float IntegralFaktor;
87
float IntegralFaktor;
81
 
88
 
82
volatile int  DiffNick,DiffRoll;
89
volatile int  DiffNick,DiffRoll;
83
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
90
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
84
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
91
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
85
unsigned char MotorWert[5];
92
unsigned char MotorWert[5];
86
volatile unsigned char SenderOkay = 0;
93
volatile unsigned char SenderOkay = 0;
Line 124... Line 131...
124
//  Nullwerte ermitteln
131
//  Nullwerte ermitteln
125
void SetNeutral(void)
132
void SetNeutral(void)
126
//############################################################################
133
//############################################################################
127
{
134
{
128
    unsigned int timer;
135
    unsigned int timer;
129
        NeutralAccX = 0;
136
        NeutralAccX = 0;
130
        NeutralAccY = 0;
137
        NeutralAccY = 0;
131
        NeutralAccZ = 0;
138
        NeutralAccZ = 0;
132
    AdNeutralNick = 0; 
139
    AdNeutralNick = 0;
133
        AdNeutralRoll = 0;     
140
        AdNeutralRoll = 0;
134
        AdNeutralGier = 0;
141
        AdNeutralGier = 0;
135
    CalibrierMittelwert();     
142
    CalibrierMittelwert();
136
    timer = SetDelay(5);    
143
    timer = SetDelay(5);
137
        while (!CheckDelay(timer));
144
        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
     }
Line 143... Line 150...
143
 
150
 
144
    if(PlatinenVersion == 10)
151
    if(PlatinenVersion == 10)
145
    {
152
    {
146
     AdNeutralNick= abs(MesswertNick); 
153
     AdNeutralNick= abs(MesswertNick);
147
         AdNeutralRoll= abs(MesswertRoll);     
154
         AdNeutralRoll= abs(MesswertRoll);
148
         AdNeutralGier= abs(MesswertGier);
155
         AdNeutralGier= abs(MesswertGier);
149
    }
156
    }
150
    else
157
    else
151
    {
158
    {
152
     AdNeutralNick= abs(MesswertNick) / 2;     
159
     AdNeutralNick= abs(MesswertNick) / 2;
153
         AdNeutralRoll= abs(MesswertRoll) / 2; 
160
         AdNeutralRoll= abs(MesswertRoll) / 2;
154
         AdNeutralGier= abs(MesswertGier) / 2;
161
         AdNeutralGier= abs(MesswertGier) / 2;
155
    }
162
    }
156
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
163
    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;
157
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
164
        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;
158
        NeutralAccZ = Aktuell_az;
165
        NeutralAccZ = Aktuell_az;
159
   
166
 
160
        Mess_IntegralNick = 0; 
167
        Mess_IntegralNick = 0;
161
    Mess_IntegralNick2 = 0;
168
    Mess_IntegralNick2 = 0;
162
    Mess_IntegralRoll = 0;     
169
    Mess_IntegralRoll = 0;
163
    Mess_IntegralRoll2 = 0;
170
    Mess_IntegralRoll2 = 0;
164
    Mess_Integral_Gier = 0;    
171
    Mess_Integral_Gier = 0;
165
    MesswertNick = 0;
172
    MesswertNick = 0;
166
    MesswertRoll = 0;
173
    MesswertRoll = 0;
167
    MesswertGier = 0;
174
    MesswertGier = 0;
168
    StartLuftdruck = Luftdruck;
175
    StartLuftdruck = Luftdruck;
169
    HoeheD = 0;
176
    HoeheD = 0;
170
    Mess_Integral_Hoch = 0;
177
    Mess_Integral_Hoch = 0;
171
    KompassStartwert = KompassValue;
178
    KompassStartwert = KompassValue;
172
    GPS_Neutral();
179
    GPS_Neutral();
173
    beeptime = 50;  
180
    beeptime = 50;
Line 174... Line 181...
174
}
181
}
175
 
182
 
176
//############################################################################
183
//############################################################################
177
// Bildet den Mittelwert aus den Messwerten
184
// Bildet den Mittelwert aus den Messwerten
178
void Mittelwert(void)
185
void Mittelwert(void)
179
//############################################################################
186
//############################################################################
180
{      
187
{
181
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
188
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
182
    ANALOG_OFF;
189
    ANALOG_OFF;
183
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
190
        if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
Line 194... Line 201...
194
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
201
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
195
    Integral_Gier  = Mess_Integral_Gier;
202
    Integral_Gier  = Mess_Integral_Gier;
196
//    Integral_Gier2 = Mess_Integral_Gier2;
203
//    Integral_Gier2 = Mess_Integral_Gier2;
197
    IntegralNick = Mess_IntegralNick;
204
    IntegralNick = Mess_IntegralNick;
198
    IntegralRoll = Mess_IntegralRoll;
205
    IntegralRoll = Mess_IntegralRoll;
199
    IntegralNick2 = Mess_IntegralNick2;
206
    IntegralNick2 = Mess_IntegralNick2;
200
    IntegralRoll2 = Mess_IntegralRoll2;
207
    IntegralRoll2 = Mess_IntegralRoll2;
201
    // ADC einschalten
208
    // ADC einschalten
202
    ANALOG_ON; 
209
    ANALOG_ON;
Line 203... Line 210...
203
 
210
 
204
/*
211
/*
205
//------------------------------------------------------------------------------
212
//------------------------------------------------------------------------------
206
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
213
    if(MesswertNick > 200)  MesswertNick += 4 * (MesswertNick - 200);
207
    else                                         
214
    else
208
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
215
    if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);
209
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
216
    if(MesswertRoll > 200)  MesswertRoll += 4 * (MesswertRoll - 200);
210
    else                                         
217
    else
211
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
218
    if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);
212
//------------------------------------------------------------------------------
219
//------------------------------------------------------------------------------
213
*/
220
*/
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--;
221
    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--;
222
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
Line 223... Line 230...
223
 
230
 
224
//############################################################################
231
//############################################################################
225
// Messwerte beim Ermitteln der Nullage
232
// Messwerte beim Ermitteln der Nullage
226
void CalibrierMittelwert(void)
233
void CalibrierMittelwert(void)
227
//############################################################################
234
//############################################################################
228
{                
235
{
229
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
236
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
230
        ANALOG_OFF;
237
        ANALOG_OFF;
231
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
238
    if(MessanzahlNick)    (MesswertNick = AccumulateNick / MessanzahlNick);
232
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
239
        if(MessanzahlRoll)    (MesswertRoll = AccumulateRoll / MessanzahlRoll);
Line 239... Line 246...
239
    AccumulateGier = 0;   MessanzahlGier = 0;
246
    AccumulateGier = 0;   MessanzahlGier = 0;
240
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
247
    accumulate_AccRoll = 0;messanzahl_AccRoll = 0;
241
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
248
    accumulate_AccNick = 0;messanzahl_AccNick = 0;
242
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
249
    accumulate_AccHoch = 0;messanzahl_AccHoch = 0;
243
    // ADC einschalten
250
    // ADC einschalten
244
    ANALOG_ON; 
251
    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--;
252
    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--;
253
    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--;
254
    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--;
255
    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;
256
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
Line 254... Line 261...
254
 
261
 
255
//############################################################################
262
//############################################################################
256
// Senden der Motorwerte per I2C-Bus
263
// Senden der Motorwerte per I2C-Bus
257
void SendMotorData(void)
264
void SendMotorData(void)
258
//############################################################################
265
//############################################################################
259
{
266
{
260
    if(MOTOR_OFF || !MotorenEin)
267
    if(MOTOR_OFF || !MotorenEin)
261
        {
268
        {
262
        Motor_Hinten = 0;
269
        Motor_Hinten = 0;
263
        Motor_Vorne = 0;
270
        Motor_Vorne = 0;
Line 270... Line 277...
270
        }
277
        }
Line 271... Line 278...
271
 
278
 
272
    DebugOut.Analog[12] = Motor_Vorne;
279
    DebugOut.Analog[12] = Motor_Vorne;
273
    DebugOut.Analog[13] = Motor_Hinten;
280
    DebugOut.Analog[13] = Motor_Hinten;
274
    DebugOut.Analog[14] = Motor_Links;
281
    DebugOut.Analog[14] = Motor_Links;
Line 275... Line 282...
275
    DebugOut.Analog[15] = Motor_Rechts;  
282
    DebugOut.Analog[15] = Motor_Rechts;
276
 
283
 
277
    //Start I2C Interrupt Mode
284
    //Start I2C Interrupt Mode
278
    twi_state = 0;
285
    twi_state = 0;
279
    motor = 0;
286
    motor = 0;
Line 280... Line 287...
280
    i2c_start();
287
    i2c_start();
281
}
288
}
282
 
289
 
283
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
290
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
284
// + Konstanten 
291
// + Konstanten
285
// + 0-250 -> normale Werte
292
// + 0-250 -> normale Werte
286
// + 251 -> Poti1
293
// + 251 -> Poti1
287
// + 252 -> Poti2
294
// + 252 -> Poti2
288
// + 253 -> Poti3
295
// + 253 -> Poti3
289
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
296
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
290
void DefaultKonstanten1(void)
297
void DefaultKonstanten1(void)
291
{
298
{
292
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
299
 EE_Parameter.Kanalbelegung[K_NICK]  = 3;
293
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
300
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
294
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
301
 EE_Parameter.Kanalbelegung[K_GAS]   = 1;
295
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
302
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
296
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
303
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
297
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
304
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
298
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
305
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
299
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
306
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
300
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;    
307
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01;
301
 EE_Parameter.Hoehe_MinGas = 30;
308
 EE_Parameter.Hoehe_MinGas = 30;
302
 EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
309
 EE_Parameter.MaxHoehe     = 251;      // Wert : 0-32   251 -> Poti1
Line 329... Line 336...
329
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
336
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
330
 EE_Parameter.ServoNickRefresh = 5;
337
 EE_Parameter.ServoNickRefresh = 5;
331
 EE_Parameter.LoopGasLimit = 50;
338
 EE_Parameter.LoopGasLimit = 50;
332
 EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
339
 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
340
 EE_Parameter.LoopConfig = 0;             // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt
334
 memcpy(EE_Parameter.Name, "Sport\0", 12);  
341
 memcpy(EE_Parameter.Name, "Sport\0", 12);
335
}
342
}
Line 336... Line 343...
336
 
343
 
337
void DefaultKonstanten2(void)
344
void DefaultKonstanten2(void)
338
{
345
{
339
 EE_Parameter.Kanalbelegung[K_NICK]  = 1;
346
 EE_Parameter.Kanalbelegung[K_NICK]  = 3;
340
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
347
 EE_Parameter.Kanalbelegung[K_ROLL]  = 2;
341
 EE_Parameter.Kanalbelegung[K_GAS]   = 3;
348
 EE_Parameter.Kanalbelegung[K_GAS]   = 1;
342
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
349
 EE_Parameter.Kanalbelegung[K_GIER]  = 4;
343
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
350
 EE_Parameter.Kanalbelegung[K_POTI1] = 5;
344
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
351
 EE_Parameter.Kanalbelegung[K_POTI2] = 6;
345
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
352
 EE_Parameter.Kanalbelegung[K_POTI3] = 7;
346
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
353
 EE_Parameter.Kanalbelegung[K_POTI4] = 8;
347
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;    
354
 EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01;
348
 EE_Parameter.Hoehe_MinGas = 30;
355
 EE_Parameter.Hoehe_MinGas = 30;
349
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
356
 EE_Parameter.MaxHoehe     = 251;     // Wert : 0-32   251 -> Poti1
350
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
357
 EE_Parameter.Hoehe_P      = 10;      // Wert : 0-32
351
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
358
 EE_Parameter.Luftdruck_D  = 50;      // Wert : 0-250
Line 375... Line 382...
375
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
382
 EE_Parameter.ServoNickMin = 50;           // Wert : 0-250     // Anschlag
376
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
383
 EE_Parameter.ServoNickMax = 150;         // Wert : 0-250     // Anschlag
377
 EE_Parameter.ServoNickRefresh = 5;
384
 EE_Parameter.ServoNickRefresh = 5;
378
 EE_Parameter.LoopGasLimit = 50;
385
 EE_Parameter.LoopGasLimit = 50;
379
 EE_Parameter.LoopThreshold = 90;         // Wert: 0-250  Schwelle für Stickausschlag
386
 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 
387
 EE_Parameter.LoopConfig = 0;              // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts
381
 memcpy(EE_Parameter.Name, "Kamera\0", 12);  
388
 memcpy(EE_Parameter.Name, "Kamera\0", 12);
382
}
389
}
Line 383... Line 390...
383
 
390
 
384
 
391
 
385
//############################################################################
392
//############################################################################
386
// Trägt ggf. das Poti als Parameter ein
393
// Trägt ggf. das Poti als Parameter ein
387
void ParameterZuordnung(void)
394
void ParameterZuordnung(void)
Line 388... Line 395...
388
//############################################################################
395
//############################################################################
389
{
396
{
Line 394... Line 401...
394
 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
401
 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);
402
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
396
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
403
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
397
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
404
 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
398
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
405
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
399
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
406
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
400
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
407
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
401
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
408
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
402
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
409
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
403
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
410
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
404
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
411
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
405
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
412
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
406
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
413
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
407
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
414
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
Line 428... Line 435...
428
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
435
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
429
         static unsigned int  modell_fliegt = 0;
436
         static unsigned int  modell_fliegt = 0;
430
     static int hoehenregler = 0;
437
     static int hoehenregler = 0;
431
     static char TimerWerteausgabe = 0;
438
     static char TimerWerteausgabe = 0;
432
     static char NeueKompassRichtungMerken = 0;
439
     static char NeueKompassRichtungMerken = 0;
433
        Mittelwert();
440
        Mittelwert();
Line 434... Line 441...
434
 
441
 
Line 435... Line 442...
435
    GRN_ON;
442
    GRN_ON;
436
 
443
 
437
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
444
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
438
// Gaswert ermitteln
445
// Gaswert ermitteln
439
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
446
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
440
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
447
        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
441
    if(GasMischanteil < 0) GasMischanteil = 0;
448
    if(GasMischanteil < 0) GasMischanteil = 0;
442
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
449
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
443
// Emfang schlecht
450
// Emfang schlecht
444
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
451
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
445
   if(SenderOkay < 100)
452
   if(SenderOkay < 100)
446
        {
453
        {
447
        if(!PcZugriff)
454
        if(!PcZugriff)
448
         {
455
         {
449
           if(BeepMuster == 0xffff)
456
           if(BeepMuster == 0xffff)
450
            {
457
            {
451
             beeptime = 15000;
458
             beeptime = 15000;
452
             BeepMuster = 0x0c00;
459
             BeepMuster = 0x0c00;
453
            }
460
            }
454
         }
461
         }
455
        if(RcLostTimer) RcLostTimer--;
462
        if(RcLostTimer) RcLostTimer--;
456
        else
463
        else
457
         {
464
         {
458
          MotorenEin = 0;
465
          MotorenEin = 0;
459
          Notlandung = 0;
466
          Notlandung = 0;
460
         }
467
         }
461
        ROT_ON;
468
        ROT_ON;
462
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
469
        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken
463
            {
470
            {
Line 467... Line 474...
467
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
474
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
468
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
475
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
469
            }
476
            }
470
         else MotorenEin = 0;
477
         else MotorenEin = 0;
471
        }
478
        }
472
        else
479
        else
473
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
480
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
474
// Emfang gut
481
// Emfang gut
475
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
482
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
476
        if(SenderOkay > 140)
483
        if(SenderOkay > 140)
477
            {
484
            {
478
            Notlandung = 0;
485
            Notlandung = 0;
479
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
486
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
480
            if(GasMischanteil > 40)
487
            if(GasMischanteil > 40)
Line 483... Line 490...
483
                }
490
                }
484
            if((modell_fliegt < 200) || (GasMischanteil < 40))
491
            if((modell_fliegt < 200) || (GasMischanteil < 40))
485
                {
492
                {
486
                SummeNick = 0;
493
                SummeNick = 0;
487
                SummeRoll = 0;
494
                SummeRoll = 0;
488
                Mess_Integral_Gier = 0;
495
                Mess_Integral_Gier = 0;
489
                Mess_Integral_Gier2 = 0;
496
                Mess_Integral_Gier2 = 0;
490
                }
497
                }
491
            if((GasMischanteil > 200) && MotorenEin == 0)
498
            if((GasMischanteil > 200) && MotorenEin == 0)
492
                {
499
                {
493
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
500
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
494
// auf Nullwerte kalibrieren
501
// auf Nullwerte kalibrieren
495
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
502
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
496
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
503
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
497
                    {
504
                    {
498
                    unsigned char setting;
505
                    unsigned char setting;
499
                    if(++delay_neutral > 200)  // nicht sofort
506
                    if(++delay_neutral > 200)  // nicht sofort
500
                        {
507
                        {
Line 512... Line 519...
512
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
519
                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken
513
                        }
520
                        }
514
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
521
                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
515
                          {
522
                          {
516
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
523
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
517
                          }  
524
                          }
518
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
525
                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
519
                        SetNeutral();
526
                        SetNeutral();
520
                        Piep(GetActiveParamSetNumber());
527
                        Piep(GetActiveParamSetNumber());
521
                        }
528
                        }
522
                    }
529
                    }
523
                 else delay_neutral = 0;
530
                 else delay_neutral = 0;
524
                }
531
                }
525
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
532
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
526
// Gas ist unten
533
// Gas ist unten
527
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
534
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
528
            if(GasMischanteil < 35)
535
            if(GasMischanteil < 35)
529
                {
536
                {
530
                // Starten
537
                // Starten
531
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
538
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
532
                    {
539
                    {
533
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
540
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
534
// Einschalten
541
// Einschalten
535
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
542
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
536
                    if(++delay_einschalten > 200)
543
                    if(++delay_einschalten > 200)
537
                        {
544
                        {
538
                        delay_einschalten = 200;
545
                        delay_einschalten = 200;
539
                        modell_fliegt = 1;
546
                        modell_fliegt = 1;
540
                        MotorenEin = 1;
547
                        MotorenEin = 1;
541
                        sollGier = 0;
548
                        sollGier = 0;
542
                        Mess_Integral_Gier = 0;
549
                        Mess_Integral_Gier = 0;
543
                        Mess_Integral_Gier2 = 0;
550
                        Mess_Integral_Gier2 = 0;
544
                        Mess_IntegralNick = 0;
551
                        Mess_IntegralNick = 0;
545
                        Mess_IntegralRoll = 0;
552
                        Mess_IntegralRoll = 0;
546
                        Mess_IntegralNick2 = IntegralNick;
553
                        Mess_IntegralNick2 = IntegralNick;
547
                        Mess_IntegralRoll2 = IntegralRoll;
554
                        Mess_IntegralRoll2 = IntegralRoll;
548
                        SummeNick = 0;
555
                        SummeNick = 0;
549
                        SummeRoll = 0;
556
                        SummeRoll = 0;
550
                        }          
557
                        }
551
                    }  
558
                    }
552
                    else delay_einschalten = 0;
559
                    else delay_einschalten = 0;
553
                //Auf Neutralwerte setzen
560
                //Auf Neutralwerte setzen
554
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
561
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
555
// Auschalten
562
// Auschalten
556
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
563
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
557
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
564
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
558
                    {
565
                    {
559
                    if(++delay_ausschalten > 200)  // nicht sofort
566
                    if(++delay_ausschalten > 200)  // nicht sofort
560
                        {
567
                        {
561
                        MotorenEin = 0;
568
                        MotorenEin = 0;
562
                        delay_ausschalten = 200;
569
                        delay_ausschalten = 200;
563
                        modell_fliegt = 0;
570
                        modell_fliegt = 0;
564
                        }
571
                        }
565
                    }
572
                    }
566
                else delay_ausschalten = 0;
573
                else delay_ausschalten = 0;
567
                }
574
                }
568
            }
575
            }
Line 569... Line 576...
569
 
576
 
570
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
577
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
571
// neue Werte von der Funke
578
// neue Werte von der Funke
572
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
579
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
573
 if(!NewPpmData-- || Notlandung)  
580
 if(!NewPpmData-- || Notlandung)
574
  {
581
  {
575
    ParameterZuordnung();
582
    ParameterZuordnung();
576
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
583
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
577
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
584
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
578
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
585
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
579
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
586
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
580
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
587
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
581
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
588
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
Line 582... Line 589...
582
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
589
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
583
 
590
 
584
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
591
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
-
 
592
    if(GyroFaktor < 0) GyroFaktor = 0;
-
 
593
    if(IntegralFaktor < 0) IntegralFaktor = 0;
-
 
594
       
-
 
595
    //------HF------- LEDs Schalten - LED1 Normal, LED2 Blitzlicht
-
 
596
        LED_Switch = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]];
-
 
597
        if (((LED_Switch > 32) & (LED_Switch < 96)) | (LED_Switch > 160)) LED_2++; else if (LED_2 > 0) LED_2--;
-
 
598
        if ((LED_Switch > 96)) LED_1++; else if (LED_1 > 0) LED_1--;
-
 
599
        if (LED_1 >= 10) {LED_1 = 10; LED1_ON;}
-
 
600
    if (LED_1 <=  1) LED1_OFF;
-
 
601
        if (LED_2 >= 10) {LED_2 = 10; LED_2on = 1;}
-
 
602
        if (LED_2 <=  1) LED_2on = 0;
-
 
603
        if (LED_2on  ==  1) LED_flash++;
-
 
604
        if (LED_flash > 77) LED2_ON; else LED2_OFF;
-
 
605
        if (LED_flash > 80) LED_flash = 0;
585
    if(GyroFaktor < 0) GyroFaktor = 0;
606
        //------HF------- End LEDs Schalten
586
    if(IntegralFaktor < 0) IntegralFaktor = 0;
607
 
587
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
608
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
588
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
609
    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
589
     {
610
     {
590
      if(IntegralNick >  60000)  
611
      if(IntegralNick >  60000)
591
      {
612
      {
592
       StickNick -=  8 * EE_Parameter.Stick_P;      
613
       StickNick -=  8 * EE_Parameter.Stick_P;
593
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
614
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
594
      }
615
      }
595
      else
616
      else
596
      if(IntegralNick < -60000)  
617
      if(IntegralNick < -60000)
597
      {
618
      {
598
       StickNick += 8 * EE_Parameter.Stick_P;
619
       StickNick += 8 * EE_Parameter.Stick_P;
599
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      
620
       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;
600
      }
621
      }
601
      if(IntegralRoll >  60000)  
622
      if(IntegralRoll >  60000)
602
      {
623
      {
603
       StickRoll -=  8 * EE_Parameter.Stick_P;      
624
       StickRoll -=  8 * EE_Parameter.Stick_P;
604
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
625
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
605
      }
626
      }
606
      else
627
      else
607
      if(IntegralRoll < -60000)  
628
      if(IntegralRoll < -60000)
608
      {
629
      {
609
       StickRoll += 8 * EE_Parameter.Stick_P;
630
       StickRoll += 8 * EE_Parameter.Stick_P;
610
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      
631
       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;
Line 611... Line 632...
611
      }
632
      }
Line 612... Line 633...
612
     }
633
     }
613
 
634
 
614
  }
635
  }
615
 
636
 
616
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
637
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
617
// Looping?
638
// Looping?
618
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
639
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
619
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
640
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) ||
620
     ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
641
     ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))
621
   {
642
   {
Line 622... Line 643...
622
    Looping_Roll = 1;
643
    Looping_Roll = 1;
623
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
644
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
624
   }
645
   }
625
   else Looping_Roll = 0;  
646
   else Looping_Roll = 0;
626
 
647
 
627
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
648
  if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) ||
628
     ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
649
     ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN))
629
   {
650
   {
630
    Looping_Nick = 1;
651
    Looping_Nick = 1;
Line 631... Line 652...
631
    Looping_Roll = 0;
652
    Looping_Roll = 0;
632
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
653
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
633
   }
654
   }
634
   else Looping_Nick = 0;  
655
   else Looping_Nick = 0;
635
   
656
 
636
 
657
 
637
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
658
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
638
// Bei Empfangsausfall im Flug 
659
// Bei Empfangsausfall im Flug
639
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
660
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
640
   if(Notlandung)
661
   if(Notlandung)
641
    {
662
    {
642
     StickGier = 0;
663
     StickGier = 0;
643
     StickNick = 0;
664
     StickNick = 0;
644
     StickRoll = 0;
665
     StickRoll = 0;
645
     GyroFaktor  = 0.1;
666
     GyroFaktor  = 0.1;
646
     IntegralFaktor = 0.005;
667
     IntegralFaktor = 0.005;
647
     Looping_Roll = 0;
668
     Looping_Roll = 0;
648
     Looping_Nick = 0;
669
     Looping_Nick = 0;
649
    }  
670
    }
650
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
671
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
651
// Gyro-Drift kompensieren
672
// Gyro-Drift kompensieren
652
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
673
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
653
#define DRIFT_FAKTOR 3
674
#define DRIFT_FAKTOR 3
654
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
675
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
655
        {
676
        {
656
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
677
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
657
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
678
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
658
        ZaehlMessungen = 0;
679
        ZaehlMessungen = 0;
659
        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
680
        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
660
        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
681
        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
661
        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
682
        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
662
        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
683
        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
663
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
684
//        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)      
685
//        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
686
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
666
        Mess_IntegralNick2 = IntegralNick;
687
        Mess_IntegralNick2 = IntegralNick;
667
        Mess_IntegralRoll2 = IntegralRoll;
688
        Mess_IntegralRoll2 = IntegralRoll;
668
        Mess_Integral_Gier2 = Integral_Gier;
689
        Mess_Integral_Gier2 = Integral_Gier;
669
    ANALOG_ON;  // ADC einschalten
690
    ANALOG_ON;  // ADC einschalten
670
        }
691
        }
671
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
692
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
672
// Integrale auf ACC-Signal abgleichen
693
// Integrale auf ACC-Signal abgleichen
673
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
694
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
674
  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
695
  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
675
  {
696
  {
Line 676... Line 697...
676
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
697
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
677
    if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
698
    if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
678
    else tmp_long /= 16;
699
    else tmp_long /= 16;
679
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
700
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
680
    if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
701
    if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
681
    else tmp_long2 /= 16;
702
    else tmp_long2 /= 16;
682
 
703
 
683
 #define AUSGLEICH 500
704
 #define AUSGLEICH 500
684
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
705
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
685
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
706
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
686
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
707
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
687
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
708
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
688
  }
709
  }
689
  else
710
  else
690
  {
711
  {
691
   tmp_long = 0;
712
   tmp_long = 0;
692
   tmp_long2 = 0;
713
   tmp_long2 = 0;
693
  }
714
  }
694
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
715
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
695
    Mess_IntegralNick -= tmp_long;
716
    Mess_IntegralNick -= tmp_long;
696
    Mess_IntegralRoll -= tmp_long2;
717
    Mess_IntegralRoll -= tmp_long2;
697
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
718
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
698
//  Gieren
719
//  Gieren
699
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
720
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
700
    if(abs(StickGier) > 35)
721
    if(abs(StickGier) > 35)
701
     {
722
     {
702
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
723
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
703
     }
724
     }
704
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
725
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
Line 705... Line 726...
705
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
726
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
706
    sollGier = tmp_int;
727
    sollGier = tmp_int;
707
    Mess_Integral_Gier -= tmp_int;  
728
    Mess_Integral_Gier -= tmp_int;
708
    if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
729
    if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000;  // begrenzen
709
    if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
730
    if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000;
710
   
731
 
711
 ANALOG_ON;     // ADC einschalten
732
 ANALOG_ON;     // ADC einschalten
712
 
733
 
713
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
734
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
714
//  Kompass
735
//  Kompass
715
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
736
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
716
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
737
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
717
     {
738
     {
718
       int w,v;
739
       int w,v;
719
       static int SignalSchlecht = 0;
740
       static int SignalSchlecht = 0;
720
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
741
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
721
       v = abs(IntegralRoll /512);
742
       v = abs(IntegralRoll /512);
Line 731... Line 752...
731
        {
752
        {
732
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
753
          ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
733
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
754
          if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32;  // nach Kompass ausrichten
734
          ANALOG_ON;  // ADC einschalten
755
          ANALOG_ON;  // ADC einschalten
735
          if(SignalSchlecht) SignalSchlecht--;
756
          if(SignalSchlecht) SignalSchlecht--;
736
        }  
757
        }
737
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
758
        else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
738
     }
759
     }
739
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
760
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 740... Line 761...
740
 
761
 
741
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
762
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
742
//  Debugwerte zuordnen
763
//  Debugwerte zuordnen
743
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
764
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
744
  if(!TimerWerteausgabe--)
765
  if(!TimerWerteausgabe--)
745
   {
766
   {
746
    TimerWerteausgabe = 49;
767
    TimerWerteausgabe = 49;
747
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
768
    DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor;
Line 767... Line 788...
767
*/
788
*/
768
//    DebugOut.Analog[9] = MesswertNick;
789
//    DebugOut.Analog[9] = MesswertNick;
769
//    DebugOut.Analog[9] = SollHoehe;
790
//    DebugOut.Analog[9] = SollHoehe;
770
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
791
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
771
//    DebugOut.Analog[11] = KompassStartwert;
792
//    DebugOut.Analog[11] = KompassStartwert;
772
//    DebugOut.Analog[10] = Parameter_Gyro_I;    
793
//    DebugOut.Analog[10] = Parameter_Gyro_I;
773
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;    
794
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
774
//    DebugOut.Analog[9] = KompassRichtung;    
795
//    DebugOut.Analog[9] = KompassRichtung;
775
//    DebugOut.Analog[10] = GasMischanteil;
796
//    DebugOut.Analog[10] = GasMischanteil;
776
//    DebugOut.Analog[3] = HoeheD * 32;
797
//    DebugOut.Analog[3] = HoeheD * 32;
777
//    DebugOut.Analog[4] = hoehenregler;
798
//    DebugOut.Analog[4] = hoehenregler;
778
  }
799
  }
Line 779... Line 800...
779
 
800
 
780
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
801
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
781
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
802
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
782
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
803
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
783
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
804
    if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor;
784
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
805
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
785
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
806
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
786
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
807
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
Line 794... Line 815...
794
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
815
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
795
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
816
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
796
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
817
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
797
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
818
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
Line 798... Line 819...
798
 
819
 
799
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
820
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
800
// Höhenregelung
821
// Höhenregelung
801
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
822
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
802
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
823
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
803
//OCR0B = 180 - (Poti1 + 120) / 4;
824
//OCR0B = 180 - (Poti1 + 120) / 4;
804
//DruckOffsetSetting = OCR0B;
825
//DruckOffsetSetting = OCR0B;
805
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
826
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
806
  {
827
  {
807
    int tmp_int;
828
    int tmp_int;
808
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
829
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
809
    {
830
    {
810
     if(Parameter_MaxHoehe < 50)
831
     if(Parameter_MaxHoehe < 50)
811
      {
832
      {
812
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
833
       SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
813
       HoehenReglerAktiv = 0;
834
       HoehenReglerAktiv = 0;
814
      }
835
      }
815
      else  
836
      else
816
        HoehenReglerAktiv = 1;
837
        HoehenReglerAktiv = 1;
817
    }
838
    }
818
    else
839
    else
819
    {
840
    {
820
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
841
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
821
     HoehenReglerAktiv = 1;
842
     HoehenReglerAktiv = 1;
Line 829... Line 850...
829
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
850
      h -= (HoeheD * Parameter_Luftdruck_D)/8;    // D-Anteil
830
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
851
      tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32;
831
      if(tmp_int > 50) tmp_int = 50;
852
      if(tmp_int > 50) tmp_int = 50;
832
      else if(tmp_int < -50) tmp_int = -50;
853
      else if(tmp_int < -50) tmp_int = -50;
833
      h -= tmp_int;
854
      h -= tmp_int;
834
      hoehenregler = (hoehenregler*15 + h) / 16;      
855
      hoehenregler = (hoehenregler*15 + h) / 16;
835
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
856
      if(hoehenregler < EE_Parameter.Hoehe_MinGas) // nicht unter MIN
836
       {
857
       {
837
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
858
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas) hoehenregler = EE_Parameter.Hoehe_MinGas;
838
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
859
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas) hoehenregler = GasMischanteil;
839
       }  
860
       }
840
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
861
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
841
      GasMischanteil = hoehenregler;
862
      GasMischanteil = hoehenregler;
842
     }
863
     }
843
  }
864
  }
844
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
865
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
845
// + Mischer und PI-Regler
866
// + Mischer und PI-Regler
846
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
867
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
847
  DebugOut.Analog[7] = GasMischanteil;
868
  DebugOut.Analog[7] = GasMischanteil;
Line 848... Line 869...
848
 
869
 
849
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
870
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
850
// Gier-Anteil
871
// Gier-Anteil
851
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
872
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
852
#define MUL_G  1.4
873
#define MUL_G  1.4
853
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
874
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
854
//GierMischanteil = 0;
875
//GierMischanteil = 0;
855
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
876
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
Line 856... Line 877...
856
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
877
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
857
 
878
 
858
    if(GasMischanteil < 10) GierMischanteil = 0;
879
    if(GasMischanteil < 10) GierMischanteil = 0;
859
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
880
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 860... Line 881...
860
// Nick-Achse
881
// Nick-Achse
861
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
882
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
862
 
883
 
863
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
884
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
864
    SummeNick += DiffNick;                                   // I-Anteil
885
    SummeNick += DiffNick;                                   // I-Anteil
865
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
886
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
866
    if(SummeNick >  16000) SummeNick =  16000;
887
    if(SummeNick >  16000) SummeNick =  16000;
867
    if(SummeNick < -16000) SummeNick = -16000;
888
    if(SummeNick < -16000) SummeNick = -16000;
868
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick                                   
889
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick
869
    // Motor Vorn
890
    // Motor Vorn
Line 870... Line 891...
870
#define MUL  2
891
#define MUL  2
871
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
892
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
872
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
893
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
873
 
894
 
874
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
895
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
875
        if ((motorwert < 0)) motorwert = 0;
896
        if ((motorwert < 0)) motorwert = 0;
876
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
897
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
877
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;       
898
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
878
        Motor_Vorne = motorwert;           
899
        Motor_Vorne = motorwert;
879
    // Motor Heck
900
    // Motor Heck
880
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
901
        motorwert = GasMischanteil - pd_ergebnis + GierMischanteil;
881
        if ((motorwert < 0)) motorwert = 0;
902
        if ((motorwert < 0)) motorwert = 0;
882
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
903
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
883
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
904
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
884
        Motor_Hinten = motorwert;              
905
        Motor_Hinten = motorwert;
885
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
906
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
886
// Roll-Achse
907
// Roll-Achse
887
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
908
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
888
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
909
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
889
        SummeRoll += DiffRoll;                                                              // I-Anteil
910
        SummeRoll += DiffRoll;                                                              // I-Anteil
890
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
911
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
891
    if(SummeRoll >  16000) SummeRoll =  16000;
912
    if(SummeRoll >  16000) SummeRoll =  16000;
892
    if(SummeRoll < -16000) SummeRoll = -16000;
913
    if(SummeRoll < -16000) SummeRoll = -16000;
893
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
914
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
894
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
915
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
895
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
916
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
896
    // Motor Links
917
    // Motor Links
897
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
918
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
898
        if ((motorwert < 0)) motorwert = 0;
919
        if ((motorwert < 0)) motorwert = 0;
899
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
920
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
900
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
921
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
901
        Motor_Links = motorwert;               
922
        Motor_Links = motorwert;
902
    // Motor Rechts
923
    // Motor Rechts
903
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
924
        motorwert = GasMischanteil - pd_ergebnis - GierMischanteil;
904
        if ((motorwert < 0)) motorwert = 0;
925
        if ((motorwert < 0)) motorwert = 0;
905
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
926
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;