Subversion Repositories FlightCtrl

Rev

Rev 2380 | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2380 Rev 2384
Line 112... Line 112...
112
unsigned char CalibrationDone = 0;
112
unsigned char CalibrationDone = 0;
113
char NeueKompassRichtungMerken = 0;
113
char NeueKompassRichtungMerken = 0;
114
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
114
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
115
//float Ki =  FAKTOR_I;
115
//float Ki =  FAKTOR_I;
116
int Ki = 10300 / 33;
116
int Ki = 10300 / 33;
-
 
117
int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter
-
 
118
 
117
unsigned char Looping_Nick = 0,Looping_Roll = 0;
119
unsigned char Looping_Nick = 0,Looping_Roll = 0;
118
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
120
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
Line 119... Line 121...
119
 
121
 
120
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
122
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
Line 165... Line 167...
165
unsigned char Parameter_GlobalConfig;
167
unsigned char Parameter_GlobalConfig;
166
unsigned char Parameter_ExtraConfig;
168
unsigned char Parameter_ExtraConfig;
167
unsigned char Parameter_MaximumAltitude;
169
unsigned char Parameter_MaximumAltitude;
168
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
170
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
169
unsigned char CareFree = 0;
171
unsigned char CareFree = 0;
170
const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps
172
//const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; // 15° steps // MartinR: so war es
-
 
173
const signed char sintab[62] = { 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16, 16, 16, 15, 14, 13, 12, 10, 8, 6, 4, 2, 0, -2, -4, -6, -8, -10, -12, -13, -14, -15, -16, -16, -16, -16, -16, -15, -14, -13, -12, -10, -8, -6, -4, -2, 0, 2, 4, 6, 8, 10, 12, 13, 14, 15, 16, 16, 16}; // 7,5° steps //MartinR
-
 
174
signed char cosinus, sinus; // MartinR : extern für PAN-Funktion
Line 171... Line 175...
171
 
175
 
172
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
176
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
-
 
177
//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es
-
 
178
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
-
 
179
//unsigned char stick_p; // MartinR: Test
-
 
180
unsigned char Parameter_NaviGpsModeControl;     // MartinR:
173
int MaxStickNick = 0,MaxStickRoll = 0;
181
 
174
unsigned int  modell_fliegt = 0;
182
unsigned int  modell_fliegt = 0;
175
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
183
volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0;
176
long GIER_GRAD_FAKTOR = 1291;
184
long GIER_GRAD_FAKTOR = 1291;
177
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
185
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
Line 207... Line 215...
207
    DebugOut.Analog[11] = ErsatzKompassInGrad;
215
    DebugOut.Analog[11] = ErsatzKompassInGrad;
208
    DebugOut.Analog[12] = Motor[0].SetPoint;
216
    DebugOut.Analog[12] = Motor[0].SetPoint;
209
    DebugOut.Analog[13] = Motor[1].SetPoint;
217
    DebugOut.Analog[13] = Motor[1].SetPoint;
210
    DebugOut.Analog[14] = Motor[2].SetPoint;
218
    DebugOut.Analog[14] = Motor[2].SetPoint;
211
    DebugOut.Analog[15] = Motor[3].SetPoint;
219
    DebugOut.Analog[15] = Motor[3].SetPoint;
-
 
220
       
-
 
221
        DebugOut.Analog[16] = cosinus;  // MartinR: zur Einstellung der Pan-Funktion
-
 
222
        DebugOut.Analog[17] = sinus;  // MartinR: test zur Einstellung der Pan-Funktion
-
 
223
        DebugOut.Analog[18] = ServoPanValue;  // MartinR:  zur Einstellung der Pan-Funktion
-
 
224
        DebugOut.Analog[19] = ServoRollValue;  // MartinR:  Test
-
 
225
       
212
    DebugOut.Analog[20] = ServoNickValue;
226
    DebugOut.Analog[20] = ServoNickValue;
213
        DebugOut.Analog[21] = HoverGas;
227
        DebugOut.Analog[21] = HoverGas;
214
    DebugOut.Analog[22] = Capacity.ActualCurrent;
228
    DebugOut.Analog[22] = Capacity.ActualCurrent;
215
    DebugOut.Analog[23] = Capacity.UsedCapacity;
229
    DebugOut.Analog[23] = Capacity.UsedCapacity;
216
        DebugOut.Analog[24] = SollHoehe/10;    
230
        DebugOut.Analog[24] = SollHoehe/10;    
Line 487... Line 501...
487
            tmpl4 *= Parameter_AchsKopplung2; //65
501
            tmpl4 *= Parameter_AchsKopplung2; //65
488
            tmpl4 /= 4096L;
502
            tmpl4 /= 4096L;
489
            KopplungsteilNickRoll = tmpl3;
503
            KopplungsteilNickRoll = tmpl3;
490
            KopplungsteilRollNick = tmpl4;
504
            KopplungsteilRollNick = tmpl4;
491
            tmpl4 -= tmpl3;
505
            tmpl4 -= tmpl3;
-
 
506
                        if(IntegralFaktor) // MartinR: nur im ACC-Mode
-
 
507
                        {
492
            ErsatzKompass += tmpl4;
508
            ErsatzKompass += tmpl4;
493
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
509
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen
494
 
510
                        }
495
            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
511
            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
496
            tmpl *= Parameter_AchsKopplung1;  // 90
512
            tmpl *= Parameter_AchsKopplung1;  // 90
497
            tmpl /= 4096L;
513
            tmpl /= 4096L;
498
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
514
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
499
            tmpl2 *= Parameter_AchsKopplung1;
515
            tmpl2 *= Parameter_AchsKopplung1;
Line 543... Line 559...
543
#define D_LIMIT 128
559
#define D_LIMIT 128
Line 544... Line 560...
544
 
560
 
545
   MesswertNick = HiResNick / 8;
561
   MesswertNick = HiResNick / 8;
Line -... Line 562...
-
 
562
   MesswertRoll = HiResRoll / 8;
-
 
563
 
-
 
564
        // MartinR : so war es Anfang  
546
   MesswertRoll = HiResRoll / 8;
565
        /*
547
 
566
       
548
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
567
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
549
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
568
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
550
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
569
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
551
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
570
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
Line -... Line 571...
-
 
571
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
-
 
572
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
-
 
573
         
-
 
574
   // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
-
 
575
        */
-
 
576
        // MartinR : so war es Ende
-
 
577
       
-
 
578
#ifdef NO_GYRO_PROGRESSION      // MartinR; keine Begrenzung
-
 
579
        #warning : "### NO_GYRO_PROGRESSION ###"
-
 
580
        //nichts tun
-
 
581
#else                                   // mit Begrenzung
-
 
582
         // MartinR : Neu Anfang
-
 
583
        if(PlatinenVersion == 10)  
-
 
584
        {
-
 
585
        if(AdWertNick > 1010) MesswertNick = +600;  
-
 
586
        if(AdWertNick > 1017) MesswertNick = +800;
-
 
587
        if(AdWertNick < 15)   MesswertNick = -600;  
-
 
588
        if(AdWertNick <  7)   MesswertNick = -800;
-
 
589
        if(AdWertRoll > 1010) MesswertRoll = +600;  
-
 
590
        if(AdWertRoll > 1017) MesswertRoll = +800;
-
 
591
        if(AdWertRoll < 15)   MesswertRoll = -600;  
-
 
592
        if(AdWertRoll <  7)   MesswertRoll = -800;
-
 
593
        }
-
 
594
        else  
-
 
595
        {  
-
 
596
        if(AdWertNick > 2000) MesswertNick = +1200;  
-
 
597
        if(AdWertNick > 2015) MesswertNick = +1600;
-
 
598
        if(AdWertNick < 15)   MesswertNick = -1200;  
-
 
599
        if(AdWertNick <  7)   MesswertNick = -1600;
-
 
600
        if(AdWertRoll > 2000) MesswertRoll = +1200;  
-
 
601
        if(AdWertRoll > 2015) MesswertRoll = +1600;
-
 
602
        if(AdWertRoll < 15)   MesswertRoll = -1200;  
-
 
603
        if(AdWertRoll <  7)   MesswertRoll = -1600;
-
 
604
        }
552
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
605
 // MartinR : Neu Ende
553
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
606
#endif
554
 
607
 
555
  if(Parameter_Gyro_D)
608
  if(Parameter_Gyro_D)
556
  {
609
  {
Line 696... Line 749...
696
 Parameter_ExtraConfig = EE_Parameter.ExtraConfig;
749
 Parameter_ExtraConfig = EE_Parameter.ExtraConfig;
697
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
750
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
698
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
751
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
699
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
752
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
700
 Ki = 10300 / (Parameter_I_Faktor + 1);
753
 Ki = 10300 / (Parameter_I_Faktor + 1);
-
 
754
 //Ki = (10300 / (Parameter_I_Faktor + 4)) + (StickGas /2); // MartinR: Test Gasabhängige Regelung
-
 
755
   
-
 
756
 if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else  KiHH = Ki; // MartinR : für HH über Schalter
-
 
757
 //if(Parameter_UserParam1 > 50) KiHH = (10300 / (Parameter_UserParam2 + 4)) + (StickGas /2); else  KiHH = Ki; // MartinR : für HH über Schalter  // MartinR: Test Gasabhängige Regelung
-
 
758
 //Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
-
 
759
 Parameter_NaviGpsModeControl = GetChannelValue(EE_Parameter.NaviGpsModeChannel); //MartinR: Standard: EE_Parameter.NaviGpsModeChannel wird übertragen
-
 
760
 
-
 
761
 if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode 
-
 
762
  // 0 = free; 100 = AID; 200 = coming home //neu 
-
 
763
 
-
 
764
 
701
 MAX_GAS = EE_Parameter.Gas_Max;
765
 MAX_GAS = EE_Parameter.Gas_Max;
702
 MIN_GAS = EE_Parameter.Gas_Min;
766
 MIN_GAS = EE_Parameter.Gas_Min;
Line 703... Line 767...
703
 
767
 
704
 if(EE_Parameter.CareFreeChannel)
768
 if(EE_Parameter.CareFreeChannel)
Line 743... Line 807...
743
void MotorRegler(void)
807
void MotorRegler(void)
744
//############################################################################
808
//############################################################################
745
{
809
{
746
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
810
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
747
         int GierMischanteil,GasMischanteil;
811
         int GierMischanteil,GasMischanteil;
-
 
812
         
-
 
813
         static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt
-
 
814
         
748
     static long sollGier = 0,tmp_long,tmp_long2;
815
     static long sollGier = 0,tmp_long,tmp_long2;
749
     static long IntegralFehlerNick = 0;
816
     static long IntegralFehlerNick = 0;
750
     static long IntegralFehlerRoll = 0;
817
     static long IntegralFehlerRoll = 0;
751
         static unsigned int RcLostTimer;
818
         static unsigned int RcLostTimer;
752
         static unsigned char delay_neutral = 0;
819
         static unsigned char delay_neutral = 0;
Line 1036... Line 1103...
1036
//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
1103
//                                                                      ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
1037
                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
1104
                                                                        NeueKompassRichtungMerken = 100; // 2 sekunden
1038
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1105
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1039
                                                                        SpeakHoTT = SPEAK_STARTING;
1106
                                                                        SpeakHoTT = SPEAK_STARTING;
1040
#endif
1107
#endif
-
 
1108
                                                                        // MartinR: hinzugefügt Anfang
-
 
1109
                                                                        stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
-
 
1110
                                                                        stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR
-
 
1111
                                                                        SummeNickHH = 0 ; // Zurücksetzen der Integratoren
-
 
1112
                                                                        SummeRollHH = 0 ; // Zurücksetzen der Integratoren
-
 
1113
                                                                        // MartinR: hinzugefügt Ende
1041
                                                                }
1114
                                                                }
1042
                                                                else
1115
                                                                else
1043
                                                                {
1116
                                                                {
1044
                                                                        beeptime = 1500; // indicate missing calibration
1117
                                                                        beeptime = 1500; // indicate missing calibration
1045
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1118
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
Line 1116... Line 1189...
1116
  {
1189
  {
1117
        static int stick_nick,stick_roll;
1190
        static int stick_nick,stick_roll;
1118
        unsigned char stick_p;
1191
        unsigned char stick_p;
1119
    ParameterZuordnung();
1192
    ParameterZuordnung();
1120
        stick_p = EE_Parameter.Stick_P;
1193
        stick_p = EE_Parameter.Stick_P;
-
 
1194
        // MartinR: original:   
-
 
1195
        /*
1121
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
1196
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
1122
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
1197
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
1123
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
1198
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
1124
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
1199
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-
 
1200
                        */
-
 
1201
// MartinR: geändert Anfang
-
 
1202
       
-
 
1203
         if(IntegralFaktor)  // ACC-Mode
-
 
1204
                {
-
 
1205
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4;
-
 
1206
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4;
-
 
1207
                stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
1208
                stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
1209
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-
 
1210
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-
 
1211
               
-
 
1212
                }
-
 
1213
        else            // HH-Mode
-
 
1214
                {
-
 
1215
                        if(Parameter_UserParam1 > 49)   // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
-
 
1216
                        {
-
 
1217
                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * Parameter_UserParam3);
-
 
1218
                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * Parameter_UserParam3);
-
 
1219
                        }
-
 
1220
               
-
 
1221
                        else
-
 
1222
                        {
-
 
1223
                        stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * stick_p);
-
 
1224
                        stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p);
-
 
1225
                        }
-
 
1226
                }
-
 
1227
// MartinR: geändert Ende
Line 1125... Line 1228...
1125
 
1228
 
1126
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1229
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1127
// CareFree und freie Wahl der vorderen Richtung
1230
// CareFree und freie Wahl der vorderen Richtung
1128
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1231
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1232
        //if(CareFree) // MartinR: so war es
1129
        if(CareFree)
1233
        if(CareFree && IntegralFaktor) // MartinR: CareFree nur im ACC-Mode
1130
        {
1234
        {
1131
                signed int nick, roll;
1235
                signed int nick, roll;
1132
                nick = stick_nick / 4;
1236
                nick = stick_nick / 4;
1133
                roll = stick_roll / 4;
1237
                roll = stick_roll / 4;
1134
                StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
1238
                StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4);
1135
                StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
1239
                StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4);
1136
        }
1240
        }
1137
        else
1241
        else
1138
        {
1242
        {
-
 
1243
                //FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; //MartinR: so war es
1139
                FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6];
1244
                FromNC_Rotate_C = (sintab[EE_Parameter.OrientationAngle * 2 + 12]) / 2; //MartinR: feinere Auflösung
-
 
1245
                //FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; //MartinR: so war es
1140
                FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle];
1246
                FromNC_Rotate_S = (sintab[EE_Parameter.OrientationAngle * 2]) / 2; //MartinR: feinere Auflösung
1141
                StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
1247
                StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8;
1142
                StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
1248
                StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8;
Line 1143... Line 1249...
1143
        }
1249
        }
Line 1162... Line 1268...
1162
    StickNick -= GPS_Nick;
1268
    StickNick -= GPS_Nick;
1163
    StickRoll -= GPS_Roll;
1269
    StickRoll -= GPS_Roll;
1164
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
1270
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127;
Line 1165... Line 1271...
1165
 
1271
 
1166
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
1272
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
1167
    IntegralFaktor = Parameter_Gyro_I;
1273
        // IntegralFaktor = Parameter_Gyro_I; // MartinR: war mal hier
1168
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
1274
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
Line 1169... Line 1275...
1169
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
1275
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
1170
 
1276
 
Line 1279... Line 1385...
1279
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1385
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1280
 MittelIntegralRoll  += IntegralRoll;
1386
 MittelIntegralRoll  += IntegralRoll;
1281
 MittelIntegralNick2 += IntegralNick2;
1387
 MittelIntegralNick2 += IntegralNick2;
1282
 MittelIntegralRoll2 += IntegralRoll2;
1388
 MittelIntegralRoll2 += IntegralRoll2;
Line -... Line 1389...
-
 
1389
 
-
 
1390
  if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR:
-
 
1391
          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 
-
 
1392
          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
-
 
1393
          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 
-
 
1394
        {
-
 
1395
        IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1396
    IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1397
    Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode       
-
 
1398
    Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1399
    Mess_Integral_Gier = 0;     // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode  
-
 
1400
        sollGier = 0;
-
 
1401
        Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1402
    //Mess_Integral_Gier2 = 0; // MartinR:  Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode
-
 
1403
        KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten
-
 
1404
        ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten
-
 
1405
        NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten
-
 
1406
        }
1283
 
1407
 
-
 
1408
// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
-
 
1409
   if(Looping_Nick || Looping_Roll || (!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
1284
 if(Looping_Nick || Looping_Roll)
1410
   // MartinR: beim ACC-Loop oder beim zurückschalten von HH auf ACC
1285
  {
1411
  {
1286
    IntegralAccNick = 0;
1412
    IntegralAccNick = 0;
1287
    IntegralAccRoll = 0;
1413
    IntegralAccRoll = 0;
1288
    MittelIntegralNick = 0;
1414
    MittelIntegralNick = 0;
Line 1294... Line 1420...
1294
    ZaehlMessungen = 0;
1420
    ZaehlMessungen = 0;
1295
    LageKorrekturNick = 0;
1421
    LageKorrekturNick = 0;
1296
    LageKorrekturRoll = 0;
1422
    LageKorrekturRoll = 0;
1297
  }
1423
  }
Line -... Line 1424...
-
 
1424
 
-
 
1425
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben
-
 
1426
          else IntegralFaktor = Parameter_Gyro_I; // MartinR: geändert
1298
 
1427
 
1299
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1428
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1300
  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
1429
  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
1301
  {
1430
  {
1302
   long tmp_long, tmp_long2;
1431
   long tmp_long, tmp_long2;
Line 1347... Line 1476...
1347
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1476
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1348
 {
1477
 {
1349
  static int cnt = 0;
1478
  static int cnt = 0;
1350
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1479
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1351
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1480
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1352
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
1481
  //if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) // MartinR: so war es
-
 
1482
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
-
 
1483
 
1353
  {
1484
  {
1354
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1485
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1355
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1486
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1356
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1487
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1357
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
1488
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
Line 1515... Line 1646...
1515
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1646
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 1516... Line 1647...
1516
 
1647
 
1517
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1648
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1518
//  Kompass
1649
//  Kompass
1519
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1650
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1651
    //if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es
1520
    if(KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))
1652
        if((KompassValue >= 0 && (Parameter_GlobalConfig & CFG_KOMPASS_AKTIV))  &&  !(Parameter_UserParam1 > 50))  // MartinR: bei HH über Schalter wird der Kompass abgeschaltet
1521
     {
1653
     {
1522
      if(CalculateCompassTimer-- == 1)
1654
      if(CalculateCompassTimer-- == 1)
1523
          {
1655
          {
1524
       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
1656
       int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt
Line 1572... Line 1704...
1572
 
1704
 
1573
#define TRIM_MAX 200
1705
#define TRIM_MAX 200
1574
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
1706
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
Line -... Line 1707...
-
 
1707
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
-
 
1708
 
-
 
1709
        //MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es
-
 
1710
    //MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es
-
 
1711
       
-
 
1712
        if(!IntegralFaktor) // MartinR : HH-Mode hinzugefügt
-
 
1713
        {
-
 
1714
        MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN)  ; // MartinR : hinzugefügt
-
 
1715
        MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ;  // MartinR : hinzugefügt
-
 
1716
        //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN);
-
 
1717
        //Mess_Integral_Gier = 0;       // MartinR: nur Kompass wird bei HH deaktiviert
-
 
1718
        //Integral_Gier = 0; // MartinR: nur Kompass wird bei HH deaktiviert
-
 
1719
        }
1575
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
1720
        else // MartinR: ACC-Mode  so war es
1576
 
1721
        {
-
 
1722
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
-
 
1723
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1577
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
1724
        }
Line 1578... Line 1725...
1578
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1725
       
1579
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1726
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1580
 
1727
 
Line 1590... Line 1737...
1590
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1737
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1591
// Höhenregelung
1738
// Höhenregelung
1592
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1739
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1593
  if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
1740
  if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen
1594
  GasMischanteil *= STICK_GAIN;
1741
  GasMischanteil *= STICK_GAIN;
-
 
1742
 
-
 
1743
//MartinR: Höhenregler nur mit 1284er Prozessor:
-
 
1744
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
-
 
1745
 
1595
        // if height control is activated
1746
        // if height control is activated
1596
 if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F))  // Höhenregelung
1747
 if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F))  // Höhenregelung
1597
        {
1748
        {
1598
                #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
1749
                #define HOVER_GAS_AVERAGE 16384L                // 16384 * 2ms = 32s averaging
1599
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
1750
                #define HC_GAS_AVERAGE 4                        // 4 * 2ms= 8ms averaging
Line 1660... Line 1811...
1660
                          BaroExpandActive--;
1811
                          BaroExpandActive--;
1661
                   }
1812
                   }
1662
                // if height control is activated by an rc channel
1813
                // if height control is activated by an rc channel
1663
        if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1814
        if(Parameter_GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1664
                {       // check if parameter is less than activation threshold
1815
                {       // check if parameter is less than activation threshold
1665
                        if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position
1816
                        // if(Parameter_HoehenSchalter < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es
-
 
1817
                        if((Parameter_HoehenSchalter < 50) || (Parameter_UserParam1 > 140) )   // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an
1666
                        {   //height control not active
1818
                        {   //height control not active
1667
                                if(!delay--)
1819
                                if(!delay--)
1668
                                {
1820
                                {
1669
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1821
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
1670
                                    if(!SpeakHoTT && HoehenReglerAktiv)  SpeakHoTT = SPEAK_ALTITUDE_OFF;
1822
                                    if(!SpeakHoTT && HoehenReglerAktiv)  SpeakHoTT = SPEAK_ALTITUDE_OFF;
Line 1685... Line 1837...
1685
                        }
1837
                        }
1686
                }
1838
                }
1687
                else // no switchable height control
1839
                else // no switchable height control
1688
                {
1840
                {
1689
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
1841
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_HoehenSchalter) * (int)EE_Parameter.Hoehe_Verstaerkung;
-
 
1842
                        // HoehenReglerAktiv = 1; // MartinR : so war es
-
 
1843
                        // MartinR : geändert Anfang
-
 
1844
                                if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern 
-
 
1845
                                {
-
 
1846
                                        HoehenReglerAktiv = 0;
-
 
1847
                                }
-
 
1848
                                else  // Höhenregler mit Sollhöhe über Poti aktiv
-
 
1849
                                {
1690
                        HoehenReglerAktiv = 1;
1850
                                        HoehenReglerAktiv = 1;
1691
                }
1851
                                }
-
 
1852
                        // MartinR : geändert Ende
-
 
1853
                }
1692
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
1854
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
1693
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
1855
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
1694
                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
1856
                tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR);  // roll angle in deg
1695
                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
1857
                CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg
1696
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
1858
                LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle
Line 2045... Line 2207...
2045
        {
2207
        {
2046
                // set undefined state to indicate vario off
2208
                // set undefined state to indicate vario off
2047
                FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
2209
                FC_StatusFlags |= (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
2048
        } // EOF no height control
2210
        } // EOF no height control
Line -... Line 2211...
-
 
2211
 
-
 
2212
// MartinR: Höhenregler nur noch mit 1284er Prozessor
-
 
2213
#endif
2049
 
2214
 
2050
   // Limits the maximum gas in case of "Out of Range emergency landing"
2215
   // Limits the maximum gas in case of "Out of Range emergency landing"
2051
   if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
2216
   if(NC_To_FC_Flags & NC_TO_FC_EMERGENCY_LANDING)
2052
        {
2217
        {
2053
         if(GasMischanteil/STICK_GAIN > HooverGasEmergencyPercent && HoverGas) GasMischanteil = HooverGasEmergencyPercent * STICK_GAIN;
2218
         if(GasMischanteil/STICK_GAIN > HooverGasEmergencyPercent && HoverGas) GasMischanteil = HooverGasEmergencyPercent * STICK_GAIN;
Line 2089... Line 2254...
2089
    }
2254
    }
2090
    tmp_int = MAX_GAS*STICK_GAIN;
2255
    tmp_int = MAX_GAS*STICK_GAIN;
2091
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
2256
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
2092
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
2257
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
Line -... Line 2258...
-
 
2258
       
-
 
2259
       
-
 
2260
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
2261
// Nick / Roll-Achse  // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst
-
 
2262
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
2263
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-
 
2264
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-
 
2265
 
-
 
2266
        // PI-Regler für Nick und Roll
-
 
2267
        if(EE_Parameter.Gyro_Stability <= 8)
-
 
2268
        {
-
 
2269
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen
-
 
2270
        pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;
-
 
2271
        }
-
 
2272
        else
-
 
2273
        {
-
 
2274
        pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
-
 
2275
        pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4;  // Überlauf verhindern
-
 
2276
        }
-
 
2277
       
-
 
2278
        if(IntegralFaktor) // MartinR : ACC-Mode
-
 
2279
         {
-
 
2280
          SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
-
 
2281
          if(SummeNick >  (STICK_GAIN * 8000L)) SummeNick =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
-
 
2282
      if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
2283
          pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick
-
 
2284
          SummeNickHH = 0 ;
-
 
2285
         
-
 
2286
          SummeRoll += IntegralRollMalFaktor - StickRoll;
-
 
2287
          if(SummeRoll >  (STICK_GAIN * 8000L)) SummeRoll =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
-
 
2288
      if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
-
 
2289
          pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll
-
 
2290
          SummeRollHH = 0;
-
 
2291
         
-
 
2292
         }
-
 
2293
    else // MartinR : HH-Mode
-
 
2294
         {
-
 
2295
          SummeNickHH += DiffNick; // I-Anteil bei HH
-
 
2296
      if(SummeNickHH >  (STICK_GAIN * 8000L)) SummeNickHH =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
-
 
2297
      if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
2298
          pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
-
 
2299
          SummeNick = 0;
-
 
2300
         
-
 
2301
          SummeRollHH += DiffRoll;  // I-Anteil bei HH
-
 
2302
      if(SummeRollHH >  (STICK_GAIN * 8000L)) SummeRollHH =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
-
 
2303
      if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
-
 
2304
          pd_ergebnis_roll += SummeRollHH / KiHH;       // MartinR: PI-Regler für Roll bei HH
-
 
2305
          SummeRoll = 0;
-
 
2306
     } 
-
 
2307
        // MartinR : geändert Ende
-
 
2308
       
-
 
2309
       
-
 
2310
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-
 
2311
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
-
 
2312
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
-
 
2313
 
-
 
2314
        //tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-
 
2315
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
-
 
2316
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;       
-
 
2317
       
-
 
2318
       
-
 
2319
// MartinR: alt
2093
 
2320
/*
2094
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2321
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2095
// Nick-Achse
2322
// Nick-Achse
2096
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2323
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2097
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
2324
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
Line 2123... Line 2350...
2123
       
2350
       
2124
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
2351
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
2125
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
2352
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
Line -... Line 2353...
-
 
2353
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
-
 
2354
 
-
 
2355
        */
2126
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
2356
        // MartinR: alt Ende
2127
 
2357
 
2128
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2358
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2129
// Universal Mixer
2359
// Universal Mixer
2130
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2360
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++