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