Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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