Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1755 - 1
/******************************************************************************************************************
2
V0.80g-Arthur-P1 20100922
3
------------------------------------------------------------------------------------------------------------------
4
Version includes only support for external HEF4017 for FC1.x hardware, NOT for Twi2Ppm converters for ESCs.
5
 
6
20100917: Transferred changes to v0.80g-Arthur-Px.
7
20100804: Arthur P.: Modified to use user parameter 7 to determine downstep for motorsmoothing
8
with 0 or 1 defaulting to the default -150% first step followed by upsmoothing and
9
values beyond that resulting in 50% (2), 75% (4), 90% (10), 95% (20), 97.5% (40), 99% (100)
10
downsteps.
11
Within timer0.c user paramater 8 is used to activate an external HEF4017 on FC 1.x hardware,
12
and/or to set the shutter interval timer in steps of 0.1sec (minimum = 1 sec), by using
13
bit 8 (128) as on/off switch, and bits 7 (0..127) for the timer counter.
14
 
15
******************************************************************************************************************/
16
/*#######################################################################################
17
Flight Control
18
#######################################################################################*/
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
20
// + Copyright (c) Holger Buss, Ingo Busker
21
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
22
// + www.MikroKopter.com
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
24
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
25
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
26
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
27
// + bzgl. der Nutzungsbedingungen aufzunehmen.
28
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
29
// + Verkauf von Luftbildaufnahmen, usw.
30
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
32
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
35
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
36
// + eindeutig als Ursprung verlinkt werden
37
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
38
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
39
// + Benutzung auf eigene Gefahr
40
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
41
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
42
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
43
// + mit unserer Zustimmung zulässig
44
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
45
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
46
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
47
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
48
// + this list of conditions and the following disclaimer.
49
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
50
// +     from this software without specific prior written permission.
51
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
52
// +     for non-commercial use (directly or indirectly)
53
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
54
// +     with our written permission
55
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
56
// +     clearly linked as origin
57
// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
58
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
59
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
60
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
61
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
62
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
63
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
64
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
65
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
66
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
67
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
68
// +  POSSIBILITY OF SUCH DAMAGE.
69
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
70
 
71
#include "main.h"
72
#include "mymath.h"
73
#include "isqrt.h"
74
 
75
unsigned char h,m,s;
76
unsigned int BaroExpandActive = 0;
77
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
78
int TrimNick, TrimRoll;
79
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
80
int Mittelwert_AccNick, Mittelwert_AccRoll;
81
unsigned int NeutralAccX=0, NeutralAccY=0;
82
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
83
int NeutralAccZ = 0;
84
unsigned char ControlHeading = 0;// in 2°
85
long IntegralNick = 0,IntegralNick2 = 0;
86
long IntegralRoll = 0,IntegralRoll2 = 0;
87
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
88
long Integral_Gier = 0;
89
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
90
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
91
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
92
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
93
long SummeNick=0,SummeRoll=0;
94
volatile long Mess_Integral_Hoch = 0;
95
int  KompassValue = 0;
96
int  KompassStartwert = 0;
97
int  KompassRichtung = 0;
98
unsigned int  KompassSignalSchlecht = 500;
99
unsigned char  MAX_GAS,MIN_GAS;
100
unsigned char HoehenReglerAktiv = 0;
101
unsigned char TrichterFlug = 0;
102
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
103
long  ErsatzKompass;
104
int   ErsatzKompassInGrad; // Kompasswert in Grad
105
int   GierGyroFehler = 0;
106
char GyroFaktor,GyroFaktorGier;
107
char IntegralFaktor,IntegralFaktorGier;
108
int  DiffNick,DiffRoll;
109
//int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0;
110
unsigned char Poti[9] = {0,0,0,0,0,0,0,0};
111
volatile unsigned char SenderOkay = 0;
112
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
113
char MotorenEin = 0,StartTrigger = 0;
114
long HoehenWert = 0;
115
long SollHoehe = 0;
116
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
117
//float Ki =  FAKTOR_I;
118
int Ki = 10300 / 33;
119
unsigned char Looping_Nick = 0,Looping_Roll = 0;
120
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
121
 
122
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
123
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
124
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
125
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
126
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
127
unsigned char Parameter_Hoehe_GPS_Z = 64;        // Wert : 0-250
128
unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
129
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
130
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
131
unsigned char Parameter_Gyro_Gier_P = 150;      // Wert : 10-250
132
unsigned char Parameter_Gyro_Gier_I = 150;      // Wert : 10-250
133
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
134
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
135
unsigned char Parameter_UserParam1 = 0;
136
unsigned char Parameter_UserParam2 = 0;
137
unsigned char Parameter_UserParam3 = 0;
138
unsigned char Parameter_UserParam4 = 0;
139
unsigned char Parameter_UserParam5 = 0;
140
unsigned char Parameter_UserParam6 = 0;
141
unsigned char Parameter_UserParam7 = 0;
142
unsigned char Parameter_UserParam8 = 0;
143
unsigned char Parameter_ServoNickControl = 100;
144
unsigned char Parameter_ServoRollControl = 100;
145
unsigned char Parameter_LoopGasLimit = 70;
146
unsigned char Parameter_AchsKopplung1 = 90;
147
unsigned char Parameter_AchsKopplung2 = 65;
148
unsigned char Parameter_CouplingYawCorrection = 64;
149
//unsigned char Parameter_AchsGegenKopplung1 = 0;
150
unsigned char Parameter_DynamicStability = 100;
151
unsigned char Parameter_J16Bitmask;             // for the J16 Output
152
unsigned char Parameter_J16Timing;              // for the J16 Output
153
unsigned char Parameter_J17Bitmask;             // for the J17 Output
154
unsigned char Parameter_J17Timing;              // for the J17 Output
155
unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard
156
unsigned char Parameter_NaviGpsGain;
157
unsigned char Parameter_NaviGpsP;
158
unsigned char Parameter_NaviGpsI;
159
unsigned char Parameter_NaviGpsD;
160
unsigned char Parameter_NaviGpsACC;
161
unsigned char Parameter_NaviOperatingRadius;
162
unsigned char Parameter_NaviWindCorrection;
163
unsigned char Parameter_NaviSpeedCompensation;
164
unsigned char Parameter_ExternalControl;
165
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
166
unsigned char CareFree = 0;
167
 
168
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
169
int MaxStickNick = 0,MaxStickRoll = 0;
170
unsigned int  modell_fliegt = 0;
171
volatile unsigned char FCFlags = 0;
172
long GIER_GRAD_FAKTOR = 1291;
173
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
174
signed int tmp_motorwert[MAX_MOTORS];
175
char VarioCharacter = ' ';
176
 
177
#define LIMIT_MIN(value, min) {if(value <= min) value = min;}
178
#define LIMIT_MAX(value, max) {if(value >= max) value = max;}
179
#define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;}
180
 
181
 
182
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
183
//  Debugwerte zuordnen
184
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
185
void CopyDebugValues(void)
186
{
187
    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
188
    DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
189
    DebugOut.Analog[2] = Mittelwert_AccNick / 4;
190
    DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
191
    DebugOut.Analog[4] = (signed int) AdNeutralGier - AdWertGier;
192
    DebugOut.Analog[5] = HoehenWert/5;
193
    DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az;
194
    DebugOut.Analog[8] = KompassValue;
195
    DebugOut.Analog[9] = UBat;
196
    DebugOut.Analog[10] = SenderOkay;
197
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
198
    DebugOut.Analog[12] = Motor[0].SetPoint;
199
    DebugOut.Analog[13] = Motor[1].SetPoint;
200
    DebugOut.Analog[14] = Motor[2].SetPoint;
201
    DebugOut.Analog[15] = Motor[3].SetPoint;
202
    DebugOut.Analog[20] = ServoNickValue;
203
    DebugOut.Analog[22] = Capacity.ActualCurrent;
204
    DebugOut.Analog[23] = Capacity.UsedCapacity;
205
//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
206
//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
207
        DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
208
    DebugOut.Analog[30] = GPS_Nick;
209
    DebugOut.Analog[31] = GPS_Roll;
210
    if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
211
}
212
 
213
void Piep(unsigned char Anzahl, unsigned int dauer)
214
{
215
 if(MotorenEin) return; //auf keinen Fall im Flug!
216
 while(Anzahl--)
217
 {
218
  beeptime = dauer;
219
  while(beeptime);
220
  Delay_ms(dauer * 2);
221
 }
222
}
223
 
224
//############################################################################
225
// Messwerte beim Ermitteln der Nullage
226
void CalibrierMittelwert(void)
227
//############################################################################
228
{
229
    unsigned char i;
230
    if(PlatinenVersion == 13) SucheGyroOffset();
231
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
232
        ANALOG_OFF;
233
        MesswertNick = AdWertNick;
234
        MesswertRoll = AdWertRoll;
235
        MesswertGier = AdWertGier;
236
        Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
237
        Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
238
   // ADC einschalten
239
    ANALOG_ON;
240
   for(i=0;i<8;i++)
241
    {
242
     int tmp;
243
         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
244
         LIMIT_MIN_MAX(tmp, 0, 255);
245
     if(Poti[i] > tmp) Poti[i]--;  else  if(Poti[i] < tmp) Poti[i]++;
246
        }
247
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
248
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
249
}
250
 
251
//############################################################################
252
//  Nullwerte ermitteln
253
void SetNeutral(unsigned char AccAdjustment)
254
//############################################################################
255
{
256
        unsigned char i;
257
        unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
258
    VersionInfo.HardwareError[0] = 0;
259
    HEF4017R_ON;
260
        NeutralAccX = 0;
261
        NeutralAccY = 0;
262
        NeutralAccZ = 0;
263
 
264
    AdNeutralNick = 0;
265
        AdNeutralRoll = 0;
266
        AdNeutralGier = 0;
267
 
268
    Parameter_AchsKopplung1 = 0;
269
    Parameter_AchsKopplung2 = 0;
270
 
271
    ExpandBaro = 0;
272
 
273
    CalibrierMittelwert();
274
    Delay_ms_Mess(100);
275
 
276
        CalibrierMittelwert();
277
 
278
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
279
     {
280
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
281
     }
282
#define NEUTRAL_FILTER 32
283
    for(i=0; i<NEUTRAL_FILTER; i++)
284
         {
285
          Delay_ms_Mess(10);
286
          gier_neutral += AdWertGier;
287
          nick_neutral += AdWertNick;
288
          roll_neutral += AdWertRoll;
289
         }
290
     AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
291
         AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
292
         AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
293
 
294
     StartNeutralRoll = AdNeutralRoll;
295
     StartNeutralNick = AdNeutralNick;
296
 
297
     if(AccAdjustment)
298
     {
299
            NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
300
            NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
301
            NeutralAccZ = Aktuell_az;
302
 
303
                // Save ACC neutral settings to eeprom
304
                SetParamWord(PID_ACC_NICK, (uint16_t)NeutralAccX);
305
                SetParamWord(PID_ACC_ROLL, (uint16_t)NeutralAccY);
306
                SetParamWord(PID_ACC_TOP,  (uint16_t)NeutralAccZ);
307
    }
308
    else
309
    {
310
                // restore from eeprom
311
                NeutralAccX = (int16_t)GetParamWord(PID_ACC_NICK);
312
                NeutralAccY = (int16_t)GetParamWord(PID_ACC_ROLL);
313
                NeutralAccZ = (int16_t)GetParamWord(PID_ACC_TOP);
314
                // strange settings?
315
                if(((unsigned int) NeutralAccX > 2048) || ((unsigned int) NeutralAccY > 2048) || ((unsigned int) NeutralAccZ > 1024))
316
                {
317
                        printf("\n\rACC not calibrated!\r\n");
318
                        NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
319
                        NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
320
                NeutralAccZ = Aktuell_az;
321
                }
322
    }
323
 
324
    MesswertNick = 0;
325
    MesswertRoll = 0;
326
    MesswertGier = 0;
327
    Delay_ms_Mess(100);
328
    Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick;
329
    Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll;
330
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
331
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
332
    Mess_IntegralNick2 = IntegralNick;
333
    Mess_IntegralRoll2 = IntegralRoll;
334
    Mess_Integral_Gier = 0;
335
    StartLuftdruck = Luftdruck;
336
    VarioMeter = 0;
337
    Mess_Integral_Hoch = 0;
338
    KompassStartwert = KompassValue;
339
    GPS_Neutral();
340
    beeptime = 50;
341
        Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
342
        Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
343
    ExternHoehenValue = 0;
344
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
345
    GierGyroFehler = 0;
346
    SendVersionToNavi = 1;
347
    LED_Init();
348
    FCFlags |= FCFLAG_CALIBRATE;
349
    FromNaviCtrl_Value.Kalman_K = -1;
350
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
351
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
352
 
353
   for(i=0;i<8;i++)
354
    {
355
     Poti[i] =  PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
356
        }
357
    SenderOkay = 100;
358
    if(ServoActive)
359
         {
360
                HEF4017R_ON;
361
                DDRD  |=0x80; // enable J7 -> Servo signal
362
     }
363
 
364
        if((AdNeutralNick < 150 * 16) || (AdNeutralNick > 850 * 16)) { VersionInfo.HardwareError[0] |= DEFEKT_G_NICK; };
365
        if((AdNeutralRoll < 150 * 16) || (AdNeutralRoll > 850 * 16)) { VersionInfo.HardwareError[0] |= DEFEKT_G_ROLL; };
366
        if((AdNeutralGier < 150 * 2)  || (AdNeutralGier > 850 * 2)) { VersionInfo.HardwareError[0] |= DEFEKT_G_GIER; };
367
        if((NeutralAccX < 300) || (NeutralAccX > 750)) { VersionInfo.HardwareError[0] |= DEFEKT_A_NICK; };
368
        if((NeutralAccY < 300) || (NeutralAccY > 750)) { VersionInfo.HardwareError[0] |= DEFEKT_A_ROLL; };
369
        if((NeutralAccZ < 512) || (NeutralAccZ > 850)) { VersionInfo.HardwareError[0] |= DEFEKT_A_Z; };
370
}
371
 
372
 
373
//############################################################################
374
// Bearbeitet die Messwerte
375
void Mittelwert(void)
376
//############################################################################
377
{
378
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
379
        static signed int oldNick, oldRoll, d2Roll, d2Nick;
380
        signed long winkel_nick, winkel_roll;
381
    unsigned char i;
382
        MesswertGier = (signed int) AdNeutralGier - AdWertGier;
383
    MesswertNick = (signed int) AdWertNickFilter / 8;
384
    MesswertRoll = (signed int) AdWertRollFilter / 8;
385
    RohMesswertNick = MesswertNick;
386
    RohMesswertRoll = MesswertRoll;
387
 
388
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
389
        Mittelwert_AccNick = (Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * AdWertAccNick))) / 4L;
390
        Mittelwert_AccRoll = (Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * AdWertAccRoll))) / 4L;
391
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
392
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
393
    NaviAccNick    += AdWertAccNick;
394
    NaviAccRoll    += AdWertAccRoll;
395
    NaviCntAcc++;
396
    IntegralAccZ  += Aktuell_az - NeutralAccZ;
397
 
398
//++++++++++++++++++++++++++++++++++++++++++++++++
399
// ADC einschalten
400
    ANALOG_ON;
401
        AdReady = 0;
402
//++++++++++++++++++++++++++++++++++++++++++++++++
403
 
404
    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
405
        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
406
        else winkel_roll = Mess_IntegralRoll;
407
 
408
    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
409
        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
410
        else winkel_nick = Mess_IntegralNick;
411
 
412
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
413
   Mess_Integral_Gier += MesswertGier;
414
   ErsatzKompass += MesswertGier;
415
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
416
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
417
         {
418
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
419
            tmpl3 *= Parameter_AchsKopplung2; //65
420
            tmpl3 /= 4096L;
421
            tmpl4 = (MesswertNick * winkel_roll) / 2048L;
422
            tmpl4 *= Parameter_AchsKopplung2; //65
423
            tmpl4 /= 4096L;
424
            KopplungsteilNickRoll = tmpl3;
425
            KopplungsteilRollNick = tmpl4;
426
            tmpl4 -= tmpl3;
427
            ErsatzKompass += tmpl4;
428
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
429
 
430
            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
431
            tmpl *= Parameter_AchsKopplung1;  // 90
432
            tmpl /= 4096L;
433
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
434
            tmpl2 *= Parameter_AchsKopplung1;
435
            tmpl2 /= 4096L;
436
            if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
437
            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
438
         }
439
      else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
440
                        TrimRoll = tmpl - tmpl2 / 100L;
441
                        TrimNick = -tmpl2 + tmpl / 100L;
442
// Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++
443
                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
444
                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
445
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
446
            Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
447
            Mess_IntegralRoll +=  MesswertRoll + TrimRoll - LageKorrekturRoll;
448
            if(Mess_IntegralRoll > Umschlag180Roll)
449
            {
450
             Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);
451
             Mess_IntegralRoll2 = Mess_IntegralRoll;
452
            }
453
            if(Mess_IntegralRoll <-Umschlag180Roll)
454
            {
455
             Mess_IntegralRoll =  (Umschlag180Roll - 25000L);
456
             Mess_IntegralRoll2 = Mess_IntegralRoll;
457
            }
458
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
459
            Mess_IntegralNick2 += MesswertNick + TrimNick;
460
            Mess_IntegralNick  += MesswertNick + TrimNick - LageKorrekturNick;
461
            if(Mess_IntegralNick > Umschlag180Nick)
462
             {
463
              Mess_IntegralNick = -(Umschlag180Nick - 25000L);
464
              Mess_IntegralNick2 = Mess_IntegralNick;
465
             }
466
            if(Mess_IntegralNick <-Umschlag180Nick)
467
            {
468
             Mess_IntegralNick =  (Umschlag180Nick - 25000L);
469
             Mess_IntegralNick2 = Mess_IntegralNick;
470
            }
471
 
472
    Integral_Gier  = Mess_Integral_Gier;
473
    IntegralNick = Mess_IntegralNick;
474
    IntegralRoll = Mess_IntegralRoll;
475
    IntegralNick2 = Mess_IntegralNick2;
476
    IntegralRoll2 = Mess_IntegralRoll2;
477
 
478
#define D_LIMIT 128
479
 
480
   MesswertNick = HiResNick / 8;
481
   MesswertRoll = HiResRoll / 8;
482
 
483
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
484
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
485
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
486
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
487
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
488
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
489
 
490
  if(Parameter_Gyro_D)
491
  {
492
   d2Nick = HiResNick - oldNick;
493
   oldNick = (oldNick + HiResNick)/2;
494
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
495
   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
496
   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
497
   d2Roll = HiResRoll - oldRoll;
498
   oldRoll = (oldRoll + HiResRoll)/2;
499
   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
500
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
501
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
502
   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
503
   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
504
  }
505
 
506
 if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
507
 else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
508
 if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
509
 else                    TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
510
 
511
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
512
  {
513
    if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
514
    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
515
    if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);
516
    else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
517
  }
518
  for(i=0;i<8;i++)
519
    {
520
     int tmp;
521
         tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 110;
522
         if(tmp > 255) tmp = 255; else if(tmp < 0) tmp = 0;
523
     if(tmp != Poti[i])
524
          {
525
           Poti[i] += (tmp - Poti[i]) / 8;
526
       if(Poti[i] > tmp) Poti[i]--;
527
           else Poti[i]++;
528
          }
529
        }
530
}
531
 
532
//############################################################################
533
// Senden der Motorwerte per I2C-Bus
534
void SendMotorData(void)
535
//############################################################################
536
{
537
 unsigned char i;
538
    if(!MotorenEin)
539
        {
540
         FCFlags &= ~(FCFLAG_MOTOR_RUN | FCFLAG_FLY);
541
                 for(i=0;i<MAX_MOTORS;i++)
542
                  {
543
                   if(!PC_MotortestActive)  MotorTest[i] = 0;
544
                   Motor[i].SetPoint = MotorTest[i];
545
                   Motor[i].SetPointLowerBits = 0;
546
/*
547
                   Motor[i].SetPoint = MotorTest[i] / 4;
548
                   Motor[i].SetPointLowerBits = MotorTest[i] % 4;
549
*/
550
                  }
551
          if(PC_MotortestActive) PC_MotortestActive--;
552
        }
553
        else FCFlags |= FCFLAG_MOTOR_RUN;
554
    //Start I2C Interrupt Mode
555
    motor_write = 0;
556
    I2C_Start(TWI_STATE_MOTOR_TX);
557
}
558
 
559
 
560
 
561
//############################################################################
562
// Trägt ggf. das Poti als Parameter ein
563
void ParameterZuordnung(void)
564
//############################################################################
565
{
566
 unsigned char tmp;
567
 #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];}
568
 #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);}
569
 
570
 CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
571
 CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
572
 CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
573
 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
574
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
575
 CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);
576
 CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
577
 CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
578
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
579
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
580
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
581
 CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
582
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
583
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
584
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
585
 CHK_POTI(Parameter_Gyro_Gier_P,EE_Parameter.Gyro_Gier_P);
586
 CHK_POTI(Parameter_Gyro_Gier_I,EE_Parameter.Gyro_Gier_I);
587
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor);
588
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1);
589
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2);
590
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3);
591
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4);
592
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5);
593
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6);
594
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7);
595
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8);
596
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl);
597
 CHK_POTI(Parameter_ServoRollControl,EE_Parameter.ServoRollControl);
598
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit);
599
 CHK_POTI(Parameter_AchsKopplung1,EE_Parameter.AchsKopplung1);
600
 CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2);
601
 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
602
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
603
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
604
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
605
 Ki = 10300 / (Parameter_I_Faktor + 1);
606
 MAX_GAS = EE_Parameter.Gas_Max;
607
 MIN_GAS = EE_Parameter.Gas_Min;
608
 
609
 tmp = EE_Parameter.OrientationModeControl;
610
 if(tmp > 50)
611
   {
612
#ifdef SWITCH_LEARNS_CAREFREE
613
    if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
614
#endif
615
        CareFree = 1;
616
    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
617
    if(FromNaviCtrl.CompassValue < 0 && CareFree) VersionInfo.HardwareError[0] |= DEFEKT_CAREFREE_ERR; else VersionInfo.HardwareError[0] &= ~DEFEKT_CAREFREE_ERR;
618
   }
619
   else CareFree = 0;
620
 
621
 if(FromNaviCtrl.CompassValue < 0 && MotorenEin && CareFree && BeepMuster == 0xffff) // ungültiger Kompasswert
622
        {
623
         beeptime = 15000;
624
         BeepMuster = 0xA400;
625
         CareFree = 0;
626
    }
627
 
628
 if(CareFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;}
629
}
630
 
631
//############################################################################
632
//
633
void MotorRegler(void)
634
//############################################################################
635
{
636
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
637
         int GierMischanteil,GasMischanteil;
638
     static long sollGier = 0,tmp_long,tmp_long2;
639
     static long IntegralFehlerNick = 0;
640
     static long IntegralFehlerRoll = 0;
641
         static unsigned int RcLostTimer;
642
         static unsigned char delay_neutral = 0;
643
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
644
         static unsigned char calibration_done = 0;
645
     static char NeueKompassRichtungMerken = 0;
646
     static long ausgleichNick, ausgleichRoll;
647
     int IntegralNickMalFaktor,IntegralRollMalFaktor;
648
         unsigned char i;
649
        Mittelwert();
650
    GRN_ON;
651
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
652
// Gaswert ermitteln
653
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
654
        GasMischanteil = StickGas;
655
    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
656
 
657
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
658
// Empfang schlecht
659
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
660
   if(SenderOkay < 100)
661
        {
662
        if(RcLostTimer) RcLostTimer--;
663
        else
664
         {
665
          MotorenEin = 0;
666
          FCFlags &= ~FCFLAG_NOTLANDUNG;
667
         }
668
        ROT_ON;
669
        if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
670
            {
671
            GasMischanteil = EE_Parameter.NotGas;
672
            FCFlags |= FCFLAG_NOTLANDUNG;
673
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
674
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
675
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
676
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
677
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
678
            }
679
         else MotorenEin = 0;
680
        }
681
        else
682
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
683
// Emfang gut
684
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
685
        if(SenderOkay > 140)
686
            {
687
                        FCFlags &= ~FCFLAG_NOTLANDUNG;
688
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
689
            if(GasMischanteil > 40 && MotorenEin)
690
                {
691
                if(modell_fliegt < 0xffff) modell_fliegt++;
692
                }
693
            if((modell_fliegt < 256))
694
                {
695
                SummeNick = 0;
696
                SummeRoll = 0;
697
                sollGier = 0;
698
                Mess_Integral_Gier = 0;
699
                if(modell_fliegt == 250)
700
                 {
701
                  NeueKompassRichtungMerken = 1;
702
                 }
703
                } else FCFlags |= FCFLAG_FLY;
704
 
705
            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
706
                {
707
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
708
// auf Nullwerte kalibrieren
709
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
710
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
711
                    {
712
                    if(++delay_neutral > 200)  // nicht sofort
713
                        {
714
                        GRN_OFF;
715
                        MotorenEin = 0;
716
                        delay_neutral = 0;
717
                        modell_fliegt = 0;
718
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
719
                        {
720
                         unsigned char setting=1;
721
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
722
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
723
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
724
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
725
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
726
                         SetActiveParamSet(setting);  // aktiven Datensatz merken
727
                        }
728
                         if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
729
                          {
730
                           WinkelOut.CalcState = 1;
731
                           beeptime = 1000;
732
                          }
733
                          else
734
                          {
735
                               ParamSet_ReadFromEEProm(GetActiveParamSet());
736
                               LipoDetection(0);
737
                                                   LIBFC_ReceiverInit(EE_Parameter.Receiver);
738
                           if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
739
                            {
740
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
741
                            }
742
                                                   ServoActive = 0;
743
                           SetNeutral(0);
744
                           calibration_done = 1;
745
                                                   ServoActive = 1;
746
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
747
                           Piep(GetActiveParamSet(),120);
748
                         }
749
                        }
750
                    }
751
                 else
752
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
753
                    {
754
                    if(++delay_neutral > 200)  // nicht sofort
755
                        {
756
                        GRN_OFF;
757
                        MotorenEin = 0;
758
                        delay_neutral = 0;
759
                        modell_fliegt = 0;
760
                        SetNeutral(1);
761
                        calibration_done = 1;
762
                        Piep(GetActiveParamSet(),120);
763
                        }
764
                    }
765
                 else delay_neutral = 0;
766
                }
767
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
768
// Gas ist unten
769
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
770
            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
771
                {
772
                                        // Motoren Starten
773
                                        if(!MotorenEin)
774
                        {
775
                                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
776
                                                {
777
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
778
// Einschalten
779
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
780
                                                        if(++delay_einschalten > 200)
781
                                                        {
782
                                                                delay_einschalten = 0;
783
                                                                if(!VersionInfo.HardwareError[0] && calibration_done)
784
                                                                {
785
                                                                        modell_fliegt = 1;
786
                                                                        MotorenEin = 1;
787
                                                                        sollGier = 0;
788
                                                                        Mess_Integral_Gier = 0;
789
                                                                        Mess_Integral_Gier2 = 0;
790
                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
791
                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
792
                                                                        Mess_IntegralNick2 = IntegralNick;
793
                                                                        Mess_IntegralRoll2 = IntegralRoll;
794
                                                                        SummeNick = 0;
795
                                                                        SummeRoll = 0;
796
                                                                        FCFlags |= FCFLAG_START;
797
                                                                        ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
798
                                                                }
799
                                                                else
800
                                                                {
801
                                                                        beeptime = 1500; // indicate missing calibration
802
                                                                }
803
                                                        }
804
                                                }
805
                                                else delay_einschalten = 0;
806
                                        }
807
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
808
// Auschalten
809
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
810
                                        else // only if motors are running
811
                                        {
812
                                                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
813
                                                {
814
                                                        if(++delay_ausschalten > 200)  // nicht sofort
815
                                                        {
816
                                                                MotorenEin = 0;
817
                                                                delay_ausschalten = 0;
818
                                                                modell_fliegt = 0;
819
                                                        }
820
                                                }
821
                                                else delay_ausschalten = 0;
822
                                        }
823
                }
824
            }
825
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
826
// neue Werte von der Funke
827
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
828
 
829
 if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG))
830
  {
831
        static int stick_nick,stick_roll;
832
    ParameterZuordnung();
833
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
834
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
835
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
836
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
837
 
838
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
839
// CareFree und freie Wahl der vorderen Richtung
840
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
841
signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8};
842
 
843
if(CareFree)
844
 {
845
    signed int nick, roll;
846
        nick = stick_nick / 4;
847
        roll = stick_roll / 4;
848
    StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
849
    StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
850
 }
851
 else
852
 {
853
        FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
854
        FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
855
    StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
856
    StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
857
 }
858
 
859
 
860
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
861
        if(StickGier > 2) StickGier -= 2;       else
862
        if(StickGier < -2) StickGier += 2; else StickGier = 0;
863
 
864
    StickNick -= (GPS_Nick + GPS_Nick2);
865
    StickRoll -= (GPS_Roll + GPS_Roll2);
866
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
867
 
868
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
869
    IntegralFaktor = Parameter_Gyro_I;
870
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
871
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
872
 
873
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
874
//+ Analoge Steuerung per Seriell
875
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
876
   if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
877
    {
878
         StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
879
         StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
880
         StickGier += ExternControl.Gier;
881
     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
882
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
883
    }
884
    if(StickGas < 0) StickGas = 0;
885
 
886
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
887
 
888
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
889
     {
890
      MaxStickNick = abs(StickNick)/STICK_GAIN;
891
      if(MaxStickNick > 100) MaxStickNick = 100;
892
     }
893
     else MaxStickNick--;
894
    if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
895
     {
896
      MaxStickRoll = abs(StickRoll)/STICK_GAIN;
897
      if(MaxStickRoll > 100) MaxStickRoll = 100;
898
     }
899
     else MaxStickRoll--;
900
    if(FCFlags & FCFLAG_NOTLANDUNG)  {MaxStickNick = 0; MaxStickRoll = 0;}
901
 
902
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
903
// Looping?
904
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
905
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
906
  else
907
   {
908
     {
909
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
910
     }
911
   }
912
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
913
   else
914
   {
915
   if(Looping_Rechts) // Hysterese
916
     {
917
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
918
     }
919
   }
920
 
921
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
922
  else
923
   {
924
    if(Looping_Oben)  // Hysterese
925
     {
926
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
927
     }
928
   }
929
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
930
   else
931
   {
932
    if(Looping_Unten) // Hysterese
933
     {
934
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
935
     }
936
   }
937
 
938
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
939
   if(Looping_Oben  || Looping_Unten) {  Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
940
  } // Ende neue Funken-Werte
941
 
942
  if(Looping_Roll || Looping_Nick)
943
   {
944
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
945
        TrichterFlug = 1;
946
   }
947
 
948
 
949
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
950
// Bei Empfangsausfall im Flug
951
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
952
   if(FCFlags & FCFLAG_NOTLANDUNG)
953
    {
954
     StickGier = 0;
955
     StickNick = 0;
956
     StickRoll = 0;
957
     GyroFaktor     = 90;
958
     IntegralFaktor = 120;
959
     GyroFaktorGier     = 90;
960
     IntegralFaktorGier = 120;
961
     Looping_Roll = 0;
962
     Looping_Nick = 0;
963
    }
964
 
965
 
966
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
967
// Integrale auf ACC-Signal abgleichen
968
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
969
#define ABGLEICH_ANZAHL 256L
970
 
971
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
972
 MittelIntegralRoll  += IntegralRoll;
973
 MittelIntegralNick2 += IntegralNick2;
974
 MittelIntegralRoll2 += IntegralRoll2;
975
 
976
 if(Looping_Nick || Looping_Roll)
977
  {
978
    IntegralAccNick = 0;
979
    IntegralAccRoll = 0;
980
    MittelIntegralNick = 0;
981
    MittelIntegralRoll = 0;
982
    MittelIntegralNick2 = 0;
983
    MittelIntegralRoll2 = 0;
984
    Mess_IntegralNick2 = Mess_IntegralNick;
985
    Mess_IntegralRoll2 = Mess_IntegralRoll;
986
    ZaehlMessungen = 0;
987
    LageKorrekturNick = 0;
988
    LageKorrekturRoll = 0;
989
  }
990
 
991
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
992
  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
993
  {
994
   long tmp_long, tmp_long2;
995
   if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
996
     {
997
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
998
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
999
      tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
1000
      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
1001
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
1002
      {
1003
      tmp_long  /= 2;
1004
      tmp_long2 /= 2;
1005
      }
1006
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
1007
      {
1008
      tmp_long  /= 3;
1009
      tmp_long2 /= 3;
1010
      }
1011
      if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1012
      if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1013
      if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1014
      if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1015
     }
1016
     else
1017
     {
1018
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1019
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
1020
      tmp_long /= 16;
1021
      tmp_long2 /= 16;
1022
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
1023
      {
1024
      tmp_long  /= 3;
1025
      tmp_long2 /= 3;
1026
      }
1027
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
1028
      {
1029
      tmp_long  /= 3;
1030
      tmp_long2 /= 3;
1031
      }
1032
 
1033
#define AUSGLEICH  32
1034
      if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
1035
      if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
1036
      if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
1037
      if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
1038
     }
1039
 
1040
   Mess_IntegralNick -= tmp_long;
1041
   Mess_IntegralRoll -= tmp_long2;
1042
  }
1043
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1044
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1045
 {
1046
  static int cnt = 0;
1047
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1048
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1049
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
1050
  {
1051
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1052
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1053
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1054
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
1055
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
1056
#define MAX_I 0
1057
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
1058
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
1059
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
1060
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
1061
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
1062
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
1063
 
1064
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
1065
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
1066
 
1067
   if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
1068
    {
1069
     LageKorrekturNick /= 2;
1070
     LageKorrekturRoll /= 2;
1071
    }
1072
 
1073
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1074
// Gyro-Drift ermitteln
1075
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1076
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
1077
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
1078
    tmp_long  = IntegralNick2 - IntegralNick;
1079
    tmp_long2 = IntegralRoll2 - IntegralRoll;
1080
 
1081
    IntegralFehlerNick = tmp_long;
1082
    IntegralFehlerRoll = tmp_long2;
1083
    Mess_IntegralNick2 -= IntegralFehlerNick;
1084
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
1085
 
1086
  if(EE_Parameter.Driftkomp)
1087
   {
1088
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; }
1089
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; }
1090
   }
1091
    GierGyroFehler = 0;
1092
 
1093
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL / 2)
1094
#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4
1095
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
1096
#define BEWEGUNGS_LIMIT 20000
1097
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
1098
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1099
        if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
1100
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
1101
        {
1102
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
1103
         {
1104
           if(last_n_p)
1105
           {
1106
            cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
1107
            ausgleichNick = IntegralFehlerNick / 8;
1108
            if(ausgleichNick > 5000) ausgleichNick = 5000;
1109
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1110
           }
1111
           else last_n_p = 1;
1112
         } else  last_n_p = 0;
1113
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
1114
         {
1115
           if(last_n_n)
1116
            {
1117
             cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
1118
             ausgleichNick = IntegralFehlerNick / 8;
1119
             if(ausgleichNick < -5000) ausgleichNick = -5000;
1120
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1121
            }
1122
           else last_n_n = 1;
1123
         } else  last_n_n = 0;
1124
        }
1125
        else
1126
        {
1127
         cnt = 0;
1128
         KompassSignalSchlecht = 1000;
1129
        }
1130
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1131
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
1132
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
1133
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
1134
 
1135
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
1136
        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
1137
        if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
1138
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
1139
        {
1140
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
1141
         {
1142
           if(last_r_p)
1143
           {
1144
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
1145
            ausgleichRoll = IntegralFehlerRoll / 8;
1146
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
1147
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1148
           }
1149
           else last_r_p = 1;
1150
         } else  last_r_p = 0;
1151
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
1152
         {
1153
           if(last_r_n)
1154
           {
1155
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
1156
            ausgleichRoll = IntegralFehlerRoll / 8;
1157
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
1158
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1159
           }
1160
           else last_r_n = 1;
1161
         } else  last_r_n = 0;
1162
        } else
1163
        {
1164
         cnt = 0;
1165
         KompassSignalSchlecht = 1000;
1166
        }
1167
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1168
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
1169
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
1170
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
1171
  }
1172
  else
1173
  {
1174
   LageKorrekturRoll = 0;
1175
   LageKorrekturNick = 0;
1176
   TrichterFlug = 0;
1177
  }
1178
 
1179
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
1180
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
1181
   MittelIntegralNick_Alt = MittelIntegralNick;
1182
   MittelIntegralRoll_Alt = MittelIntegralRoll;
1183
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
1184
    IntegralAccNick = 0;
1185
    IntegralAccRoll = 0;
1186
    IntegralAccZ = 0;
1187
    MittelIntegralNick = 0;
1188
    MittelIntegralRoll = 0;
1189
    MittelIntegralNick2 = 0;
1190
    MittelIntegralRoll2 = 0;
1191
    ZaehlMessungen = 0;
1192
 } //  ZaehlMessungen >= ABGLEICH_ANZAHL
1193
 
1194
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1195
//  Gieren
1196
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1197
    if(abs(StickGier) > 15) // war 35
1198
     {
1199
      KompassSignalSchlecht = 1000;
1200
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
1201
       {
1202
         NeueKompassRichtungMerken = 1;
1203
        };
1204
     }
1205
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1206
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
1207
    sollGier = tmp_int;
1208
    Mess_Integral_Gier -= tmp_int;
1209
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
1210
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1211
 
1212
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1213
//  Kompass
1214
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1215
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1216
     {
1217
       int w,v,r,fehler,korrektur;
1218
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1219
       v = abs(IntegralRoll /512);
1220
       if(v > w) w = v; // grösste Neigung ermitteln
1221
       korrektur = w / 8 + 2;
1222
       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1223
           //fehler += MesswertGier / 12;
1224
 
1225
       if(!KompassSignalSchlecht && w < 25)
1226
        {
1227
        GierGyroFehler += fehler;
1228
        if(NeueKompassRichtungMerken)
1229
         {
1230
                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
1231
          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
1232
          NeueKompassRichtungMerken = 0;
1233
         }
1234
        }
1235
       ErsatzKompass += (fehler * 16) / korrektur;
1236
       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
1237
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
1238
       if(w >= 0)
1239
        {
1240
          if(!KompassSignalSchlecht)
1241
          {
1242
           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
1243
           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
1244
           v = (r * w) / v;  // nach Kompass ausrichten
1245
           w = 3 * Parameter_KompassWirkung;
1246
           if(v > w) v = w; // Begrenzen
1247
           else
1248
           if(v < -w) v = -w;
1249
           Mess_Integral_Gier += v;
1250
          }
1251
          if(KompassSignalSchlecht) KompassSignalSchlecht--;
1252
        }
1253
        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
1254
     }
1255
 
1256
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1257
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1258
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1259
  if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};
1260
 
1261
  if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
1262
  if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
1263
 
1264
#define TRIM_MAX 200
1265
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
1266
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
1267
 
1268
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
1269
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1270
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1271
 
1272
    // Maximalwerte abfangen
1273
//    #define MAX_SENSOR  (4096*STICK_GAIN)
1274
    #define MAX_SENSOR  (4096)
1275
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1276
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1277
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1278
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1279
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
1280
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1281
 
1282
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1283
// Höhenregelung
1284
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1285
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1286
  if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
1287
  GasMischanteil *= STICK_GAIN;
1288
        // if height control is activated
1289
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick))  // Höhenregelung
1290
        {
1291
                #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
1292
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
1293
 
1294
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1295
#define OPA_OFFSET_STEP 15
1296
#else
1297
#define OPA_OFFSET_STEP 10
1298
#endif
1299
                int HCGas, HeightDeviation = 0,GasReduction = 0;
1300
                static int HeightTrimming = 0;  // rate for change of height setpoint
1301
                static int FilterHCGas = 0;
1302
                static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023;
1303
                static unsigned long HoverGasFilter = 0;
1304
                static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0;
1305
            int CosAttitude;    // for projection of hoover gas
1306
 
1307
                // get the current hooverpoint
1308
                DebugOut.Analog[21] = HoverGas;
1309
 
1310
        // Expand the measurement
1311
                // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs
1312
          if(!BaroExpandActive)
1313
                   {
1314
                        if(MessLuftdruck > 920)
1315
                        {   // increase offset
1316
             if(OCR0A < (255 - OPA_OFFSET_STEP))
1317
                           {
1318
                                ExpandBaro -= 1;
1319
                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down
1320
                                beeptime = 300;
1321
                                BaroExpandActive = 350;
1322
                           }
1323
                           else
1324
                           {
1325
                            BaroAtLowerLimit = 1;
1326
               }
1327
                        }
1328
                        // measurement of air pressure close to lower limit and
1329
                        else
1330
                        if(MessLuftdruck < 100)
1331
                        {   // decrease offset
1332
                          if(OCR0A > OPA_OFFSET_STEP)
1333
                           {
1334
                                ExpandBaro += 1;
1335
                                OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up
1336
                                beeptime = 300;
1337
                                BaroExpandActive = 350;
1338
                           }
1339
                           else
1340
                           {
1341
                            BaroAtUpperLimit = 1;
1342
               }
1343
                        }
1344
                        else
1345
                        {
1346
                            BaroAtUpperLimit = 0;
1347
                                BaroAtLowerLimit = 0;
1348
                        }
1349
                   }
1350
                   else // delay, because of expanding the Baro-Range
1351
                   {
1352
                    // now clear the D-values
1353
                          SummenHoehe = HoehenWert * SM_FILTER;
1354
                          VarioMeter = 0;
1355
                          BaroExpandActive--;
1356
                   }
1357
 
1358
                // if height control is activated by an rc channel
1359
        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1360
                {       // check if parameter is less than activation threshold
1361
                        if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position
1362
                        {   //height control not active
1363
                                if(!delay--)
1364
                                {
1365
                                        HoehenReglerAktiv = 0; // disable height control
1366
                                        SollHoehe = HoehenWert;  // update SetPoint with current reading
1367
                                        delay = 1;
1368
                                }
1369
                        }
1370
                        else
1371
                        {       //height control is activated
1372
                                HoehenReglerAktiv = 1; // enable height control
1373
                                delay = 200;
1374
                        }
1375
                }
1376
                else // no switchable height control
1377
                {
1378
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
1379
                        HoehenReglerAktiv = 1;
1380
                }
1381
 
1382
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
1383
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
1384
                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
1385
                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
1386
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
1387
                CosAttitude = c_cos_8192(CosAttitude);  // cos of actual attitude
1388
                VarioCharacter = ' ';
1389
                if(HoehenReglerAktiv && !(FCFlags & FCFLAG_NOTLANDUNG))
1390
                {
1391
                        #define HEIGHT_TRIM_UP          0x01
1392
                        #define HEIGHT_TRIM_DOWN        0x02
1393
                        static unsigned char HeightTrimmingFlag = 0x00;
1394
 
1395
                        #define HEIGHT_CONTROL_STICKTHRESHOLD 15
1396
                // Holger original version
1397
                // start of height control algorithm
1398
                // the height control is only an attenuation of the actual gas stick.
1399
                // I.e. it will work only if the gas stick is higher than the hover gas
1400
                // and the hover height will be allways larger than height setpoint.
1401
        if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER))  // Regler wird über Schalter gesteuert)
1402
              {  // old version
1403
                        HCGas = GasMischanteil; // take current stick gas as neutral point for the height control
1404
                        HeightTrimming = 0;
1405
          }
1406
                  else
1407
                  {
1408
                // alternative height control
1409
                // PD-Control with respect to hoover point
1410
                // the thrust loss out of horizontal attitude is compensated
1411
                // the setpoint will be fine adjusted with the gas stick position
1412
                        if(FCFlags & FCFLAG_FLY) // trim setpoint only when flying
1413
                        {   // gas stick is above hoover point
1414
                                if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit)
1415
                                {
1416
                                        if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN)
1417
                                        {
1418
                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN;
1419
                                                SollHoehe = HoehenWert; // update setpoint to current heigth
1420
                                        }
1421
                                        HeightTrimmingFlag |= HEIGHT_TRIM_UP;
1422
                                        HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD));
1423
                                        VarioCharacter = '+';
1424
                                } // gas stick is below hoover point
1425
                                else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit )
1426
                                {
1427
                                        if(HeightTrimmingFlag & HEIGHT_TRIM_UP)
1428
                                        {
1429
                                                HeightTrimmingFlag &= ~HEIGHT_TRIM_UP;
1430
                                                SollHoehe = HoehenWert; // update setpoint to current heigth
1431
                                        }
1432
                                        HeightTrimmingFlag |= HEIGHT_TRIM_DOWN;
1433
                                        HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD));
1434
                                        VarioCharacter = '-';
1435
                                }
1436
                                else // Gas Stick in Hover Range
1437
                                {
1438
                                        if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN))
1439
                                        {
1440
                                                HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN);
1441
                                                HeightTrimming = 0;
1442
                                                SollHoehe = HoehenWert; // update setpoint to current height
1443
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1444
                                                if(!StartTrigger && HoehenWert > 50)
1445
                                                {
1446
                                                 StartTrigger = 1;
1447
                                                }
1448
                                        }
1449
                                        VarioCharacter = '=';
1450
                                }
1451
                                // Trim height set point
1452
                                if(abs(HeightTrimming) > 512)
1453
                                {
1454
                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
1455
                                        HeightTrimming = 0;
1456
                    LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
1457
                                        if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
1458
                                        //update hoover gas stick value when setpoint is shifted
1459
                       if(!EE_Parameter.Hoehe_StickNeutralPoint)
1460
                       {
1461
                           StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
1462
                           StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
1463
                           if(StickGasHover < 70) StickGasHover = 70;
1464
                           else if(StickGasHover > 150) StickGasHover = 150;
1465
                       }
1466
                                }
1467
              if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
1468
                        } //if FCFlags & MKFCFLAG_FLY
1469
                        else
1470
                        {
1471
                         SollHoehe = HoehenWert - 400;
1472
                         if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
1473
                         else StickGasHover = 120;
1474
                         HoverGas = GasMischanteil;
1475
                         }
1476
                        HCGas = HoverGas;      // take hover gas (neutral point)
1477
                   }
1478
         if(HoehenWert > SollHoehe || !(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT))
1479
                 {
1480
                        // from this point the Heigth Control Algorithm is identical for both versions
1481
                        if(BaroExpandActive) // baro range expanding active
1482
                        {
1483
                                HCGas = HoverGas; // hover while expanding baro adc range
1484
                                HeightDeviation = 0;
1485
                        } // EOF // baro range expanding active
1486
                        else // valid data from air pressure sensor
1487
                        {
1488
                                // ------------------------- P-Part ----------------------------
1489
                                tmp_long = (HoehenWert - SollHoehe); // positive when too high
1490
                                LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
1491
                                HeightDeviation = (int)(tmp_long); // positive when too high
1492
                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
1493
                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
1494
                                GasReduction = tmp_long;
1495
                                // ------------------------- D-Part 1: Vario Meter ----------------------------
1496
                                tmp_int = VarioMeter / 8;
1497
                                LIMIT_MIN_MAX(tmp_int, -127, 128);     
1498
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1499
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
1500
                                if(HeightTrimmingFlag)  tmp_int /= 4; // reduce d-part while trimming setpoint
1501
                                else
1502
                                if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
1503
                                GasReduction += tmp_int;
1504
                        } // EOF no baro range expanding
1505
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
1506
            if(Parameter_Hoehe_ACC_Wirkung)
1507
                         {
1508
                          tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
1509
                          LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
1510
                          GasReduction += tmp_long;
1511
                         }
1512
                        // ------------------------ D-Part 3: GpsZ  ----------------------------------
1513
                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
1514
            LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
1515
                        GasReduction += tmp_int;
1516
            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
1517
                        // ------------------------                  ----------------------------------
1518
                        HCGas -= GasReduction;
1519
                        // limit deviation from hoover point within the target region
1520
                        if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
1521
                        {
1522
                         unsigned int tmp;
1523
                         tmp = abs(HeightDeviation);
1524
                         if(tmp <= 60)
1525
                         {
1526
                          LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point
1527
                         }
1528
                         else
1529
                         {  
1530
                                tmp = (tmp - 60) / 32;
1531
                                if(tmp > 15) tmp = 15;
1532
                           if(HeightDeviation > 0)
1533
                                {
1534
                                tmp = (HoverGasMin * (16 - tmp)) / 16;
1535
                                LIMIT_MIN_MAX(HCGas, tmp, HoverGasMax); // limit gas around the hoover point
1536
                                }
1537
                                else
1538
                                {
1539
                                tmp = (HoverGasMax * (tmp + 16)) / 16;
1540
                                LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point
1541
                                }
1542
                          }
1543
                        }
1544
                        // strech control output by inverse attitude projection 1/cos
1545
            // + 1/cos(angle)  ++++++++++++++++++++++++++
1546
                        tmp_long2 = (int32_t)HCGas;
1547
                        tmp_long2 *= 8192L;
1548
                        tmp_long2 /= CosAttitude;
1549
                        HCGas = (int16_t)tmp_long2;
1550
                        // update height control gas averaging
1551
                        FilterHCGas = (FilterHCGas * (HC_GAS_AVERAGE - 1) + HCGas) / HC_GAS_AVERAGE;
1552
                        // limit height control gas pd-control output
1553
                        LIMIT_MIN_MAX(FilterHCGas, EE_Parameter.Hoehe_MinGas * STICK_GAIN, (MAX_GAS - 20) * STICK_GAIN);
1554
                        // set GasMischanteil to HeightControlGasFilter
1555
            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
1556
                        {  // old version
1557
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1558
                                GasMischanteil = FilterHCGas;
1559
                        }
1560
                        else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
1561
                  }
1562
                }// EOF height control active
1563
                else // HC not active
1564
                {
1565
                        //update hoover gas stick value when HC is not active
1566
                        if(!EE_Parameter.Hoehe_StickNeutralPoint)
1567
                        {
1568
                                StickGasHover = HoverGas/STICK_GAIN; // rescale back to stick value
1569
                                StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning;
1570
                        }
1571
                        else StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
1572
            LIMIT_MIN_MAX(StickGasHover, 70, 150); // reserve some range for trim up and down
1573
                        FilterHCGas = GasMischanteil;
1574
                }
1575
 
1576
                // Hover gas estimation by averaging gas control output on small z-velocities
1577
                // this is done only if height contol option is selected in global config and aircraft is flying
1578
                if((FCFlags & FCFLAG_FLY))// && !(FCFlags & FCFLAG_NOTLANDUNG))
1579
                {
1580
                        if(HoverGasFilter == 0 || StartTrigger == 1)  HoverGasFilter = HOVER_GAS_AVERAGE * (unsigned long)(GasMischanteil); // init estimation
1581
                        if(StartTrigger == 1) StartTrigger = 2;
1582
                                tmp_long2 = (int32_t)GasMischanteil; // take current thrust
1583
                                tmp_long2 *= CosAttitude;            // apply attitude projection
1584
                                tmp_long2 /= 8192;
1585
                                // average vertical projected thrust
1586
                            if(modell_fliegt < 4000) // the first 8 seconds
1587
                                {   // reduce the time constant of averaging by factor of 4 to get much faster a stable value
1588
                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/16L);
1589
                                        HoverGasFilter += 16L * tmp_long2;
1590
                                }
1591
                if(modell_fliegt < 8000) // the first 16 seconds
1592
                                {   // reduce the time constant of averaging by factor of 2 to get much faster a stable value
1593
                                        HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L);
1594
                                        HoverGasFilter += 4L * tmp_long2;
1595
                                }
1596
                          else //later
1597
                          if(abs(VarioMeter) < 100) // only on small vertical speed
1598
                                {
1599
                                        HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE;
1600
                                        HoverGasFilter += tmp_long2;
1601
                                }
1602
                                HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE);
1603
                                if(EE_Parameter.Hoehe_HoverBand)
1604
                                {
1605
                                        int16_t band;
1606
                                        band = HoverGas / EE_Parameter.Hoehe_HoverBand; // the higher the parameter the smaller the range
1607
                                        HoverGasMin = HoverGas - band;
1608
                                        HoverGasMax = HoverGas + band;
1609
                                }
1610
                                else
1611
                                {       // no limit
1612
                                        HoverGasMin = 0;
1613
                                        HoverGasMax = 1023;
1614
                                }
1615
                }
1616
                 else
1617
                  {
1618
                   StartTrigger = 0;
1619
                   HoverGasFilter = 0;
1620
                   HoverGas = 0;
1621
                  }
1622
        }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL
1623
        // limit gas to parameter setting
1624
  LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN);
1625
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1626
 
1627
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1628
// all BL-Ctrl connected?
1629
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1630
  if(MissingMotor || Capacity.MinOfMaxPWM != 255)
1631
  if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
1632
   {
1633
    modell_fliegt = 1;
1634
        GasMischanteil = (MIN_GAS + 10) * STICK_GAIN;
1635
   }
1636
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1637
// + Mischer und PI-Regler
1638
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1639
  DebugOut.Analog[7] = GasMischanteil;
1640
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1641
// Gier-Anteil
1642
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1643
    GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
1644
#define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
1645
   if(GasMischanteil > MIN_GIERGAS)
1646
    {
1647
     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
1648
     if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
1649
    }
1650
    else
1651
    {
1652
     if(GierMischanteil > (MIN_GIERGAS / 2))  GierMischanteil = MIN_GIERGAS / 2;
1653
     if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
1654
    }
1655
    tmp_int = MAX_GAS*STICK_GAIN;
1656
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
1657
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
1658
 
1659
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1660
// Nick-Achse
1661
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1662
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1663
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1664
    else  SummeNick += DiffNick; // I-Anteil bei HH
1665
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
1666
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1667
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick
1668
    // Motor Vorn
1669
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1670
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
1671
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
1672
 
1673
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1674
// Roll-Achse
1675
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1676
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
1677
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
1678
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1679
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1680
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1681
    pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki;   // PI-Regler für Roll
1682
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1683
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
1684
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
1685
 
1686
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1687
// Universal Mixer
1688
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1689
        for(i=0; i<MAX_MOTORS; i++)
1690
        {
1691
                signed int tmp_int;
1692
                if(Mixer.Motor[i][0] > 0)
1693
                {
1694
                        // Gas
1695
                        if(Mixer.Motor[i][0] == 64) tmp_int = GasMischanteil; else tmp_int =  ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
1696
                        // Nick
1697
                        if(Mixer.Motor[i][1] == 64) tmp_int += pd_ergebnis_nick;
1698
                        else if(Mixer.Motor[i][1] == -64) tmp_int -= pd_ergebnis_nick;
1699
                        else tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
1700
            // Roll
1701
                        if(Mixer.Motor[i][2] == 64) tmp_int += pd_ergebnis_roll;
1702
                        else if(Mixer.Motor[i][2] == -64) tmp_int -= pd_ergebnis_roll;
1703
                        else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
1704
            // Gier
1705
                        if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
1706
                        else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
1707
                        else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
1708
 
1709
 
1710
/******************************************************************************************************************
1711
Arthur P: the original code allowed the motor value to drop to 0 or negative values
1712
straight off, i.e. could amplify an intended decrease excessively while upregulation
1713
is dampened. The modification would still allow immediate drop below intended value
1714
but would dampen this. This would still allow for airbraking of the prop to have effect
1715
but it might lead to less sudden excessive drops in rpm with only gradual recovery.
1716
090807 Arthur P: Due to problems with uart.c which still refers to user parameter 1 and 2 and
1717
possible timing issues with the shutter interval load, removed the shutter interval functions
1718
and switched to use of userparam6 for the motor smoothing.
1719
091114 Inserted modification into 0.76g source code.
1720
20100804 Modified v.0.80d code where motorsmoothing is no longer a separate function.
1721
Downsmoothing either uses default v.0.7x+ 150% downstep (user para 7 == 0),
1722
50% downstep (user para 7 == 1 or 2), or downsteps of x% (userpara7 ==):
1723
66.6% (3), 75% (4), 80% (5), 90% (10), 95% (20), 97.5% (40), 98% (50), 99% (100).
1724
******************************************************************************************************************/
1725
 
1726
                        if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
1727
//                      else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
1728
                        else
1729
                        {
1730
                                if(Parameter_UserParam7 < 2)
1731
                                { // Original function
1732
                                        tmp_int = 2 * tmp_int - tmp_motorwert[i];
1733
                                }
1734
                                else
1735
                                {
1736
                                        // If userpara7 >= 2 then allow >= 50% of the intended step down to rapidly reach the intended value.
1737
                                        tmp_int = tmp_int + ((tmp_motorwert[i] - tmp_int)/Parameter_UserParam7);
1738
                                }
1739
                        }
1740
 
1741
                        LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4);
1742
                        Motor[i].SetPoint = tmp_int / 4;
1743
                        Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total)
1744
            tmp_motorwert[i] = tmp_int;
1745
                }
1746
                else
1747
                {
1748
                        Motor[i].SetPoint = 0;
1749
                        Motor[i].SetPointLowerBits = 0;
1750
                }
1751
        }
1752
}