Subversion Repositories FlightCtrl

Rev

Rev 1226 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
/*#######################################################################################
2
Flight Control
3
#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + Copyright (c) 04.2007 Holger Buss
6
// + Nur für den privaten Gebrauch
7
// + www.MikroKopter.com
8
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051 killagreg 9
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
10
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
11
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
12
// + bzgl. der Nutzungsbedingungen aufzunehmen.
1 ingob 13
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
14
// + Verkauf von Luftbildaufnahmen, usw.
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051 killagreg 16
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
1 ingob 17
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
18
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
19
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
20
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
21
// + eindeutig als Ursprung verlinkt werden
22
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
23
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
24
// + Benutzung auf eigene Gefahr
25
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051 killagreg 27
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
1 ingob 28
// + mit unserer Zustimmung zulässig
29
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
30
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1051 killagreg 32
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
1 ingob 33
// + this list of conditions and the following disclaimer.
34
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
35
// +     from this software without specific prior written permission.
1051 killagreg 36
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
1 ingob 37
// +     for non-commercial use (directly or indirectly)
1051 killagreg 38
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
1 ingob 39
// +     with our written permission
1051 killagreg 40
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
41
// +     clearly linked as origin
831 hbuss 42
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
1 ingob 43
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
44
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
45
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
46
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
47
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
48
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
49
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
492 hbuss 50
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
1 ingob 51
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
1051 killagreg 52
// +  POSSIBILITY OF SUCH DAMAGE.
1 ingob 53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
54
 
55
#include "main.h"
395 hbuss 56
#include "eeprom.c"
1286 MartinR 57
#include "mymath.c" // MartinR: für Killagreg´s Erweiterung
58
#include "mymath.h" // MartinR: für Killagreg´s Erweiterung
1 ingob 59
 
60
unsigned char h,m,s;
173 holgerb 61
volatile unsigned int I2CTimeout = 100;
1153 hbuss 62
int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll;
1166 hbuss 63
int TrimNick, TrimRoll;
1051 killagreg 64
int AdNeutralGierBias;
927 hbuss 65
int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;
66
int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;
805 hbuss 67
int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;
1286 MartinR 68
//volatile float NeutralAccZ = 0; // MartinR  : so war es
69
volatile int NeutralAccZ = 0; // MartinR geändert
70
 
71
volatile int NeutralAccZ2 = 0; // MartinR: für HP-Filter 
72
signed int h_p,h_d,h_delta,Ziel_d;  // MartinR: P,D Anteil beim eigenen Höhenregler
73
signed int SchwebeGas; // MartinR: für neuen Höhenregler
74
 
1 ingob 75
unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;
693 hbuss 76
long IntegralNick = 0,IntegralNick2 = 0;
77
long IntegralRoll = 0,IntegralRoll2 = 0;
78
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
79
long Integral_Gier = 0;
80
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
81
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
82
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
83
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
1 ingob 84
volatile long Mess_Integral_Hoch = 0;
1153 hbuss 85
int  KompassValue = 0;
86
int  KompassStartwert = 0;
87
int  KompassRichtung = 0;
693 hbuss 88
unsigned int  KompassSignalSchlecht = 500;
855 hbuss 89
unsigned char  MAX_GAS,MIN_GAS;
1 ingob 90
unsigned char Notlandung = 0;
91
unsigned char HoehenReglerAktiv = 0;
880 hbuss 92
unsigned char TrichterFlug = 0;
1286 MartinR 93
 
94
unsigned char delay_Hoehenregler = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
95
signed int delay_SchwebeGas = 0; // MartinR: zur Begrenzung der Sinkgeschwindigkeit
96
 
395 hbuss 97
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
693 hbuss 98
long  ErsatzKompass;
99
int   ErsatzKompassInGrad; // Kompasswert in Grad
100
int   GierGyroFehler = 0;
1211 hbuss 101
char GyroFaktor,GyroFaktorGier;
102
char IntegralFaktor,IntegralFaktorGier;
1153 hbuss 103
int  DiffNick,DiffRoll;
1051 killagreg 104
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
1 ingob 105
volatile unsigned char SenderOkay = 0;
595 hbuss 106
int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0;
1 ingob 107
char MotorenEin = 0;
1286 MartinR 108
//int HoehenWert = 0; //MartinR: so war es
109
long HoehenWert = 0; //MartinR: geändert
110
 
111
long HoehenWertalt = 0; //MartinR: Test
112
unsigned char iHoehe; // MartinR: Zähler für HoeheD
113
signed long HoehenWertSumme = 0; // MartinR: für neuen Höhenregler D-Anteil
114
 
115
//int SollHoehe = 0;//MartinR: so war es
116
long SollHoehe = 0;//MartinR: geändert
117
 
498 hbuss 118
int LageKorrekturRoll = 0,LageKorrekturNick = 0;
1153 hbuss 119
//float Ki =  FAKTOR_I;
120
int Ki = 10300 / 33;
1286 MartinR 121
 
122
int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter
123
 
395 hbuss 124
unsigned char Looping_Nick = 0,Looping_Roll = 0;
125
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
1 ingob 126
 
127
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
128
unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250
129
unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32
130
unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250
131
unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250
1111 hbuss 132
unsigned char Parameter_Gyro_D = 8;             // Wert : 0-250
173 holgerb 133
unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250
1 ingob 134
unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250
135
unsigned char Parameter_Gier_P = 2;             // Wert : 1-20
136
unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20
137
unsigned char Parameter_UserParam1 = 0;
138
unsigned char Parameter_UserParam2 = 0;
139
unsigned char Parameter_UserParam3 = 0;
140
unsigned char Parameter_UserParam4 = 0;
499 hbuss 141
unsigned char Parameter_UserParam5 = 0;
142
unsigned char Parameter_UserParam6 = 0;
143
unsigned char Parameter_UserParam7 = 0;
144
unsigned char Parameter_UserParam8 = 0;
1 ingob 145
unsigned char Parameter_ServoNickControl = 100;
173 holgerb 146
unsigned char Parameter_LoopGasLimit = 70;
1120 hbuss 147
unsigned char Parameter_AchsKopplung1 = 90;
148
unsigned char Parameter_AchsKopplung2 = 65;
149
unsigned char Parameter_CouplingYawCorrection = 64;
150
//unsigned char Parameter_AchsGegenKopplung1 = 0;
499 hbuss 151
unsigned char Parameter_DynamicStability = 100;
921 hbuss 152
unsigned char Parameter_J16Bitmask;             // for the J16 Output
153
unsigned char Parameter_J16Timing;              // for the J16 Output
154
unsigned char Parameter_J17Bitmask;             // for the J17 Output
155
unsigned char Parameter_J17Timing;              // for the J17 Output
156
unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard
1051 killagreg 157
unsigned char Parameter_NaviGpsGain;
158
unsigned char Parameter_NaviGpsP;
159
unsigned char Parameter_NaviGpsI;
160
unsigned char Parameter_NaviGpsD;
161
unsigned char Parameter_NaviGpsACC;
993 hbuss 162
unsigned char Parameter_NaviOperatingRadius;
163
unsigned char Parameter_NaviWindCorrection;
164
unsigned char Parameter_NaviSpeedCompensation;
921 hbuss 165
unsigned char Parameter_ExternalControl;
1 ingob 166
struct mk_param_struct EE_Parameter;
492 hbuss 167
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
1286 MartinR 168
//int MaxStickNick = 0,MaxStickRoll = 0;MartinR: so war es
169
 
170
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
171
 
871 hbuss 172
unsigned int  modell_fliegt = 0;
1209 hbuss 173
volatile unsigned char MikroKopterFlags = 0;
1121 hbuss 174
long GIER_GRAD_FAKTOR = 1291;
1153 hbuss 175
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
1209 hbuss 176
unsigned char RequiredMotors = 4;
177
unsigned char Motor[MAX_MOTORS];
178
signed int tmp_motorwert[MAX_MOTORS];
1155 hbuss 179
 
1166 hbuss 180
int MotorSmoothing(int neu, int alt)
181
{
182
 int motor;
183
 if(neu > alt) motor = (1*(int)alt + neu) / 2;
1286 MartinR 184
 //else   motor = neu - (alt - neu)*1; // MartinR: so war es
185
 else   motor = neu; // MartinR: Entsprechend Vorschlag von MartinW geändert
1166 hbuss 186
//if(Poti2 < 20)  return(neu);
187
 return(motor);
188
}
189
 
190
 
1 ingob 191
void Piep(unsigned char Anzahl)
192
{
193
 while(Anzahl--)
194
 {
195
  if(MotorenEin) return; //auf keinen Fall im Flug!
196
  beeptime = 100;
197
  Delay_ms(250);
198
 }
199
}
200
 
201
//############################################################################
202
//  Nullwerte ermitteln
203
void SetNeutral(void)
204
//############################################################################
205
{
1111 hbuss 206
 unsigned char i;
207
 unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;
1171 hbuss 208
    ServoActive = 0; HEF4017R_ON;
1051 killagreg 209
        NeutralAccX = 0;
1 ingob 210
        NeutralAccY = 0;
211
        NeutralAccZ = 0;
1286 MartinR 212
        NeutralAccZ2 = 0; // MartinR
213
 
1051 killagreg 214
    AdNeutralNick = 0;
215
        AdNeutralRoll = 0;
1 ingob 216
        AdNeutralGier = 0;
927 hbuss 217
    AdNeutralGierBias = 0;
395 hbuss 218
    Parameter_AchsKopplung1 = 0;
1120 hbuss 219
    Parameter_AchsKopplung2 = 0;
1036 hbuss 220
    ExpandBaro = 0;
1051 killagreg 221
    CalibrierMittelwert();
395 hbuss 222
    Delay_ms_Mess(100);
1 ingob 223
        CalibrierMittelwert();
224
    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
1051 killagreg 225
     {
1 ingob 226
      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
227
     }
1166 hbuss 228
#define NEUTRAL_FILTER 32
229
    for(i=0; i<NEUTRAL_FILTER; i++)
1111 hbuss 230
         {
231
          Delay_ms_Mess(10);
1216 killagreg 232
          gier_neutral += AdWertGier;
1166 hbuss 233
          nick_neutral += AdWertNick;
234
          roll_neutral += AdWertRoll;
1111 hbuss 235
         }
1173 hbuss 236
     AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
237
         AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);
238
         AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);
1111 hbuss 239
     AdNeutralGierBias = AdNeutralGier;
401 hbuss 240
     StartNeutralRoll = AdNeutralRoll;
241
     StartNeutralNick = AdNeutralNick;
1051 killagreg 242
    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)
513 hbuss 243
    {
1171 hbuss 244
      NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);
245
          NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);
513 hbuss 246
          NeutralAccZ = Aktuell_az;
1286 MartinR 247
          NeutralAccZ2 = NeutralAccZ; // MartinR
513 hbuss 248
    }
1051 killagreg 249
    else
513 hbuss 250
    {
251
      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);
252
          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);
253
          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);
254
    }
1051 killagreg 255
 
1 ingob 256
    MesswertNick = 0;
257
    MesswertRoll = 0;
258
    MesswertGier = 0;
1111 hbuss 259
    Delay_ms_Mess(100);
1174 hbuss 260
    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
261
    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
1173 hbuss 262
    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
263
    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
264
    Mess_IntegralNick2 = IntegralNick;
265
    Mess_IntegralRoll2 = IntegralRoll;
266
    Mess_Integral_Gier = 0;
1 ingob 267
    StartLuftdruck = Luftdruck;
268
    HoeheD = 0;
269
    Mess_Integral_Hoch = 0;
270
    KompassStartwert = KompassValue;
271
    GPS_Neutral();
1051 killagreg 272
    beeptime = 50;
882 hbuss 273
        Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L;
274
        Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L;
492 hbuss 275
    ExternHoehenValue = 0;
693 hbuss 276
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
277
    GierGyroFehler = 0;
723 hbuss 278
    SendVersionToNavi = 1;
921 hbuss 279
    LED_Init();
280
    MikroKopterFlags |= FLAG_CALIBRATE;
992 hbuss 281
    FromNaviCtrl_Value.Kalman_K = -1;
1173 hbuss 282
    FromNaviCtrl_Value.Kalman_MaxDrift = 0;
992 hbuss 283
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
1172 hbuss 284
    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;
285
    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;
286
    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;
287
    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;
1216 killagreg 288
    ServoActive = 1;
1171 hbuss 289
    SenderOkay = 100;
1 ingob 290
}
291
 
292
//############################################################################
395 hbuss 293
// Bearbeitet die Messwerte
1 ingob 294
void Mittelwert(void)
295
//############################################################################
1051 killagreg 296
{
1111 hbuss 297
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
298
        static signed int oldNick, oldRoll, d2Roll, d2Nick;
1286 MartinR 299
        static signed int oldNick2, oldRoll2 ; //MartinR : für geänderte d2* Ermittlung
1153 hbuss 300
        signed long winkel_nick, winkel_roll;
1171 hbuss 301
 
1111 hbuss 302
        MesswertGier = (signed int) AdNeutralGier - AdWertGier;
303
//    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;
1171 hbuss 304
    MesswertNick = (signed int) AdWertNickFilter / 8;
305
    MesswertRoll = (signed int) AdWertRollFilter / 8;
1153 hbuss 306
    RohMesswertNick = MesswertNick;
307
    RohMesswertRoll = MesswertRoll;
1120 hbuss 308
//DebugOut.Analog[21] = MesswertNick;
309
//DebugOut.Analog[22] = MesswertRoll;
1121 hbuss 310
//DebugOut.Analog[22] = Mess_Integral_Gier;
1216 killagreg 311
//DebugOut.Analog[21] = MesswertNick;
312
//DebugOut.Analog[22] = MesswertRoll;
1166 hbuss 313
 
395 hbuss 314
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
1166 hbuss 315
        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;
316
        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;
317
        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;
395 hbuss 318
    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;
319
    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;
805 hbuss 320
    NaviAccNick    += AdWertAccNick;
321
    NaviAccRoll    += AdWertAccRoll;
322
    NaviCntAcc++;
1286 MartinR 323
        //IntegralAccZ  += Aktuell_az - NeutralAccZ; // MartinR: so war es
324
        IntegralAccZ += AdWertAccHoch;// MartinR: AdWertAccHoch = Aktuell_az - NeutralAccZ; in analog.c
1153 hbuss 325
 
1155 hbuss 326
//++++++++++++++++++++++++++++++++++++++++++++++++
327
// ADC einschalten
1286 MartinR 328
        //ANALOG_ON; // MartinR : so war es
329
        // ANALOG_START; // MartinR verschoben zu timer0
1155 hbuss 330
        AdReady = 0;
331
//++++++++++++++++++++++++++++++++++++++++++++++++
332
 
1216 killagreg 333
    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;
334
        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;
1153 hbuss 335
        else winkel_roll = Mess_IntegralRoll;
336
 
1216 killagreg 337
    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;
338
        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;
1153 hbuss 339
        else winkel_nick = Mess_IntegralNick;
340
 
1120 hbuss 341
// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++
1153 hbuss 342
   Mess_Integral_Gier += MesswertGier;
343
   ErsatzKompass += MesswertGier;
395 hbuss 344
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
1286 MartinR 345
 
346
if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0;  // MartinR: zusätzlich
347
 
348
          //if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))  // MartinR : so war es
349
          if(!Looping_Nick && !Looping_Roll && IntegralFaktor && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) // MartinR: zusätzlich "&& IntegralFaktor"
395 hbuss 350
         {
1153 hbuss 351
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
1120 hbuss 352
            tmpl3 *= Parameter_AchsKopplung2; //65
1111 hbuss 353
            tmpl3 /= 4096L;
1153 hbuss 354
            tmpl4 = (MesswertNick * winkel_roll) / 2048L;
1216 killagreg 355
            tmpl4 *= Parameter_AchsKopplung2; //65
1111 hbuss 356
            tmpl4 /= 4096L;
1153 hbuss 357
            KopplungsteilNickRoll = tmpl3;
358
            KopplungsteilRollNick = tmpl4;
1111 hbuss 359
            tmpl4 -= tmpl3;
360
            ErsatzKompass += tmpl4;
1166 hbuss 361
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
1111 hbuss 362
 
1153 hbuss 363
            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
1111 hbuss 364
            tmpl *= Parameter_AchsKopplung1;  // 90
880 hbuss 365
            tmpl /= 4096L;
1153 hbuss 366
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
395 hbuss 367
            tmpl2 *= Parameter_AchsKopplung1;
880 hbuss 368
            tmpl2 /= 4096L;
1225 hbuss 369
            if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
1153 hbuss 370
            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
395 hbuss 371
         }
1166 hbuss 372
      else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;
1111 hbuss 373
 
1166 hbuss 374
TrimRoll = tmpl - tmpl2 / 100L;
375
TrimNick = -tmpl2 + tmpl / 100L;
376
 
1111 hbuss 377
// Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++
378
                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
379
                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
395 hbuss 380
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
1166 hbuss 381
            Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
382
            Mess_IntegralRoll +=  MesswertRoll + TrimRoll - LageKorrekturRoll;
1051 killagreg 383
            if(Mess_IntegralRoll > Umschlag180Roll)
395 hbuss 384
            {
882 hbuss 385
             Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);
395 hbuss 386
             Mess_IntegralRoll2 = Mess_IntegralRoll;
1051 killagreg 387
            }
395 hbuss 388
            if(Mess_IntegralRoll <-Umschlag180Roll)
389
            {
882 hbuss 390
             Mess_IntegralRoll =  (Umschlag180Roll - 25000L);
395 hbuss 391
             Mess_IntegralRoll2 = Mess_IntegralRoll;
1051 killagreg 392
            }
395 hbuss 393
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
1166 hbuss 394
            Mess_IntegralNick2 += MesswertNick + TrimNick;
395
            Mess_IntegralNick  += MesswertNick + TrimNick - LageKorrekturNick;
1051 killagreg 396
            if(Mess_IntegralNick > Umschlag180Nick)
882 hbuss 397
             {
398
              Mess_IntegralNick = -(Umschlag180Nick - 25000L);
399
              Mess_IntegralNick2 = Mess_IntegralNick;
1051 killagreg 400
             }
401
            if(Mess_IntegralNick <-Umschlag180Nick)
395 hbuss 402
            {
882 hbuss 403
             Mess_IntegralNick =  (Umschlag180Nick - 25000L);
395 hbuss 404
             Mess_IntegralNick2 = Mess_IntegralNick;
1051 killagreg 405
            }
1111 hbuss 406
 
1 ingob 407
    Integral_Gier  = Mess_Integral_Gier;
408
    IntegralNick = Mess_IntegralNick;
409
    IntegralRoll = Mess_IntegralRoll;
1051 killagreg 410
    IntegralNick2 = Mess_IntegralNick2;
1 ingob 411
    IntegralRoll2 = Mess_IntegralRoll2;
412
 
1166 hbuss 413
#define D_LIMIT 128
414
 
1286 MartinR 415
   //MesswertNick = HiResNick / 8;// MartinR : so war es
416
   //MesswertRoll = HiResRoll / 8;// MartinR : so war es
417
   MesswertNick = AdWertNickFilter / 8;// MartinR : anstelle HiResNick : AdWertNickFilter
418
   MesswertRoll = AdWertRollFilter / 8;// MartinR : ist im vergleich zu HiRes.. eimal mehr gemittelt
1166 hbuss 419
 
1286 MartinR 420
 
421
 // MartinR : so war es Anfang
422
        /*
1167 hbuss 423
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
424
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
425
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
426
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
427
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
428
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
1286 MartinR 429
 
430
   // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
431
        */
432
        // MartinR : so war es Ende
433
 
434
         // MartinR : Neu Anfang
435
        if(PlatinenVersion == 10)  
436
        {
437
        if(AdWertNick > 1010) MesswertNick = +600;  
438
        if(AdWertNick > 1017) MesswertNick = +800;
439
        if(AdWertNick < 15)   MesswertNick = -600;  
440
        if(AdWertNick <  7)   MesswertNick = -800;
441
        if(AdWertRoll > 1010) MesswertRoll = +600;  
442
        if(AdWertRoll > 1017) MesswertRoll = +800;
443
        if(AdWertRoll < 15)   MesswertRoll = -600;  
444
        if(AdWertRoll <  7)   MesswertRoll = -800;
445
        }
446
        else  
447
        {  
448
        if(AdWertNick > 2000) MesswertNick = +1200;  
449
        if(AdWertNick > 2015) MesswertNick = +1600;
450
        if(AdWertNick < 15)   MesswertNick = -1200;  
451
        if(AdWertNick <  7)   MesswertNick = -1600;
452
        if(AdWertRoll > 2000) MesswertRoll = +1200;  
453
        if(AdWertRoll > 2015) MesswertRoll = +1600;
454
        if(AdWertRoll < 15)   MesswertRoll = -1200;  
455
        if(AdWertRoll <  7)   MesswertRoll = -1600;
456
        }
457
 // MartinR : Neu Ende
1167 hbuss 458
 
1216 killagreg 459
  if(Parameter_Gyro_D)
1111 hbuss 460
  {
1286 MartinR 461
  /* MartinR: so war es Anfang
462
 
1166 hbuss 463
   d2Nick = HiResNick - oldNick;
464
   oldNick = (oldNick + HiResNick)/2;
1111 hbuss 465
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
466
   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
1166 hbuss 467
   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
468
   d2Roll = HiResRoll - oldRoll;
469
   oldRoll = (oldRoll + HiResRoll)/2;
1111 hbuss 470
   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
471
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
1166 hbuss 472
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
473
   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
474
   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
1286 MartinR 475
 
476
   */ //MartinR: so war es Ende
477
 
478
   // MartinR :neu Anfang
479
   d2Nick = MesswertNick - oldNick2;
480
   oldNick2 = oldNick;
481
   oldNick = MesswertNick;
482
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
483
   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
484
   if(d2Nick > 0) d2Nick --;
485
   if(d2Nick < 0) d2Nick ++;
486
 
487
   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
488
 
489
   d2Roll = MesswertRoll - oldRoll2;
490
   oldRoll2 = oldRoll;
491
   oldRoll = MesswertRoll;
492
   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
493
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;  
494
   if(d2Roll > 0) d2Roll --;
495
   if(d2Roll < 0) d2Roll ++;
496
 
497
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
498
   // MartinR :neu Ende
499
 
1216 killagreg 500
  }
1111 hbuss 501
 
1166 hbuss 502
 if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
503
 else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
504
 if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
505
 else                    TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
1153 hbuss 506
 
507
  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)
508
  {
509
    if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);
510
    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);
511
    if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);
512
    else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);
513
  }
514
 
1 ingob 515
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
516
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
517
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
518
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
519
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
520
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
521
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
522
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
523
}
524
 
525
//############################################################################
526
// Messwerte beim Ermitteln der Nullage
527
void CalibrierMittelwert(void)
528
//############################################################################
1051 killagreg 529
{
1021 hbuss 530
    if(PlatinenVersion == 13) SucheGyroOffset();
1 ingob 531
    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern
532
        ANALOG_OFF;
395 hbuss 533
        MesswertNick = AdWertNick;
534
        MesswertRoll = AdWertRoll;
535
        MesswertGier = AdWertGier;
536
        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;
537
        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;
538
        Mittelwert_AccHoch = (long)AdWertAccHoch;
539
   // ADC einschalten
1051 killagreg 540
    ANALOG_ON;
1 ingob 541
    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;
542
    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;
543
    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;
544
    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;
545
    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;
546
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
547
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
548
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
395 hbuss 549
 
550
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
720 ingob 551
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
1 ingob 552
}
553
 
554
//############################################################################
555
// Senden der Motorwerte per I2C-Bus
556
void SendMotorData(void)
557
//############################################################################
1051 killagreg 558
{
1209 hbuss 559
 unsigned char i;
921 hbuss 560
    if(!MotorenEin)
1 ingob 561
        {
1212 hbuss 562
         MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);
1216 killagreg 563
                 for(i=0;i<MAX_MOTORS;i++)
564
                  {
565
                   if(!PC_MotortestActive)  MotorTest[i] = 0;
1212 hbuss 566
                   Motor[i] = MotorTest[i];
1216 killagreg 567
                  }
1212 hbuss 568
          if(PC_MotortestActive) PC_MotortestActive--;
1216 killagreg 569
        }
1212 hbuss 570
        else MikroKopterFlags |= FLAG_MOTOR_RUN;
1111 hbuss 571
 
1212 hbuss 572
    DebugOut.Analog[12] = Motor[0];
573
    DebugOut.Analog[13] = Motor[1];
574
    DebugOut.Analog[14] = Motor[3];
575
    DebugOut.Analog[15] = Motor[2];
1 ingob 576
 
577
    //Start I2C Interrupt Mode
578
    twi_state = 0;
579
    motor = 0;
1051 killagreg 580
    i2c_start();
1 ingob 581
}
582
 
583
 
584
 
585
//############################################################################
586
// Trägt ggf. das Poti als Parameter ein
1051 killagreg 587
void ParameterZuordnung(void)
1 ingob 588
//############################################################################
589
{
921 hbuss 590
 #define CHK_POTI_MM(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}
591
 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; }
1 ingob 592
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);
921 hbuss 593
 CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);
594
 CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);
1 ingob 595
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);
596
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);
921 hbuss 597
 CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);
1 ingob 598
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);
1120 hbuss 599
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255);
1051 killagreg 600
 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);
601
 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);
602
 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);
603
 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);
604
 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);
605
 CHK_POTI(Parameter_UserParam5,EE_Parameter.UserParam5,0,255);
606
 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);
607
 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);
608
 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,0,255);
1 ingob 609
 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);
173 holgerb 610
 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);
395 hbuss 611
 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);
1120 hbuss 612
 CHK_POTI(Parameter_AchsKopplung2,    EE_Parameter.AchsKopplung2,0,255);
613
 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);
614
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
499 hbuss 615
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);
921 hbuss 616
 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);
617
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
618
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);
1153 hbuss 619
 Ki = 10300 / (Parameter_I_Faktor + 1);
1286 MartinR 620
 
621
 if(Parameter_UserParam1 > 140) KiHH = 10300 / (Parameter_UserParam2 + 1); else  KiHH = Ki; // MartinR : für HH über Schalter
622
 Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
623
 if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode 
624
  // 0 = AID; 100 = free; 200 = coming home //so war es 
625
  // 0 = free; 100 = AID; 200 = coming home //neu 
626
 
627
 
1 ingob 628
 MAX_GAS = EE_Parameter.Gas_Max;
629
 MIN_GAS = EE_Parameter.Gas_Min;
630
}
631
 
632
 
819 hbuss 633
 
1 ingob 634
//############################################################################
635
//
636
void MotorRegler(void)
637
//############################################################################
638
{
1209 hbuss 639
         int pd_ergebnis_nick,pd_ergebnis_roll,h,tmp_int;
1 ingob 640
         int GierMischanteil,GasMischanteil;
641
     static long SummeNick=0,SummeRoll=0;
1286 MartinR 642
 
643
         static long SummeNickHH=0,SummeRollHH=0; // MartinR: Für ACC-HH Umschaltung
644
 
1 ingob 645
     static long sollGier = 0,tmp_long,tmp_long2;
395 hbuss 646
     static long IntegralFehlerNick = 0;
647
     static long IntegralFehlerRoll = 0;
1 ingob 648
         static unsigned int RcLostTimer;
649
         static unsigned char delay_neutral = 0;
650
         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;
651
     static int hoehenregler = 0;
652
     static char TimerWerteausgabe = 0;
653
     static char NeueKompassRichtungMerken = 0;
395 hbuss 654
     static long ausgleichNick, ausgleichRoll;
1153 hbuss 655
     int IntegralNickMalFaktor,IntegralRollMalFaktor;
1209 hbuss 656
         unsigned char i;
1051 killagreg 657
        Mittelwert();
658
 
1 ingob 659
    GRN_ON;
1051 killagreg 660
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 661
// Gaswert ermitteln
1051 killagreg 662
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
663
        GasMischanteil = StickGas;
831 hbuss 664
    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;
1051 killagreg 665
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693 hbuss 666
// Empfang schlecht
1051 killagreg 667
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 668
   if(SenderOkay < 100)
669
        {
1051 killagreg 670
        if(!PcZugriff)
173 holgerb 671
         {
1051 killagreg 672
           if(BeepMuster == 0xffff)
673
            {
173 holgerb 674
             beeptime = 15000;
675
             BeepMuster = 0x0c00;
1051 killagreg 676
            }
173 holgerb 677
         }
1051 killagreg 678
        if(RcLostTimer) RcLostTimer--;
679
        else
1 ingob 680
         {
681
          MotorenEin = 0;
682
          Notlandung = 0;
1051 killagreg 683
         }
1 ingob 684
        ROT_ON;
693 hbuss 685
        if(modell_fliegt > 1000)  // wahrscheinlich in der Luft --> langsam absenken
1 ingob 686
            {
687
            GasMischanteil = EE_Parameter.NotGas;
688
            Notlandung = 1;
744 hbuss 689
            PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
1051 killagreg 690
            PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
1 ingob 691
            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;
692
            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;
693
            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;
173 holgerb 694
            }
1 ingob 695
         else MotorenEin = 0;
696
        }
1051 killagreg 697
        else
698
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 699
// Emfang gut
1051 killagreg 700
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 701
        if(SenderOkay > 140)
702
            {
703
            Notlandung = 0;
704
            RcLostTimer = EE_Parameter.NotGasZeit * 50;
921 hbuss 705
            if(GasMischanteil > 40 && MotorenEin)
1 ingob 706
                {
707
                if(modell_fliegt < 0xffff) modell_fliegt++;
708
                }
871 hbuss 709
            if((modell_fliegt < 256))
1 ingob 710
                {
711
                SummeNick = 0;
712
                SummeRoll = 0;
1051 killagreg 713
                if(modell_fliegt == 250)
918 hbuss 714
                 {
1051 killagreg 715
                  NeueKompassRichtungMerken = 1;
918 hbuss 716
                  sollGier = 0;
1051 killagreg 717
                  Mess_Integral_Gier = 0;
927 hbuss 718
//                  Mess_Integral_Gier2 = 0;
1051 killagreg 719
                 }
921 hbuss 720
                } else MikroKopterFlags |= FLAG_FLY;
1051 killagreg 721
 
595 hbuss 722
            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)
1 ingob 723
                {
1051 killagreg 724
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 725
// auf Nullwerte kalibrieren
1051 killagreg 726
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 727
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte
728
                    {
729
                    if(++delay_neutral > 200)  // nicht sofort
730
                        {
731
                        GRN_OFF;
732
                        MotorenEin = 0;
733
                        delay_neutral = 0;
734
                        modell_fliegt = 0;
735
                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)
736
                        {
304 ingob 737
                         unsigned char setting=1;
1 ingob 738
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;
739
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;
740
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;
741
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;
742
                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;
1063 killagreg 743
                         SetActiveParamSetNumber(setting);  // aktiven Datensatz merken
1 ingob 744
                        }
820 hbuss 745
//                        else
1051 killagreg 746
                         if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) < 30 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -70)
747
                          {
819 hbuss 748
                           WinkelOut.CalcState = 1;
749
                           beeptime = 1000;
750
                          }
751
                          else
1 ingob 752
                          {
819 hbuss 753
                               ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);
754
                           if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?
755
                            {
1 ingob 756
                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();
1051 killagreg 757
                            }
819 hbuss 758
                           SetNeutral();
759
                           Piep(GetActiveParamSetNumber());
760
                         }
1051 killagreg 761
                        }
1 ingob 762
                    }
1051 killagreg 763
                 else
513 hbuss 764
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
765
                    {
766
                    if(++delay_neutral > 200)  // nicht sofort
767
                        {
768
                        GRN_OFF;
769
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],0xff); // Werte löschen
770
                        MotorenEin = 0;
771
                        delay_neutral = 0;
772
                        modell_fliegt = 0;
773
                        SetNeutral();
774
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK],NeutralAccX / 256); // ACC-NeutralWerte speichern
775
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1],NeutralAccX % 256); // ACC-NeutralWerte speichern
776
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL],NeutralAccY / 256);
777
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1],NeutralAccY % 256);
778
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z],(int)NeutralAccZ / 256);
779
                        eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1],(int)NeutralAccZ % 256);
780
                        Piep(GetActiveParamSetNumber());
1051 killagreg 781
                        }
513 hbuss 782
                    }
1 ingob 783
                 else delay_neutral = 0;
784
                }
1051 killagreg 785
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 786
// Gas ist unten
1051 killagreg 787
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
595 hbuss 788
            if(PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] < 35-120)
1 ingob 789
                {
790
                // Starten
791
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)
792
                    {
1051 killagreg 793
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 794
// Einschalten
1051 killagreg 795
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 796
                    if(++delay_einschalten > 200)
797
                        {
798
                        delay_einschalten = 200;
799
                        modell_fliegt = 1;
800
                        MotorenEin = 1;
801
                        sollGier = 0;
1051 killagreg 802
                        Mess_Integral_Gier = 0;
1 ingob 803
                        Mess_Integral_Gier2 = 0;
1173 hbuss 804
                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
805
                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
1 ingob 806
                        Mess_IntegralNick2 = IntegralNick;
807
                        Mess_IntegralRoll2 = IntegralRoll;
808
                        SummeNick = 0;
809
                        SummeRoll = 0;
1286 MartinR 810
                                                SchwebeGas = 0; // MartinR: für neuen Höhenregler
921 hbuss 811
                        MikroKopterFlags |= FLAG_START;
1051 killagreg 812
                        }
813
                    }
1 ingob 814
                    else delay_einschalten = 0;
815
                //Auf Neutralwerte setzen
1051 killagreg 816
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 817
// Auschalten
1051 killagreg 818
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 819
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)
820
                    {
821
                    if(++delay_ausschalten > 200)  // nicht sofort
822
                        {
823
                        MotorenEin = 0;
824
                        delay_ausschalten = 200;
825
                        modell_fliegt = 0;
1051 killagreg 826
                        }
1 ingob 827
                    }
828
                else delay_ausschalten = 0;
829
                }
830
            }
831
 
1051 killagreg 832
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 833
// neue Werte von der Funke
1051 killagreg 834
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216 killagreg 835
 
1051 killagreg 836
 if(!NewPpmData-- || Notlandung)
1 ingob 837
  {
604 hbuss 838
        static int stick_nick,stick_roll;
1 ingob 839
    ParameterZuordnung();
1286 MartinR 840
 
841
        // MartinR: original:   
842
        /*
1051 killagreg 843
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
723 hbuss 844
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
805 hbuss 845
    StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
604 hbuss 846
 
723 hbuss 847
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
848
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
805 hbuss 849
    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
1286 MartinR 850
 
851
        */
852
        // MartinR: geändert Anfang
853
        if(Parameter_UserParam1 > 140)  // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
854
                {
855
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4;
856
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ;
857
                }
858
 
859
         else
860
 
861
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
862
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
863
 
864
 
865
         if(IntegralFaktor)  
866
                {
867
                stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
868
                stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
869
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
870
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
871
 
872
                StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam
873
                StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
874
                }
875
        else            // wenn HH , MartinR
876
                {
877
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
878
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
879
                StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
880
                StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
881
                }
882
 
883
// MartinR: geändert Ende
723 hbuss 884
 
1286 MartinR 885
 
1 ingob 886
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
1120 hbuss 887
        if(StickGier > 2) StickGier -= 2;       else
888
        if(StickGier < -2) StickGier += 2; else StickGier = 0;
889
 
595 hbuss 890
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
1153 hbuss 891
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
892
    IntegralFaktor = Parameter_Gyro_I;
1211 hbuss 893
    GyroFaktorGier     = (Parameter_Gyro_P + 10.0);
894
    IntegralFaktorGier = Parameter_Gyro_I;
1 ingob 895
 
595 hbuss 896
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
897
//+ Analoge Steuerung per Seriell
898
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1286 MartinR 899
// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen??
900
 
921 hbuss 901
   if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
595 hbuss 902
    {
903
         StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
904
         StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
905
         StickGier += ExternControl.Gier;
906
     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
907
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
908
    }
855 hbuss 909
    if(StickGas < 0) StickGas = 0;
492 hbuss 910
 
1286 MartinR 911
        //if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0; // MartinR: Original
912
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 140)) IntegralFaktor = 0;  // MartinR
913
 
1 ingob 914
    if(GyroFaktor < 0) GyroFaktor = 0;
915
    if(IntegralFaktor < 0) IntegralFaktor = 0;
723 hbuss 916
 
1051 killagreg 917
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
928 hbuss 918
     {
1051 killagreg 919
      MaxStickNick = abs(StickNick)/STICK_GAIN;
928 hbuss 920
      if(MaxStickNick > 100) MaxStickNick = 100;
1051 killagreg 921
     }
928 hbuss 922
     else MaxStickNick--;
1051 killagreg 923
    if(abs(StickRoll/STICK_GAIN) > MaxStickRoll)
928 hbuss 924
     {
1051 killagreg 925
      MaxStickRoll = abs(StickRoll)/STICK_GAIN;
928 hbuss 926
      if(MaxStickRoll > 100) MaxStickRoll = 100;
1051 killagreg 927
     }
928 hbuss 928
     else MaxStickRoll--;
723 hbuss 929
    if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}
930
 
1051 killagreg 931
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173 holgerb 932
// Looping?
1051 killagreg 933
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
993 hbuss 934
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_LINKS)  Looping_Links = 1;
1051 killagreg 935
  else
936
   {
395 hbuss 937
     {
1051 killagreg 938
      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;
939
     }
940
   }
993 hbuss 941
  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;
1051 killagreg 942
   else
395 hbuss 943
   {
944
   if(Looping_Rechts) // Hysterese
945
     {
946
      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;
947
     }
1051 killagreg 948
   }
173 holgerb 949
 
993 hbuss 950
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_OBEN) Looping_Oben = 1;
1051 killagreg 951
  else
952
   {
395 hbuss 953
    if(Looping_Oben)  // Hysterese
954
     {
1051 killagreg 955
      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;
956
     }
957
   }
993 hbuss 958
  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.BitConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;
1051 killagreg 959
   else
395 hbuss 960
   {
961
    if(Looping_Unten) // Hysterese
962
     {
963
      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;
964
     }
1051 killagreg 965
   }
395 hbuss 966
 
967
   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;
1153 hbuss 968
   if(Looping_Oben  || Looping_Unten) {  Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;
395 hbuss 969
  } // Ende neue Funken-Werte
970
 
971
  if(Looping_Roll || Looping_Nick)
972
   {
173 holgerb 973
    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;
1153 hbuss 974
        TrichterFlug = 1;
173 holgerb 975
   }
976
 
1051 killagreg 977
 
978
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
979
// Bei Empfangsausfall im Flug
980
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 981
   if(Notlandung)
982
    {
983
     StickGier = 0;
984
     StickNick = 0;
985
     StickRoll = 0;
1211 hbuss 986
     GyroFaktor     = 90;
987
     IntegralFaktor = 120;
988
     GyroFaktorGier     = 90;
989
     IntegralFaktorGier = 120;
173 holgerb 990
     Looping_Roll = 0;
991
     Looping_Nick = 0;
1051 killagreg 992
    }
395 hbuss 993
 
994
 
1051 killagreg 995
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 996
// Integrale auf ACC-Signal abgleichen
1051 killagreg 997
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395 hbuss 998
#define ABGLEICH_ANZAHL 256L
999
 
1000
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1001
 MittelIntegralRoll  += IntegralRoll;
1002
 MittelIntegralNick2 += IntegralNick2;
1003
 MittelIntegralRoll2 += IntegralRoll2;
1004
 
1286 MartinR 1005
 //if(Looping_Nick || Looping_Roll) // MartinR: so war es
1006
 if(Looping_Nick || Looping_Roll || !IntegralFaktor)  // MartinR: "|| !IntegralFaktor" hinzugefügt
395 hbuss 1007
  {
1008
    IntegralAccNick = 0;
1009
    IntegralAccRoll = 0;
1010
    MittelIntegralNick = 0;
1011
    MittelIntegralRoll = 0;
1012
    MittelIntegralNick2 = 0;
1013
    MittelIntegralRoll2 = 0;
1286 MartinR 1014
 
1015
        IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
1016
    IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 
1017
    Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0    
1018
    Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0    
1019
    Mess_Integral_Gier = 0;     // MartinR: im HH-Modus alle unbenutzten Integratoren = 0       
1020
    Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
1021
 
395 hbuss 1022
    Mess_IntegralNick2 = Mess_IntegralNick;
1023
    Mess_IntegralRoll2 = Mess_IntegralRoll;
1024
    ZaehlMessungen = 0;
498 hbuss 1025
    LageKorrekturNick = 0;
1026
    LageKorrekturRoll = 0;
395 hbuss 1027
  }
1028
 
1051 killagreg 1029
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1286 MartinR 1030
  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt 
469 hbuss 1031
  {
1032
   long tmp_long, tmp_long2;
1171 hbuss 1033
   if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
992 hbuss 1034
     {
1035
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051 killagreg 1036
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
1037
      tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
1038
      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);
992 hbuss 1039
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
1040
      {
1041
      tmp_long  /= 2;
1042
      tmp_long2 /= 2;
1043
      }
1044
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
1045
      {
1046
      tmp_long  /= 3;
1047
      tmp_long2 /= 3;
1048
      }
1049
      if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1050
      if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1051
      if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
1052
      if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
1053
     }
1051 killagreg 1054
     else
992 hbuss 1055
     {
1056
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
1051 killagreg 1057
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
992 hbuss 1058
      tmp_long /= 16;
1059
      tmp_long2 /= 16;
1060
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
1061
      {
1062
      tmp_long  /= 3;
1063
      tmp_long2 /= 3;
1216 killagreg 1064
      }
1065
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
992 hbuss 1066
      {
1067
      tmp_long  /= 3;
1068
      tmp_long2 /= 3;
1069
      }
1155 hbuss 1070
 
1071
#define AUSGLEICH  32
992 hbuss 1072
      if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
1073
      if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
1074
      if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
1075
      if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
1076
     }
1166 hbuss 1077
 
1078
//if(Poti2 > 20) { tmp_long = 0; tmp_long2 = 0;}
1111 hbuss 1079
   Mess_IntegralNick -= tmp_long;
1080
   Mess_IntegralRoll -= tmp_long2;
469 hbuss 1081
  }
1051 killagreg 1082
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395 hbuss 1083
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1084
 {
1085
  static int cnt = 0;
1086
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1087
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1286 MartinR 1088
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
173 holgerb 1089
  {
395 hbuss 1090
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1091
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1092
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1093
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
1094
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
492 hbuss 1095
#define MAX_I 0//(Poti2/10)
395 hbuss 1096
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
1097
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
1098
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
1051 killagreg 1099
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++
1100
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
395 hbuss 1101
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
614 hbuss 1102
 
1103
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
498 hbuss 1104
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
395 hbuss 1105
 
992 hbuss 1106
   if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
614 hbuss 1107
    {
1108
     LageKorrekturNick /= 2;
720 ingob 1109
     LageKorrekturRoll /= 2;
614 hbuss 1110
    }
498 hbuss 1111
 
1051 killagreg 1112
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395 hbuss 1113
// Gyro-Drift ermitteln
1051 killagreg 1114
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
395 hbuss 1115
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
1116
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
1051 killagreg 1117
    tmp_long  = IntegralNick2 - IntegralNick;
1118
    tmp_long2 = IntegralRoll2 - IntegralRoll;
395 hbuss 1119
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
1120
 
1121
    IntegralFehlerNick = tmp_long;
1122
    IntegralFehlerRoll = tmp_long2;
1123
    Mess_IntegralNick2 -= IntegralFehlerNick;
1124
    Mess_IntegralRoll2 -= IntegralFehlerRoll;
1125
 
1126
//    IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2;
1127
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
1111 hbuss 1128
  if(EE_Parameter.Driftkomp)
1129
   {
927 hbuss 1130
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; }
1131
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; }
1111 hbuss 1132
   }
1153 hbuss 1133
//DebugOut.Analog[22] = MittelIntegralRoll / 26;
921 hbuss 1134
//DebugOut.Analog[24] = GierGyroFehler;
693 hbuss 1135
    GierGyroFehler = 0;
720 ingob 1136
 
1137
 
1138
/*DebugOut.Analog[17] = IntegralAccNick / 26;
395 hbuss 1139
DebugOut.Analog[18] = IntegralAccRoll / 26;
1140
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
1141
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
720 ingob 1142
*/
1143
//DebugOut.Analog[21] = MittelIntegralNick / 26;
1144
//MittelIntegralRoll = MittelIntegralRoll;
469 hbuss 1145
//DebugOut.Analog[28] = ausgleichNick;
720 ingob 1146
/*
395 hbuss 1147
DebugOut.Analog[29] = ausgleichRoll;
498 hbuss 1148
DebugOut.Analog[30] = LageKorrekturRoll * 10;
720 ingob 1149
*/
395 hbuss 1150
 
1225 hbuss 1151
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL / 2) 
1152
#define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 
1153
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16) //16
492 hbuss 1154
#define BEWEGUNGS_LIMIT 20000
395 hbuss 1155
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
1156
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1225 hbuss 1157
        if(labs(IntegralFehlerNick) > FEHLER_LIMIT1) cnt = 4;
1173 hbuss 1158
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
492 hbuss 1159
        {
1051 killagreg 1160
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
395 hbuss 1161
         {
1051 killagreg 1162
           if(last_n_p)
395 hbuss 1163
           {
1173 hbuss 1164
            cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
492 hbuss 1165
            ausgleichNick = IntegralFehlerNick / 8;
401 hbuss 1166
            if(ausgleichNick > 5000) ausgleichNick = 5000;
498 hbuss 1167
            LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051 killagreg 1168
           }
395 hbuss 1169
           else last_n_p = 1;
1170
         } else  last_n_p = 0;
1051 killagreg 1171
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
395 hbuss 1172
         {
1173
           if(last_n_n)
1051 killagreg 1174
            {
1173 hbuss 1175
             cnt += labs(IntegralFehlerNick) / (FEHLER_LIMIT2 / 8);
492 hbuss 1176
             ausgleichNick = IntegralFehlerNick / 8;
401 hbuss 1177
             if(ausgleichNick < -5000) ausgleichNick = -5000;
498 hbuss 1178
             LageKorrekturNick += ausgleichNick / ABGLEICH_ANZAHL;
1051 killagreg 1179
            }
395 hbuss 1180
           else last_n_n = 1;
1181
         } else  last_n_n = 0;
1051 killagreg 1182
        }
1183
        else
847 hbuss 1184
        {
1185
         cnt = 0;
921 hbuss 1186
         KompassSignalSchlecht = 1000;
1051 killagreg 1187
        }
499 hbuss 1188
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1173 hbuss 1189
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
395 hbuss 1190
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
1191
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
401 hbuss 1192
 
395 hbuss 1193
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
492 hbuss 1194
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
1225 hbuss 1195
        if(labs(IntegralFehlerRoll) > FEHLER_LIMIT1) cnt = 4;
498 hbuss 1196
        ausgleichRoll = 0;
1173 hbuss 1197
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*8))
492 hbuss 1198
        {
1051 killagreg 1199
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
395 hbuss 1200
         {
1051 killagreg 1201
           if(last_r_p)
395 hbuss 1202
           {
1173 hbuss 1203
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
492 hbuss 1204
            ausgleichRoll = IntegralFehlerRoll / 8;
401 hbuss 1205
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
498 hbuss 1206
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
1051 killagreg 1207
           }
395 hbuss 1208
           else last_r_p = 1;
1209
         } else  last_r_p = 0;
1051 killagreg 1210
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
395 hbuss 1211
         {
1051 killagreg 1212
           if(last_r_n)
395 hbuss 1213
           {
1173 hbuss 1214
            cnt += labs(IntegralFehlerRoll) / (FEHLER_LIMIT2 / 8);
492 hbuss 1215
            ausgleichRoll = IntegralFehlerRoll / 8;
401 hbuss 1216
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
498 hbuss 1217
            LageKorrekturRoll += ausgleichRoll / ABGLEICH_ANZAHL;
395 hbuss 1218
           }
1219
           else last_r_n = 1;
1220
         } else  last_r_n = 0;
1051 killagreg 1221
        } else
492 hbuss 1222
        {
1223
         cnt = 0;
921 hbuss 1224
         KompassSignalSchlecht = 1000;
1051 killagreg 1225
        }
499 hbuss 1226
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
1173 hbuss 1227
                if(FromNaviCtrl_Value.Kalman_MaxDrift) if(cnt > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift;
395 hbuss 1228
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
1229
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
173 holgerb 1230
  }
1051 killagreg 1231
  else
498 hbuss 1232
  {
1233
   LageKorrekturRoll = 0;
1234
   LageKorrekturNick = 0;
880 hbuss 1235
   TrichterFlug = 0;
498 hbuss 1236
  }
1051 killagreg 1237
 
498 hbuss 1238
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
1051 killagreg 1239
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
1240
   MittelIntegralNick_Alt = MittelIntegralNick;
1241
   MittelIntegralRoll_Alt = MittelIntegralRoll;
1242
// +++++++++++++++++++++++++++++++++++++++++++++++++++++
395 hbuss 1243
    IntegralAccNick = 0;
1244
    IntegralAccRoll = 0;
1245
    IntegralAccZ = 0;
1246
    MittelIntegralNick = 0;
1247
    MittelIntegralRoll = 0;
1248
    MittelIntegralNick2 = 0;
1249
    MittelIntegralRoll2 = 0;
1250
    ZaehlMessungen = 0;
1173 hbuss 1251
 } //  ZaehlMessungen >= ABGLEICH_ANZAHL
395 hbuss 1252
 
1173 hbuss 1253
 
1051 killagreg 1254
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1255
//  Gieren
1051 killagreg 1256
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
693 hbuss 1257
//    if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;};
1051 killagreg 1258
    if(abs(StickGier) > 15) // war 35
1 ingob 1259
     {
921 hbuss 1260
      KompassSignalSchlecht = 1000;
1051 killagreg 1261
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX))
1262
       {
1263
         NeueKompassRichtungMerken = 1;
824 hbuss 1264
        };
1 ingob 1265
     }
395 hbuss 1266
    tmp_int  = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo  y = ax + bx²
1051 killagreg 1267
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
173 holgerb 1268
    sollGier = tmp_int;
1051 killagreg 1269
    Mess_Integral_Gier -= tmp_int;
395 hbuss 1270
    if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000;  // begrenzen
1271
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1051 killagreg 1272
 
1273
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1274
//  Kompass
1051 killagreg 1275
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1175 hbuss 1276
//DebugOut.Analog[16] = KompassSignalSchlecht;
819 hbuss 1277
 
1051 killagreg 1278
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1 ingob 1279
     {
819 hbuss 1280
       int w,v,r,fehler,korrektur;
1 ingob 1281
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1282
       v = abs(IntegralRoll /512);
1283
       if(v > w) w = v; // grösste Neigung ermitteln
824 hbuss 1284
       korrektur = w / 8 + 1;
921 hbuss 1285
       fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180;
1171 hbuss 1286
       if(abs(MesswertGier) > 128)
1036 hbuss 1287
            {
1288
                 fehler = 0;
1051 killagreg 1289
                }
921 hbuss 1290
       if(!KompassSignalSchlecht && w < 25)
1291
        {
1292
        GierGyroFehler += fehler;
1051 killagreg 1293
        if(NeueKompassRichtungMerken)
1294
         {
921 hbuss 1295
          beeptime = 200;
847 hbuss 1296
//         KompassStartwert = KompassValue;
1171 hbuss 1297
                  ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
921 hbuss 1298
          KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR);
1299
          NeueKompassRichtungMerken = 0;
1300
         }
1 ingob 1301
        }
824 hbuss 1302
       ErsatzKompass += (fehler * 8) / korrektur;
921 hbuss 1303
       w = (w * Parameter_KompassWirkung) / 32;           // auf die Wirkung normieren
1 ingob 1304
       w = Parameter_KompassWirkung - w;                  // Wirkung ggf drosseln
921 hbuss 1305
       if(w >= 0)
1 ingob 1306
        {
1051 killagreg 1307
          if(!KompassSignalSchlecht)
693 hbuss 1308
          {
1051 killagreg 1309
           v = 64 + ((MaxStickNick + MaxStickRoll)) / 8;
847 hbuss 1310
           r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassStartwert) % 360) - 180;
1311
//           r = KompassRichtung;
819 hbuss 1312
           v = (r * w) / v;  // nach Kompass ausrichten
1313
           w = 3 * Parameter_KompassWirkung;
693 hbuss 1314
           if(v > w) v = w; // Begrenzen
1051 killagreg 1315
           else
693 hbuss 1316
           if(v < -w) v = -w;
1317
           Mess_Integral_Gier += v;
1051 killagreg 1318
          }
693 hbuss 1319
          if(KompassSignalSchlecht) KompassSignalSchlecht--;
1051 killagreg 1320
        }
921 hbuss 1321
        else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek
1051 killagreg 1322
     }
1323
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1324
 
1051 killagreg 1325
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1326
//  Debugwerte zuordnen
1051 killagreg 1327
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1328
  if(!TimerWerteausgabe--)
1329
   {
395 hbuss 1330
    TimerWerteausgabe = 24;
805 hbuss 1331
 
1171 hbuss 1332
    DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4);
1333
    DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4);
1334
    DebugOut.Analog[2] = Mittelwert_AccNick / 4;
1335
    DebugOut.Analog[3] = Mittelwert_AccRoll / 4;
1 ingob 1336
    DebugOut.Analog[4] = MesswertGier;
1337
    DebugOut.Analog[5] = HoehenWert;
1224 hbuss 1338
    DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
1 ingob 1339
    DebugOut.Analog[8] = KompassValue;
297 holgerb 1340
    DebugOut.Analog[9] = UBat;
1051 killagreg 1341
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
297 holgerb 1342
    DebugOut.Analog[10] = SenderOkay;
929 hbuss 1343
    //DebugOut.Analog[16] = Mittelwert_AccHoch;
1083 killagreg 1344
    //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
1345
    //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
819 hbuss 1346
    DebugOut.Analog[19] = WinkelOut.CalcState;
1286 MartinR 1347
        //DebugOut.Analog[20] = ServoValue; // MartinR: DebugOut.Analog[20] wird in timer0.c ausgegeben!!
1171 hbuss 1348
//    DebugOut.Analog[24] = MesswertNick/2;
1349
//    DebugOut.Analog[25] = MesswertRoll/2;
1173 hbuss 1350
    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
1172 hbuss 1351
//    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
1352
//    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1215 hbuss 1353
    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
1216 killagreg 1354
    DebugOut.Analog[30] = GPS_Nick;
720 ingob 1355
    DebugOut.Analog[31] = GPS_Roll;
1356
 
805 hbuss 1357
 
1358
//    DebugOut.Analog[19] -= DebugOut.Analog[19]/128;
1359
//    if(DebugOut.Analog[19] > 0) DebugOut.Analog[19]--; else DebugOut.Analog[19]++;
1360
 
173 holgerb 1361
/*    DebugOut.Analog[16] = motor_rx[0];
1362
    DebugOut.Analog[17] = motor_rx[1];
1363
    DebugOut.Analog[18] = motor_rx[2];
1364
    DebugOut.Analog[19] = motor_rx[3];
1365
    DebugOut.Analog[20] = motor_rx[0] + motor_rx[1] + motor_rx[2] + motor_rx[3];
1366
    DebugOut.Analog[20] /= 14;
1367
    DebugOut.Analog[21] = motor_rx[4];
1368
    DebugOut.Analog[22] = motor_rx[5];
1369
    DebugOut.Analog[23] = motor_rx[6];
1370
    DebugOut.Analog[24] = motor_rx[7];
1371
    DebugOut.Analog[25] = motor_rx[4] + motor_rx[5] + motor_rx[6] + motor_rx[7];
1372
*/
1373
//    DebugOut.Analog[9] = MesswertNick;
1 ingob 1374
//    DebugOut.Analog[9] = SollHoehe;
1375
//    DebugOut.Analog[10] = Mess_Integral_Gier / 128;
1376
//    DebugOut.Analog[11] = KompassStartwert;
1051 killagreg 1377
//    DebugOut.Analog[10] = Parameter_Gyro_I;
1378
//    DebugOut.Analog[10] = EE_Parameter.Gyro_I;
1379
//    DebugOut.Analog[9] = KompassRichtung;
1 ingob 1380
//    DebugOut.Analog[10] = GasMischanteil;
1381
//    DebugOut.Analog[3] = HoeheD * 32;
1382
//    DebugOut.Analog[4] = hoehenregler;
1383
  }
1384
 
1051 killagreg 1385
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1386
//  Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen
1051 killagreg 1387
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216 killagreg 1388
  if(TrichterFlug)  { SummeRoll = 0; SummeNick = 0;};
604 hbuss 1389
 
1171 hbuss 1390
  if(!Looping_Nick) IntegralNickMalFaktor = (IntegralNick * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralNickMalFaktor = 0;
1391
  if(!Looping_Roll) IntegralRollMalFaktor = (IntegralRoll * IntegralFaktor) /  (44000 / STICK_GAIN); else IntegralRollMalFaktor = 0;
1153 hbuss 1392
 
1167 hbuss 1393
#define TRIM_MAX 200
1166 hbuss 1394
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
1395
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
1286 MartinR 1396
 
1397
  if(!IntegralFaktor) // MartinR : hinzugefügt
1398
        {
1399
        MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN)  ; // MartinR : hinzugefügt
1400
        MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ;  // MartinR : hinzugefügt
1401
        }
1402
        else // MartinR so war es
1403
        {
1166 hbuss 1404
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
1405
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1286 MartinR 1406
        }
1407
 
1211 hbuss 1408
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1166 hbuss 1409
 
1 ingob 1410
    // Maximalwerte abfangen
1286 MartinR 1411
        // MartinR : Prüfen ob Unterscheidung nach Platinenversion erforderlich?
1153 hbuss 1412
//    #define MAX_SENSOR  (4096*STICK_GAIN)
1413
    #define MAX_SENSOR  (4096*4)
1216 killagreg 1414
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1 ingob 1415
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1416
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1417
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1418
    if(MesswertGier >  MAX_SENSOR) MesswertGier =  MAX_SENSOR;
1419
    if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR;
1420
 
1051 killagreg 1421
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1210 hbuss 1422
// all BL-Ctrl connected?
1423
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216 killagreg 1424
  if(MissingMotor) if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0)
1425
   {
1426
    modell_fliegt = 1;
1427
        GasMischanteil = MIN_GAS;
1428
   }
1210 hbuss 1429
 
1430
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1431
// Höhenregelung
1432
// Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht
1051 killagreg 1433
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
855 hbuss 1434
  GasMischanteil *= STICK_GAIN;
1286 MartinR 1435
 
1436
// MartinR : so war es:
1437
/*  
1 ingob 1438
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
1439
  {
1440
    int tmp_int;
1036 hbuss 1441
        static char delay = 100;
1 ingob 1442
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1443
    {
1216 killagreg 1444
      if(Parameter_MaxHoehe < 50)
1 ingob 1445
      {
1036 hbuss 1446
       if(!delay--)
1447
            {
1225 hbuss 1448
         if((MessLuftdruck > 1000) && OCR0A < 254)
1036 hbuss 1449
                  {
1225 hbuss 1450
                    if(OCR0A < 244)
1451
                     {
1452
                      ExpandBaro -= 10;
1453
                      OCR0A = DruckOffsetSetting - ExpandBaro;
1454
                         }
1455
                         else OCR0A = 254;
1456
                beeptime = 300;
1457
            delay = 250;
1036 hbuss 1458
                  }
1225 hbuss 1459
                  else      
1460
         if((MessLuftdruck < 100) && OCR0A > 1)
1036 hbuss 1461
                  {
1225 hbuss 1462
                   if(OCR0A > 10)
1463
                    {
1464
                     ExpandBaro += 10;
1465
                     OCR0A = DruckOffsetSetting - ExpandBaro;
1466
                        }
1467
                         else OCR0A = 1;
1036 hbuss 1468
                   beeptime = 300;
1469
           delay = 250;
1470
                  }
1471
          else
1051 killagreg 1472
          {
1036 hbuss 1473
               SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters
1474
           HoehenReglerAktiv = 0;
1475
                   delay = 1;
1476
                  }
1051 killagreg 1477
                }
1 ingob 1478
      }
1051 killagreg 1479
      else
1480
        {
1036 hbuss 1481
         HoehenReglerAktiv = 1;
1482
                 delay = 200;
1051 killagreg 1483
                }
1 ingob 1484
    }
1051 killagreg 1485
    else
1 ingob 1486
    {
492 hbuss 1487
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
1 ingob 1488
     HoehenReglerAktiv = 1;
1489
    }
1490
 
1491
    if(Notlandung) SollHoehe = 0;
1492
    h = HoehenWert;
1493
    if((h > SollHoehe) && HoehenReglerAktiv)      // zu hoch --> drosseln
1051 killagreg 1494
     {
855 hbuss 1495
      h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
1 ingob 1496
      h = GasMischanteil - h;         // vom Gas abziehen
871 hbuss 1497
      h -= (HoeheD)/(8/STICK_GAIN);    // D-Anteil
1498
      tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
1499
      if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN;
1500
      else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN);
1 ingob 1501
      h -= tmp_int;
1051 killagreg 1502
      hoehenregler = (hoehenregler*15 + h) / 16;
855 hbuss 1503
      if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
1 ingob 1504
       {
855 hbuss 1505
         if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
1506
         if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
1051 killagreg 1507
       }
1 ingob 1508
      if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
1509
      GasMischanteil = hoehenregler;
1051 killagreg 1510
     }
1 ingob 1511
  }
855 hbuss 1512
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1286 MartinR 1513
  */
1514
 
1515
  // MartinR: Neuer Höhenregler von MartinR: Anfang
1111 hbuss 1516
 
1286 MartinR 1517
   if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung
1518
  {
1519
    int tmp_int;
1520
        static char delay = 100;
1521
  // Definitionen Vorschlag Killagreg Anfang
1522
  #define HOOVER_GAS_AVERAGE 4192 // 4192 * 2ms = 8.2s averaging 
1523
  #define HEIGHT_CONTROL_GAS_AVERAGE 16
1524
        int16_t CosNick, CosRoll;
1525
        //int16_t HeightControlGas; // MartinR: Schwebegas
1526
        //static int16_t FilterHeightControlGas = 0; // MartinR: SchwebegasFilter
1527
        static uint32_t HooverGasEstimation = 0;
1528
        //static uint8_t delay = 100;
1529
 
1530
        int32_t tmp_long3;
1531
 // Ende Definitionen Vorschlag Killagreg
1532
 
1533
        // calculate cos of nick and roll angle used for projection of the vertical hoover gas  // Vorschlag von Killagerg:
1534
        CosNick = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg // =IntegralNick/GIER_GRAD_FAKTOR
1535
        CHECK_MIN_MAX(CosNick, -60, 60); // limit nick angle 
1536
        CosNick = c_cos_8192(CosNick);
1537
        CosRoll = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg 
1538
        CHECK_MIN_MAX(CosRoll, -60, 60); // limit roll angle 
1539
        CosRoll = c_cos_8192(CosRoll);
1540
 
1541
 
1542
// Neigungskorrektur ACCHoch:      
1543
          tmp_long3 = (int32_t) (NeutralAccZ-(Parameter_UserParam6*4)); // take current thrust 
1544
          tmp_long3 *= CosNick; // apply nick projection 
1545
          tmp_long3 /= 8192; tmp_long3 *= CosRoll; // apply roll projection 
1546
          tmp_long3 /= 8129; // average vertical projected thrust 
1547
 
1548
        //Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren // MartinR: zu Höhenregler in fc.c verschoben
1549
        //Mess_Integral_Hoch += tmp_long3; // Integrieren // MartinR: mit Neigungskorrektur // Test
1550
 
1551
        tmp_int = (int16_t) (AdWertAccHoch + NeutralAccZ - tmp_long3 - (Parameter_UserParam6*4));
1552
        if (tmp_int < 0) tmp_int ++ ; // MartinR: Digitalisierungsrauschen abmindern
1553
        if (tmp_int > 0) tmp_int -- ; // MartinR: entspricht - 1 Bit
1554
 
1555
        Mess_Integral_Hoch += tmp_int; // Integrieren // MartinR: mit Neigungskorrektur
1556
        Mess_Integral_Hoch -= Mess_Integral_Hoch / 256; // dämfen // MartinR: zu Höhenregler in fc.c verschoben
1557
        CHECK_MIN_MAX(Mess_Integral_Hoch, -16000, 16000); // limit 
1558
 
1559
 
1560
        // MartinR: Änderungen HoheD und D-Anteil Höhenregler (h_d) Anfang :
1561
                HoehenWertSumme += HoehenWert ;
1562
                //if (++ iHoehe >= 40) // MartinR: Wartezeit um auch kleine Änderungsgeschwindigkeiten erkennen zu können
1563
                if (++ iHoehe >= Parameter_UserParam5) // MartinR: Wartezeit einstellbar
1564
                {
1565
                 HoehenWertSumme = HoehenWertSumme / iHoehe ; // MartinR: Mittelwert während der Wartezeit
1566
                 HoeheD = (int) Parameter_Luftdruck_D * (HoehenWert - HoehenWertalt) ;
1567
                 if (HoeheD < 0) HoeheD += Parameter_Luftdruck_D ; // MartinR: Digitalisierungsrauschen abmindern
1568
                 if (HoeheD > 0) HoeheD -= Parameter_Luftdruck_D ; // MartinR: entspricht - 1 Bit vom Höhenwert
1569
 
1570
                 iHoehe = 0 ;
1571
                 HoehenWertalt = HoehenWertSumme ;
1572
                 HoehenWertSumme = 0 ;
1573
                }      
1574
                // D-Anteil berechnen:
1575
                h_d = (int) (HoeheD)/(64/STICK_GAIN);    // D-Anteil (8*8=64)
1576
                // ACC-D-Anteil
1577
                tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
1578
                if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN;
1579
                else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN);
1580
                h_d += tmp_int; // positiver D-Anteil bedeutet Steigen
1581
 
1582
        // MartinR: Änderungen HoheD und D-Anteil Höhenregler (h_d) Ende
1583
 
1584
 
1585
    if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1586
    {
1587
      //if(Parameter_MaxHoehe < 50)  // MartinR :so war es
1588
 
1589
 
1590
            if(Parameter_MaxHoehe < 50  || (Parameter_UserParam1 > 140) )   // MartinR: Schalter aus oder HH über UsererParam1 an
1591
      {
1592
          // Höhenregler nicht aktiv
1593
           if(!delay--)
1594
            {
1595
                 #define OPA_OFFSET_STEP 10
1596
         if((MessLuftdruck > 1000) && (OCR0A < (255 - OPA_OFFSET_STEP)) )
1597
                  {
1598
                    ExpandBaro -= 1;
1599
                    OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
1600
                beeptime = 300;
1601
            delay = 250;
1602
                  }
1603
                  else if((MessLuftdruck < 100) && (OCR0A > OPA_OFFSET_STEP))
1604
                  {
1605
                    ExpandBaro += 1;
1606
                    OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro;
1607
                    beeptime = 300;
1608
            delay = 250;
1609
                  }
1610
                  else      
1611
         if((MessLuftdruck < 100) && OCR0A > 1)
1612
                  {
1613
                   if(OCR0A > 10)
1614
                    {
1615
                     ExpandBaro += 10;
1616
                     OCR0A = DruckOffsetSetting - ExpandBaro;
1617
                        }
1618
                         else OCR0A = 1;
1619
                   beeptime = 300;
1620
           delay = 250;
1621
                  }
1622
          else
1623
          {    
1624
               //SollHoehe = HoehenWert - 20;  // Parameter_MaxHoehe ist der PPM-Wert des Schalters // MartinR : so war es
1625
                   SollHoehe = HoehenWert; // MartinR : geändert
1626
           HoehenReglerAktiv = 0;
1627
                   delay = 1;
1628
                  }
1629
                }
1630
                SollHoehe = HoehenWert;  // MartinR: SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1
1631
                //SchwebeGas = GasMischanteil;  // MartinR: SchwebeGas wird nachgeführt bis HoehenreglerAktiv = 1 
1632
                // Deaktiv wegen HooverGasEstimation/HOOVER_GAS_AVERAGE
1633
 
1634
                HoehenReglerAktiv = 0; // MartinR: zur Sicherheit
1635
      }
1636
      else // Schalter an und kein HH über Schalter
1637
        {
1638
         HoehenReglerAktiv = 1;
1639
                 // Schwebegas ist der letzte GasMischanteil vor dem Aktivieren der Höhenregelung
1640
                 delay = 200;
1641
                }
1642
    }
1643
    else // Sollhöhe über Poti
1644
    {
1645
     //SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20; // MartinR : so war es
1646
         //HoehenReglerAktiv = 1; // MartinR : so war es
1647
         if(Parameter_UserParam1 > 140) // HH über Schalter: Höhenregler abgeschaltet, Nachführen von Parametern 
1648
         {
1649
           SollHoehe = HoehenWert;  // SollHoehe wird nachgeführt bis HoehenreglerAktiv = 1
1650
       HoehenReglerAktiv = 0;
1651
         }
1652
         else  // Höhenregler mit Sollhöhe über Poti aktiv
1653
         {
1654
          // SchwebeGas = GasMischanteil;  // SchwebeGas zunächst = Gasmischanteil bis bessere Lösung
1655
          // Deaktiv wegen HooverGasEstimation/HOOVER_GAS_AVERAGE
1656
      HoehenReglerAktiv = 1;
1657
          // MartinR: Versuch Höhenregler zu verlangsamen.  Steig- / Sinkrate im MK-Tool über UserParam4 einstellbar
1658
                if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 > SollHoehe)
1659
                {
1660
                        if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
1661
                        {
1662
                        delay_Hoehenregler = 0;
1663
                        SollHoehe ++ ; //MartinR: Höhenänderung begrenzen.
1664
                        }
1665
                }
1666
                else
1667
                {
1668
                        if (((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20 < SollHoehe)
1669
                        {
1670
                                if(++delay_Hoehenregler > (Parameter_UserParam4 / 4))
1671
                                {
1672
                                delay_Hoehenregler = 0;
1673
                                SollHoehe -- ; //MartinR: Höhenänderung begrenzen.
1674
                                }
1675
                        }
1676
                }
1677
         }
1678
    }
1679
 
1680
    if(Notlandung) SollHoehe = 0;
1681
 
1682
         if(HoehenReglerAktiv && !(Parameter_UserParam1 > 140))      // kein HH
1683
        {
1684
          h_delta = (int) SollHoehe - (int) HoehenWert; // positiver Wert: MK ist unterhalb der Sollhöhe
1685
 
1686
          h_p = (h_delta * (int) Parameter_Hoehe_P) / (16 / STICK_GAIN); // Differenz bestimmen --> P-Anteil
1687
 
1688
 
1689
          /* an Anfang Höhenregler verschoben, da h_d für Schwebegas- Abschätzung verwendet wird
1690
      h_d = (int) (HoeheD)/(64/STICK_GAIN);    // D-Anteil (8*8=64)
1691
          // ACC-D-Anteil
1692
      tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN);
1693
      if(tmp_int > 50*STICK_GAIN) tmp_int = 50*STICK_GAIN;
1694
      else if(tmp_int < -(50*STICK_GAIN)) tmp_int = -(50*STICK_GAIN);
1695
      h_d += tmp_int; // positiver D-Anteil bedeutet Steigen
1696
          */
1697
 
1698
 
1699
          //if(HooverGasEstimation == 0) HeightControlGas = GasMixFraction; // take stick gas // MartinR: SchwebeGas = GasMischanteil;
1700
          if(HooverGasEstimation == 0) SchwebeGas = GasMischanteil;
1701
          else SchwebeGas = (int16_t)(HooverGasEstimation/HOOVER_GAS_AVERAGE); // take hoover point 
1702
          // MartinR: SchwebeGas = HooverGasEstimation/HOOVER_GAS_AVERAGE 
1703
 
1704
          h = SchwebeGas + h_p - h_d ;
1705
 
1706
          // strech by actual attitude projection // Neigungskorrektur
1707
          tmp_long3 = (int32_t)h;   // MartinR: HeightControlGas = h
1708
          tmp_long3 *= 8192; tmp_long3 /= CosNick;
1709
          tmp_long3 *= 8192; tmp_long3 /= CosRoll;
1710
          h = (int16_t)tmp_long3;
1711
 
1712
          hoehenregler = (hoehenregler*7 + h) / 8;
1713
 
1714
          if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN
1715
      {
1716
        if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN;
1717
        if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil;
1718
      }  
1719
          if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas
1720
          //else hoehenregler += (GasMischanteil-hoehenregler)/16  ; // um die Höhe noch leicht mit Gas korrigieren zu können
1721
          //if(SchwebeGas < 20)  SchwebeGas = 20; // nicht unter 20
1722
 
1723
          GasMischanteil = hoehenregler;
1724
        }
1725
        else  // Integratoren auf Null halten
1726
        {
1727
        Mess_Integral_Hoch = 0;
1728
        hoehenregler = GasMischanteil;
1729
        // h_p = 0 ;
1730
        // h_d = 0 ;
1731
        }      
1732
        // Hoover gas estimation by averaging gas control output on small z-velocities 
1733
        // this is done only if height contol option is selected in global config and aircraft is flying 
1734
        if(MikroKopterFlags & FLAG_FLY) //MikroKopterFlags  FLAG_FLY
1735
        {
1736
         if(HooverGasEstimation == 0) HooverGasEstimation = HOOVER_GAS_AVERAGE * (uint32_t)GasMischanteil; // init estimation 
1737
         // HOOVER_GAS_AVERAGE= 4192
1738
         if(abs(h_d) < 8)
1739
          {
1740
           tmp_long3 = (int32_t)GasMischanteil; // take current thrust 
1741
           tmp_long3 *= CosNick; // apply nick projection 
1742
           tmp_long3 /= 8192; tmp_long3 *= CosRoll; // apply roll projection 
1743
           tmp_long3 /= 8129; // average vertical projected thrust 
1744
           HooverGasEstimation -= HooverGasEstimation/HOOVER_GAS_AVERAGE;
1745
           HooverGasEstimation += tmp_long3;  
1746
          }
1747
        }
1748
 
1749
        DebugOut.Analog[16] = HooverGasEstimation/HOOVER_GAS_AVERAGE;
1750
        //DebugOut.Analog[16] = Mess_Integral_Hoch;
1751
        DebugOut.Analog[17] = h_d;
1752
 
1753
  } // EOF: EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG  
1754
 
1755
 
1756
  if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN;
1757
 
1758
 // MartinR: Ende neuer Höhenregler 
1759
 
1051 killagreg 1760
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1761
// + Mischer und PI-Regler
1051 killagreg 1762
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
173 holgerb 1763
  DebugOut.Analog[7] = GasMischanteil;
1051 killagreg 1764
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1765
// Gier-Anteil
1051 killagreg 1766
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
188 holgerb 1767
#define MUL_G  1.0
855 hbuss 1768
    GierMischanteil = MesswertGier - sollGier * STICK_GAIN;     // Regler für Gier
604 hbuss 1769
// GierMischanteil = 0;
855 hbuss 1770
#define MIN_GIERGAS  (40*STICK_GAIN)  // unter diesem Gaswert trotzdem Gieren
693 hbuss 1771
   if(GasMischanteil > MIN_GIERGAS)
1772
    {
1051 killagreg 1773
     if(GierMischanteil > (GasMischanteil / 2)) GierMischanteil = GasMischanteil / 2;
1774
     if(GierMischanteil < -(GasMischanteil / 2)) GierMischanteil = -(GasMischanteil / 2);
693 hbuss 1775
    }
1051 killagreg 1776
    else
693 hbuss 1777
    {
1051 killagreg 1778
     if(GierMischanteil > (MIN_GIERGAS / 2))  GierMischanteil = MIN_GIERGAS / 2;
1779
     if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
693 hbuss 1780
    }
855 hbuss 1781
    tmp_int = MAX_GAS*STICK_GAIN;
1051 killagreg 1782
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
855 hbuss 1783
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
513 hbuss 1784
 
1051 killagreg 1785
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 1786
// Nick-Achse
1051 killagreg 1787
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
723 hbuss 1788
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1286 MartinR 1789
 
1790
        // MartinR : so war es Anfang
1791
        /*
1153 hbuss 1792
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1051 killagreg 1793
    else  SummeNick += DiffNick; // I-Anteil bei HH
855 hbuss 1794
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
1795
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1153 hbuss 1796
    pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
1 ingob 1797
    // Motor Vorn
1286 MartinR 1798
        */
1799
        // MartinR : so war es Ende
1800
        // MartinR : geändert Anfang
1801
 
1802
        if(IntegralFaktor) // MartinR : ACC-Mode
1803
         {
1804
          SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1805
          if(SummeNick >  (STICK_GAIN * 8000L)) SummeNick =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
1806
      if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
1807
          pd_ergebnis_nick = DiffNick + (SummeNick / Ki);
1808
          SummeNickHH = 0 ;
1809
         }
1810
    else // MartinR : HH-Mode
1811
         {
1812
          SummeNickHH += DiffNick; // I-Anteil bei HH
1813
      if(SummeNickHH >  (STICK_GAIN * 8000L)) SummeNickHH =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
1814
      if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
1815
          pd_ergebnis_nick = DiffNick + SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
1816
          SummeNick = 0;
1817
     } 
1818
 
1819
        // MartinR : geändert Ende
1820
 
499 hbuss 1821
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1153 hbuss 1822
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
1823
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
173 holgerb 1824
 
1153 hbuss 1825
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1826
// Roll-Achse
1827
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1828
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
1286 MartinR 1829
 
1830
        // MartinR : so war es Anfang
1831
        /*
1153 hbuss 1832
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
1833
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1834
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1835
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1836
    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
1286 MartinR 1837
        */
1838
        // MartinR : so war es Ende
1839
        // MartinR : geändert Anfang
1840
 
1841
        if(IntegralFaktor) // MartinR : ACC-Mode
1842
         {
1843
         SummeRoll += IntegralRollMalFaktor - StickRoll;
1844
         if(SummeRoll >  (STICK_GAIN * 8000L)) SummeRoll =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
1845
     if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
1846
         tmp_int = SummeRoll / Ki;
1847
         pd_ergebnis_roll = DiffRoll + tmp_int;         // MartinR: PI-Regler im ACC-Mode
1848
         //SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH
1849
         // MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht ändern!
1850
         SummeRollHH = 0;
1851
         }
1852
    else // MartinR : HH-Mode
1853
         {               
1854
          SummeRollHH += DiffRoll;  // I-Anteil bei HH
1855
      if(SummeRollHH >  (STICK_GAIN * 8000L)) SummeRollHH =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
1856
      if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
1857
          pd_ergebnis_roll = DiffRoll + SummeRollHH / KiHH;     // MartinR: PI-Regler für Roll bei HH
1858
          SummeRoll = 0;
1859
         }
1860
 
1861
        // MartinR : geändert Ende
1862
 
1153 hbuss 1863
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1864
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
1865
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
1866
 
1867
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1209 hbuss 1868
// Universal Mixer
1155 hbuss 1869
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1216 killagreg 1870
 for(i=0; i<MAX_MOTORS; i++)
1209 hbuss 1871
 {
1872
  signed int tmp_int;
1873
  if(Mixer.Motor[i][0] > 0)
1874
   {
1875
    tmp_int =  ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L;
1876
    tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L;
1877
    tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L;
1878
    tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
1879
    tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]);  // Filter
1216 killagreg 1880
        tmp_int = tmp_motorwert[i] / STICK_GAIN;
1209 hbuss 1881
        CHECK_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS);
1216 killagreg 1882
    Motor[i] = tmp_int;
1209 hbuss 1883
   }
1884
   else Motor[i] = 0;
1885
 }
1167 hbuss 1886
/*
1153 hbuss 1887
if(Poti1 > 20)  Motor1 = 0;
1888
if(Poti1 > 90)  Motor6 = 0;
1889
if(Poti1 > 140) Motor2 = 0;
1175 hbuss 1890
//if(Poti1 > 200) Motor7 = 0;
1167 hbuss 1891
*/
1111 hbuss 1892
}