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