Subversion Repositories FlightCtrl

Rev

Rev 1793 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1793 Rev 1797
Line 71... Line 71...
71
long IntegralRoll = 0,IntegralRoll2 = 0;
71
long IntegralRoll = 0,IntegralRoll2 = 0;
72
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
72
long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;
73
long Integral_Gier = 0;
73
long Integral_Gier = 0;
74
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
74
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
75
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
75
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
76
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
76
//long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; //MartinR so war es
-
 
77
long Mess_Integral_Gier = 0; //MartinR: Mess_Integral_Gier2 unbenutzt
77
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
78
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
78
long SummeNick=0,SummeRoll=0;
79
long SummeNick=0,SummeRoll=0;
79
volatile long Mess_Integral_Hoch = 0;
80
volatile long Mess_Integral_Hoch = 0;
80
int  KompassValue = 0;
81
int  KompassValue = 0;
81
int  KompassStartwert = 0;
82
int  KompassStartwert = 0;
Line 99... Line 100...
99
long HoehenWert = 0;
100
long HoehenWert = 0;
100
long SollHoehe = 0;
101
long SollHoehe = 0;
101
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
102
int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0;
102
//float Ki =  FAKTOR_I;
103
//float Ki =  FAKTOR_I;
103
int Ki = 10300 / 33;
104
int Ki = 10300 / 33;
-
 
105
int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter
-
 
106
 
104
unsigned char Looping_Nick = 0,Looping_Roll = 0;
107
unsigned char Looping_Nick = 0,Looping_Roll = 0;
105
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
108
unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;
Line 106... Line 109...
106
 
109
 
107
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
110
unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250
Line 150... Line 153...
150
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
153
unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5;
151
unsigned char CareFree = 0;
154
unsigned char CareFree = 0;
152
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};
155
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};
Line 153... Line 156...
153
 
156
 
154
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
157
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
-
 
158
//int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es
-
 
159
int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0;  // MartinR: stick_.._neutral hinzugefügt
155
int MaxStickNick = 0,MaxStickRoll = 0;
160
 
156
unsigned int  modell_fliegt = 0;
161
unsigned int  modell_fliegt = 0;
157
volatile unsigned char FC_StatusFlags = 0;
162
volatile unsigned char FC_StatusFlags = 0;
158
long GIER_GRAD_FAKTOR = 1291;
163
long GIER_GRAD_FAKTOR = 1291;
159
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
164
signed int KopplungsteilNickRoll,KopplungsteilRollNick;
Line 183... Line 188...
183
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
188
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
184
    DebugOut.Analog[12] = Motor[0].SetPoint;
189
    DebugOut.Analog[12] = Motor[0].SetPoint;
185
    DebugOut.Analog[13] = Motor[1].SetPoint;
190
    DebugOut.Analog[13] = Motor[1].SetPoint;
186
    DebugOut.Analog[14] = Motor[2].SetPoint;
191
    DebugOut.Analog[14] = Motor[2].SetPoint;
187
    DebugOut.Analog[15] = Motor[3].SetPoint;
192
    DebugOut.Analog[15] = Motor[3].SetPoint;
-
 
193
        //DebugOut.Analog[16] = DiffNick;  // MartinR: test
-
 
194
        //DebugOut.Analog[17] = DiffRoll;  // MartinR: test
-
 
195
        //DebugOut.Analog[18] = MesswertNick;  // MartinR: test
-
 
196
        //DebugOut.Analog[19] = MesswertRoll;  // MartinR: test
188
    DebugOut.Analog[20] = ServoNickValue;
197
    DebugOut.Analog[20] = ServoNickValue;
189
    DebugOut.Analog[22] = Capacity.ActualCurrent;
198
    DebugOut.Analog[22] = Capacity.ActualCurrent;
190
    DebugOut.Analog[23] = Capacity.UsedCapacity;
199
    DebugOut.Analog[23] = Capacity.UsedCapacity;
191
//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
200
//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
192
//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
201
//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
Line 196... Line 205...
196
    if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
205
    if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe;
Line 197... Line 206...
197
 
206
 
198
//if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/)  { beeptime = 1000; DebugOut.Analog[25]++; }
207
//if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/)  { beeptime = 1000; DebugOut.Analog[25]++; }
Line 199... Line -...
199
}
-
 
200
 
-
 
201
 
208
}
202
 
209
 
203
void Piep(unsigned char Anzahl, unsigned int dauer)
210
void Piep(unsigned char Anzahl, unsigned int dauer)
204
{
211
{
205
 if(MotorenEin) return; //auf keinen Fall im Flug!
212
 if(MotorenEin) return; //auf keinen Fall im Flug!
Line 366... Line 373...
366
//############################################################################
373
//############################################################################
367
{
374
{
368
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
375
    static signed long tmpl,tmpl2,tmpl3,tmpl4;
369
        static signed int oldNick, oldRoll, d2Roll, d2Nick;
376
        static signed int oldNick, oldRoll, d2Roll, d2Nick;
370
        signed long winkel_nick, winkel_roll;
377
        signed long winkel_nick, winkel_roll;
371
    unsigned char i;
378
    //unsigned char i; // MartinR: wird bei V0.81d nicht mehr benötigt, da Poti-Auswertung verschoben
372
        MesswertGier = (signed int) AdNeutralGier - AdWertGier;
379
        MesswertGier = (signed int) AdNeutralGier - AdWertGier;
373
    MesswertNick = (signed int) AdWertNickFilter / 8;
380
    //MesswertNick = (signed int) AdWertNickFilter / 8; // MartinR: so war es
374
    MesswertRoll = (signed int) AdWertRollFilter / 8;
381
    //MesswertRoll = (signed int) AdWertRollFilter / 8; // MartinR: so war es
-
 
382
        MesswertNick = (signed int) AdWertNickFilter  ; // MartinR die Division /8 erfolgt bereits in der analog.c
-
 
383
    MesswertRoll = (signed int) AdWertRollFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c
375
    RohMesswertNick = MesswertNick;
384
    RohMesswertNick = MesswertNick;
376
    RohMesswertRoll = MesswertRoll;
385
    RohMesswertRoll = MesswertRoll;
Line 377... Line 386...
377
 
386
 
378
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
387
// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++
Line 463... Line 472...
463
    IntegralNick = Mess_IntegralNick;
472
    IntegralNick = Mess_IntegralNick;
464
    IntegralRoll = Mess_IntegralRoll;
473
    IntegralRoll = Mess_IntegralRoll;
465
    IntegralNick2 = Mess_IntegralNick2;
474
    IntegralNick2 = Mess_IntegralNick2;
466
    IntegralRoll2 = Mess_IntegralRoll2;
475
    IntegralRoll2 = Mess_IntegralRoll2;
Line -... Line 476...
-
 
476
 
467
 
477
//#define D_LIMIT 128 // MartinR: so war es
-
 
478
#define D_LIMIT 16
468
#define D_LIMIT 128
479
// MartinR: Änderung war notwendig, da die Division /8 bereits in der analog.c erfolgt
469
 
480
 
470
   MesswertNick = HiResNick / 8;
481
   //MesswertNick = HiResNick / 8; // MartinR : so war es
-
 
482
  // MesswertRoll = HiResRoll / 8; // MartinR : so war es
-
 
483
   MesswertNick = HiResNick ; // MartinR die Division /8 erfolgt bereits in der analog.c
Line -... Line 484...
-
 
484
   MesswertRoll = HiResRoll ; // MartinR die Division /8 erfolgt bereits in der analog.c
-
 
485
 
-
 
486
        // MartinR : so war es Anfang  
471
   MesswertRoll = HiResRoll / 8;
487
        /*
472
 
488
       
473
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
489
   if(AdWertNick < 15)   MesswertNick = -1000;  if(AdWertNick <  7)   MesswertNick = -2000;
474
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
490
   if(PlatinenVersion == 10)  { if(AdWertNick > 1010) MesswertNick = +1000;  if(AdWertNick > 1017) MesswertNick = +2000; }
475
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
491
   else  {  if(AdWertNick > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }
476
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
492
   if(AdWertRoll < 15)   MesswertRoll = -1000;  if(AdWertRoll <  7)   MesswertRoll = -2000;
-
 
493
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
-
 
494
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
-
 
495
         
-
 
496
   // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000
-
 
497
        */
-
 
498
        // MartinR : so war es Ende
-
 
499
       
-
 
500
         // MartinR : Neu Anfang
-
 
501
        if(PlatinenVersion == 10)  
-
 
502
        {
-
 
503
        if(AdWertNick > 1010) MesswertNick = +600;  
-
 
504
        if(AdWertNick > 1017) MesswertNick = +800;
-
 
505
        if(AdWertNick < 15)   MesswertNick = -600;  
-
 
506
        if(AdWertNick <  7)   MesswertNick = -800;
-
 
507
        if(AdWertRoll > 1010) MesswertRoll = +600;  
-
 
508
        if(AdWertRoll > 1017) MesswertRoll = +800;
-
 
509
        if(AdWertRoll < 15)   MesswertRoll = -600;  
-
 
510
        if(AdWertRoll <  7)   MesswertRoll = -800;
-
 
511
        }
-
 
512
        else  
-
 
513
        {  
-
 
514
        if(AdWertNick > 2000) MesswertNick = +1200;  
-
 
515
        if(AdWertNick > 2015) MesswertNick = +1600;
-
 
516
        if(AdWertNick < 15)   MesswertNick = -1200;  
-
 
517
        if(AdWertNick <  7)   MesswertNick = -1600;
-
 
518
        if(AdWertRoll > 2000) MesswertRoll = +1200;  
-
 
519
        if(AdWertRoll > 2015) MesswertRoll = +1600;
-
 
520
        if(AdWertRoll < 15)   MesswertRoll = -1200;  
-
 
521
        if(AdWertRoll <  7)   MesswertRoll = -1600;
Line 477... Line 522...
477
   if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000;  if(AdWertRoll > 1017) MesswertRoll = +2000; }
522
        }
-
 
523
 // MartinR : Neu Ende
-
 
524
 
478
   else { if(AdWertRoll > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }
525
  if(Parameter_Gyro_D)
479
 
526
  // MartinR: hier sind Änderungen erforderlich, da u.a. MesswertNick = HiResNick / 8 von der fc.c in die analog.c verschoben wurde
480
  if(Parameter_Gyro_D)
527
  // Hintergrund: Code einsparen
481
  {
528
  {
482
   d2Nick = HiResNick - oldNick;
529
   d2Nick = HiResNick - oldNick;
483
   oldNick = (oldNick + HiResNick)/2;
-
 
484
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
530
   oldNick = (oldNick + HiResNick)/2;
485
   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
531
   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;
486
 
532
   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;
487
   d2Roll = HiResRoll - oldRoll;
533
   d2Roll = HiResRoll - oldRoll;
488
   oldRoll = (oldRoll + HiResRoll)/2;
534
   oldRoll = (oldRoll + HiResRoll)/2;
489
   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
535
   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;
-
 
536
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
490
   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;
537
   
-
 
538
   //MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es 
491
 
539
   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert 
492
   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;
540
   //MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es 
493
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;
541
   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert 
Line 494... Line 542...
494
   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);
542
   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt
495
   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);
543
   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt
496
  }
544
  }
Line 572... Line 620...
572
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
620
 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);
573
 CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);
621
 CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3);
574
 CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
622
 CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4);
575
 CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
623
 CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5);
576
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
624
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
577
 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe);
625
 //CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); // MartinR: Zeile war doppelt
578
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
626
 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung);
579
 CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
627
 CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z);
580
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
628
 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung);
581
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
629
 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I);
582
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
630
 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D);
Line 599... Line 647...
599
 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
647
 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection);
600
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
648
// CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);
601
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
649
 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability);
602
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
650
 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl);
603
 Ki = 10300 / (Parameter_I_Faktor + 1);
651
 Ki = 10300 / (Parameter_I_Faktor + 1);
-
 
652
 
-
 
653
   
-
 
654
 if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else  KiHH = Ki; // MartinR : für HH über Schalter
-
 
655
 Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen
-
 
656
 if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode 
-
 
657
  // 0 = free; 100 = AID; 200 = coming home //neu 
-
 
658
 
-
 
659
 
604
 MAX_GAS = EE_Parameter.Gas_Max;
660
 MAX_GAS = EE_Parameter.Gas_Max;
605
 MIN_GAS = EE_Parameter.Gas_Min;
661
 MIN_GAS = EE_Parameter.Gas_Min;
Line 606... Line 662...
606
 
662
 
607
 tmp = EE_Parameter.OrientationModeControl;
663
 tmp = EE_Parameter.OrientationModeControl;
Line 631... Line 687...
631
void MotorRegler(void)
687
void MotorRegler(void)
632
//############################################################################
688
//############################################################################
633
{
689
{
634
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
690
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
635
         int GierMischanteil,GasMischanteil;
691
         int GierMischanteil,GasMischanteil;
-
 
692
         
-
 
693
         static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt
-
 
694
         
636
     static long sollGier = 0,tmp_long,tmp_long2;
695
     static long sollGier = 0,tmp_long,tmp_long2;
637
     static long IntegralFehlerNick = 0;
696
     static long IntegralFehlerNick = 0;
638
     static long IntegralFehlerRoll = 0;
697
     static long IntegralFehlerRoll = 0;
639
         static unsigned int RcLostTimer;
698
         static unsigned int RcLostTimer;
640
         static unsigned char delay_neutral = 0;
699
         static unsigned char delay_neutral = 0;
Line 741... Line 800...
741
                           SetNeutral(0);
800
                           SetNeutral(0);
742
                           calibration_done = 1;
801
                           calibration_done = 1;
743
                                                   ServoActive = 1;
802
                                                   ServoActive = 1;
744
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
803
                                                   DDRD  |=0x80; // enable J7 -> Servo signal
745
                           Piep(GetActiveParamSet(),120);
804
                           Piep(GetActiveParamSet(),120);
-
 
805
                                                        PPM_in[13] = Parameter_UserParam5 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+
-
 
806
                                                        PPM_in[14] = Parameter_UserParam6 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+
746
                         }
807
                         }
747
                        }
808
                        }
748
                    }
809
                    }
749
                 else
810
                 else
750
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
811
                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)  // ACC Neutralwerte speichern
Line 783... Line 844...
783
                                                                {
844
                                                                {
784
                                                                        modell_fliegt = 1;
845
                                                                        modell_fliegt = 1;
785
                                                                        MotorenEin = 1;
846
                                                                        MotorenEin = 1;
786
                                                                        sollGier = 0;
847
                                                                        sollGier = 0;
787
                                                                        Mess_Integral_Gier = 0;
848
                                                                        Mess_Integral_Gier = 0;
788
                                                                        Mess_Integral_Gier2 = 0;
849
                                                                        //Mess_Integral_Gier2 = 0; //MartinR: Mess_Integral_Gier2 unbenutzt
789
                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
850
                                                                        Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;
790
                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
851
                                                                        Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;
791
                                                                        Mess_IntegralNick2 = IntegralNick;
852
                                                                        Mess_IntegralNick2 = IntegralNick;
792
                                                                        Mess_IntegralRoll2 = IntegralRoll;
853
                                                                        Mess_IntegralRoll2 = IntegralRoll;
793
                                                                        SummeNick = 0;
854
                                                                        SummeNick = 0;
Line 827... Line 888...
827
 
888
 
828
 if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
889
 if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING))
829
  {
890
  {
830
        static int stick_nick,stick_roll;
891
        static int stick_nick,stick_roll;
-
 
892
    ParameterZuordnung();
-
 
893
                // MartinR: original:   
831
    ParameterZuordnung();
894
        /*
832
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
895
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
833
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
896
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
834
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
897
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-
 
898
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-
 
899
       
-
 
900
                        */
-
 
901
// MartinR: geändert Anfang
-
 
902
        if(Parameter_UserParam1 > 50)   // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist
-
 
903
                {
-
 
904
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4;
-
 
905
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ;
-
 
906
                //stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral);
-
 
907
                //stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral);
-
 
908
                }
-
 
909
               
-
 
910
         else
-
 
911
                {
-
 
912
                stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-
 
913
                stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-
 
914
                stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
915
                stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
916
                }
-
 
917
       
-
 
918
         if(IntegralFaktor)  
-
 
919
                {
-
 
920
                //stick_nick_neutral = stick_nick; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
921
                //stick_roll_neutral = stick_roll; //  beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR
-
 
922
                stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-
 
923
                stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
-
 
924
               
-
 
925
                //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam  
-
 
926
                //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam
-
 
927
                }
-
 
928
        /*else          // wenn HH , MartinR
-
 
929
                {
-
 
930
                //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
-
 
931
                //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben
-
 
932
                //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam
-
 
933
                //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam
-
 
934
                }
-
 
935
        */
Line 835... Line 936...
835
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
936
// MartinR: geändert Ende
836
 
937
 
837
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
938
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
838
// CareFree und freie Wahl der vorderen Richtung
939
// CareFree und freie Wahl der vorderen Richtung
Line 860... Line 961...
860
    StickNick -= (GPS_Nick + GPS_Nick2);
961
    StickNick -= (GPS_Nick + GPS_Nick2);
861
    StickRoll -= (GPS_Roll + GPS_Roll2);
962
    StickRoll -= (GPS_Roll + GPS_Roll2);
862
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
963
        StickGas  = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;
Line 863... Line 964...
863
 
964
 
864
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
965
    GyroFaktor     = (Parameter_Gyro_P + 10.0);
865
    IntegralFaktor = Parameter_Gyro_I;
966
    // IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
866
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
967
    GyroFaktorGier     = (Parameter_Gyro_Gier_P + 10.0);
867
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
-
 
868
 
968
    IntegralFaktorGier = Parameter_Gyro_Gier_I;
869
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
969
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
870
//+ Analoge Steuerung per Seriell
970
//+ Analoge Steuerung per Seriell
-
 
971
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
972
// MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen??
871
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
973
 
872
   if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
974
   if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128)
873
    {
975
    {
874
         StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
976
         StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;
875
         StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
977
         StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;
876
         StickGier += ExternControl.Gier;
978
         StickGier += ExternControl.Gier;
877
     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
979
     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;
878
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
980
     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;
879
    }
981
    }
Line 880... Line 982...
880
    if(StickGas < 0) StickGas = 0;
982
    if(StickGas < 0) StickGas = 0;
Line 881... Line 983...
881
 
983
 
882
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
984
        //if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0; // MartinR: so war es, verschoben um code zu sparen 
883
 
985
 
884
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
986
    if(abs(StickNick/STICK_GAIN) > MaxStickNick)
Line 967... Line 1069...
967
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
1069
 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren
968
 MittelIntegralRoll  += IntegralRoll;
1070
 MittelIntegralRoll  += IntegralRoll;
969
 MittelIntegralNick2 += IntegralNick2;
1071
 MittelIntegralNick2 += IntegralNick2;
970
 MittelIntegralRoll2 += IntegralRoll2;
1072
 MittelIntegralRoll2 += IntegralRoll2;
Line 971... Line 1073...
971
 
1073
 
-
 
1074
// if(Looping_Nick || Looping_Roll)  // MartinR: so war es
972
 if(Looping_Nick || Looping_Roll)
1075
   if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert
973
  {
1076
  {
974
    IntegralAccNick = 0;
1077
    IntegralAccNick = 0;
975
    IntegralAccRoll = 0;
1078
    IntegralAccRoll = 0;
976
    MittelIntegralNick = 0;
1079
    MittelIntegralNick = 0;
Line 981... Line 1084...
981
    Mess_IntegralRoll2 = Mess_IntegralRoll;
1084
    Mess_IntegralRoll2 = Mess_IntegralRoll;
982
    ZaehlMessungen = 0;
1085
    ZaehlMessungen = 0;
983
    LageKorrekturNick = 0;
1086
    LageKorrekturNick = 0;
984
    LageKorrekturRoll = 0;
1087
    LageKorrekturRoll = 0;
985
  }
1088
  }
-
 
1089
 
-
 
1090
  if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))    ) // MartinR:
-
 
1091
          // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH 
-
 
1092
          // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten!
-
 
1093
          // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet 
-
 
1094
        {
-
 
1095
       
-
 
1096
        IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
-
 
1097
    IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 
-
 
1098
    Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0    
-
 
1099
    Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0    
-
 
1100
    //Mess_Integral_Gier = 0;   // MartinR: im HH-Modus alle unbenutzten Integratoren = 0       
-
 
1101
        //Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
-
 
1102
    //Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
-
 
1103
        }
-
 
1104
 
-
 
1105
        if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben
-
 
1106
          else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen
Line 986... Line 1107...
986
 
1107
 
987
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1108
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1109
    if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es, wegen Kameraausgleich wieder aktiviert
-
 
1110
  //if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt 
988
  if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin))
1111
 
989
  {
1112
  {
990
   long tmp_long, tmp_long2;
1113
   long tmp_long, tmp_long2;
991
   if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
1114
   if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/)
992
     {
1115
     {
Line 1040... Line 1163...
1040
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1163
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
1041
 {
1164
 {
1042
  static int cnt = 0;
1165
  static int cnt = 0;
1043
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1166
  static char last_n_p,last_n_n,last_r_p,last_r_n;
1044
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1167
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
1045
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp)
1168
  //if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) // MartinR: so war es
-
 
1169
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt
-
 
1170
 
1046
  {
1171
  {
1047
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1172
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
1048
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1173
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
1049
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1174
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
1050
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
1175
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
Line 1206... Line 1331...
1206
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
1331
    if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000;
Line 1207... Line 1332...
1207
 
1332
 
1208
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1333
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1209
//  Kompass
1334
//  Kompass
-
 
1335
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1210
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1336
    if((KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))  &&  !(Parameter_UserParam1 > 50))  // MartinR: bei HH über Schalter wird der Kompass abgeschaltet
1211
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
1337
        //if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))  // MartinR: so war es
1212
     {
1338
     {
1213
       int w,v,r,fehler,korrektur;
1339
       int w,v,r,fehler,korrektur;
1214
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1340
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
1215
       v = abs(IntegralRoll /512);
1341
       v = abs(IntegralRoll /512);
Line 1259... Line 1385...
1259
 
1385
 
1260
#define TRIM_MAX 200
1386
#define TRIM_MAX 200
1261
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
1387
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
Line -... Line 1388...
-
 
1388
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
-
 
1389
 
-
 
1390
        //MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es
-
 
1391
    //MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es
-
 
1392
       
-
 
1393
        if(!IntegralFaktor) // MartinR : HH-Mode hinzugefügt
-
 
1394
        {
-
 
1395
        MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN)  ; // MartinR : hinzugefügt
-
 
1396
        MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ;  // MartinR : hinzugefügt
-
 
1397
        //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN);
-
 
1398
        //Mess_Integral_Gier = 0;       // MartinR: im HH-Modus alle unbenutzten Integratoren = 0       //neu: nur Kompass wird bei HH deaktiviert
-
 
1399
        //Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0
-
 
1400
        }
1262
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;
1401
        else // MartinR: ACC-Mode  so war es
1263
 
1402
        {
-
 
1403
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
-
 
1404
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
-
 
1405
        //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1264
    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);
1406
        }
Line 1265... Line 1407...
1265
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1407
       
1266
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1408
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1267
 
1409
 
Line 1352... Line 1494...
1352
                   }
1494
                   }
Line 1353... Line 1495...
1353
 
1495
 
1354
                // if height control is activated by an rc channel
1496
                // if height control is activated by an rc channel
1355
        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1497
        if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)  // Regler wird über Schalter gesteuert
1356
                {       // check if parameter is less than activation threshold
1498
                {       // check if parameter is less than activation threshold
-
 
1499
                        // if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position  // MartinR :so war es
1357
                        if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position
1500
                        if(Parameter_MaxHoehe < 50  || (Parameter_UserParam1 > 140) )   // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an
1358
                        {   //height control not active
1501
                        {   //height control not active
1359
                                if(!delay--)
1502
                                if(!delay--)
1360
                                {
1503
                                {
1361
                                        HoehenReglerAktiv = 0; // disable height control
1504
                                        HoehenReglerAktiv = 0; // disable height control
Line 1370... Line 1513...
1370
                        }
1513
                        }
1371
                }
1514
                }
1372
                else // no switchable height control
1515
                else // no switchable height control
1373
                {
1516
                {
1374
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
1517
                        SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung;
-
 
1518
                        // HoehenReglerAktiv = 1; // MartinR : so war es
-
 
1519
                        // MartinR : geändert Anfang
-
 
1520
                                if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern 
-
 
1521
                                {
-
 
1522
                                        HoehenReglerAktiv = 0;
-
 
1523
                                }
-
 
1524
                                else  // Höhenregler mit Sollhöhe über Poti aktiv
-
 
1525
                                {
1375
                        HoehenReglerAktiv = 1;
1526
                                        HoehenReglerAktiv = 1;
-
 
1527
                                }
-
 
1528
                        // MartinR : geändert Ende
1376
                }
1529
                }
Line 1377... Line 1530...
1377
 
1530
 
1378
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
1531
                // calculate cos of nick and roll angle used for projection of the vertical hoover gas
1379
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
1532
                tmp_int  = (int)(IntegralNick/GIER_GRAD_FAKTOR);  // nick angle in deg
Line 1432... Line 1585...
1432
                                        if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN))
1585
                                        if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN))
1433
                                        {
1586
                                        {
1434
                                                FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
1587
                                                FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN);
1435
                                                HeightTrimming = 0;
1588
                                                HeightTrimming = 0;
1436
                                                SollHoehe = HoehenWert; // update setpoint to current height
1589
                                                SollHoehe = HoehenWert; // update setpoint to current height
-
 
1590
                                                //MartinR: Sollhöhe sollte sich hier nicht mehr ändern!
1437
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1591
                                                if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500;
1438
                                                if(!StartTrigger && HoehenWert > 50)
1592
                                                if(!StartTrigger && HoehenWert > 50)
1439
                                                {
1593
                                                {
1440
                                                 StartTrigger = 1;
1594
                                                 StartTrigger = 1;
1441
                                                }
1595
                                                }
Line 1445... Line 1599...
1445
                                // Trim height set point
1599
                                // Trim height set point
1446
                                if(abs(HeightTrimming) > 512)
1600
                                if(abs(HeightTrimming) > 512)
1447
                                {
1601
                                {
1448
                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
1602
                                        SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint
1449
                                        HeightTrimming = 0;
1603
                                        HeightTrimming = 0;
1450
                    LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied
1604
                    //LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied // MartinR: so war es
-
 
1605
                                        LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // MartinR: geändert
-
 
1606
                                        // MartinR: limits eventuell verändern
1451
                                        if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
1607
                                        if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100;
1452
                                        //update hoover gas stick value when setpoint is shifted
1608
                                        //update hoover gas stick value when setpoint is shifted
1453
                       if(!EE_Parameter.Hoehe_StickNeutralPoint)
1609
                       if(!EE_Parameter.Hoehe_StickNeutralPoint)
1454
                       {
1610
                       {
1455
                           StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
1611
                           StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value
Line 1457... Line 1613...
1457
                           if(StickGasHover < 70) StickGasHover = 70;
1613
                           if(StickGasHover < 70) StickGasHover = 70;
1458
                           else if(StickGasHover > 150) StickGasHover = 150;
1614
                           else if(StickGasHover > 150) StickGasHover = 150;
1459
                       }
1615
                       }
1460
                                }
1616
                                }
1461
              if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
1617
              if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active
1462
                        } //if FCFlags & MKFCFLAG_FLY
1618
                        } //if FC_StatusFlags & MKFCFLAG_FLY
1463
                        else
1619
                        else
1464
                        {
1620
                        {
1465
                         SollHoehe = HoehenWert - 400;
1621
                         SollHoehe = HoehenWert - 400;
1466
                         if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
1622
                         if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint;
1467
                         else StickGasHover = 120;
1623
                         else StickGasHover = 120;
Line 1478... Line 1634...
1478
                                HeightDeviation = 0;
1634
                                HeightDeviation = 0;
1479
                        } // EOF // baro range expanding active
1635
                        } // EOF // baro range expanding active
1480
                        else // valid data from air pressure sensor
1636
                        else // valid data from air pressure sensor
1481
                        {
1637
                        {
1482
                                // ------------------------- P-Part ----------------------------
1638
                                // ------------------------- P-Part ----------------------------
-
 
1639
                                //DebugOut.Analog[16] = SollHoehe;  // MartinR: test
1483
                                tmp_long = (HoehenWert - SollHoehe); // positive when too high
1640
                                tmp_long = (HoehenWert - SollHoehe); // positive when too high
1484
                                LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
1641
                                LIMIT_MIN_MAX(tmp_long, -32767L, 32767L);       // avoid overflov when casting to int16_t
1485
                                HeightDeviation = (int)(tmp_long); // positive when too high
1642
                                HeightDeviation = (int)(tmp_long); // positive when too high
1486
                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
1643
                                tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part
1487
                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense
1644
                                LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense // MartinR: so war es
-
 
1645
                                // MartinR: weshalb unsymmetrisch?
-
 
1646
                                //LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 127 * STICK_GAIN); // more than the full range makes no sense // MartinR: geändert
1488
                                GasReduction = tmp_long;
1647
                                GasReduction = tmp_long;
1489
                                // ------------------------- D-Part 1: Vario Meter ----------------------------
1648
                                // ------------------------- D-Part 1: Vario Meter ----------------------------
1490
                                tmp_int = VarioMeter / 8;
1649
                                tmp_int = VarioMeter / 8;
1491
                                LIMIT_MIN_MAX(tmp_int, -127, 128);
1650
                                LIMIT_MIN_MAX(tmp_int, -127, 128);
1492
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1651
                                tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter
1493
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
1652
                                LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN);
-
 
1653
                                /* // MartinR: so war es Anfang
1494
                                if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint
1654
                                if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint
1495
                                else
1655
                                else
1496
                                if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
1656
                                if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode
-
 
1657
                                */ // MartinR: so war es Ende
-
 
1658
                                tmp_int /= 4; // MartinR: geändert: keine veränderung des d-part im "Deckel" mode
-
 
1659
                                if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint // MartinR: geändert
1497
                                GasReduction += tmp_int;
1660
                                GasReduction += tmp_int;
1498
                        } // EOF no baro range expanding
1661
                        } // EOF no baro range expanding
1499
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
1662
                        // ------------------------ D-Part 2: ACC-Z Integral  ------------------------
1500
            if(Parameter_Hoehe_ACC_Wirkung)
1663
            if(Parameter_Hoehe_ACC_Wirkung)
1501
                         {
1664
                         {
1502
                          tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
1665
                          tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN);
1503
                          LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN);
1666
                          //LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es
-
 
1667
                          // MartinR: weshalb unsymmetrisch?
-
 
1668
                          LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert
1504
                          GasReduction += tmp_long;
1669
                          GasReduction += tmp_long;
1505
                         }
1670
                         }
1506
                        // ------------------------ D-Part 3: GpsZ  ----------------------------------
1671
                        // ------------------------ D-Part 3: GpsZ  ----------------------------------
1507
                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
1672
                        tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L;
1508
            LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN);
1673
            //LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es
-
 
1674
                        // MartinR: weshalb unsymmetrisch?
-
 
1675
                        LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert
1509
                        GasReduction += tmp_int;
1676
                        GasReduction += tmp_int;
1510
            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
1677
            GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value
-
 
1678
                        //DebugOut.Analog[17] = GasReduction;  // MartinR: test
1511
                        // ------------------------                  ----------------------------------
1679
                        // ------------------------                  ----------------------------------
1512
                        HCGas -= GasReduction;
1680
                        HCGas -= GasReduction;
1513
                        // limit deviation from hoover point within the target region
1681
                        // limit deviation from hoover point within the target region
1514
                        if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
1682
                        if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero
1515
                        {
1683
                        {
Line 1549... Line 1717...
1549
            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
1717
            if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT)
1550
                        {  // old version
1718
                        {  // old version
1551
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1719
                                LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas
1552
                                GasMischanteil = FilterHCGas;
1720
                                GasMischanteil = FilterHCGas;
1553
                        }
1721
                        }
1554
                        else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode
1722
                        //else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es
-
 
1723
                        else GasMischanteil = FilterHCGas ; // MartinR: geändert, um  Überschwinger bei Höhenänderung zu verringern
1555
                  }
1724
                  }
1556
                }// EOF height control active
1725
                }// EOF height control active
1557
                else // HC not active
1726
                else // HC not active
1558
                {
1727
                {
1559
                        //update hoover gas stick value when HC is not active
1728
                        //update hoover gas stick value when HC is not active
Line 1655... Line 1824...
1655
     if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
1824
     if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2);
1656
    }
1825
    }
1657
    tmp_int = MAX_GAS*STICK_GAIN;
1826
    tmp_int = MAX_GAS*STICK_GAIN;
1658
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
1827
    if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil));
1659
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
1828
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
-
 
1829
       
-
 
1830
       
-
 
1831
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1832
// Nick / Roll-Achse  // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst
-
 
1833
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1834
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-
 
1835
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
-
 
1836
 
-
 
1837
        // PI-Regler für Nick und Roll
-
 
1838
        if(EE_Parameter.Gyro_Stability <= 8)
-
 
1839
        {
-
 
1840
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen
-
 
1841
        pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;
-
 
1842
        }
-
 
1843
        else
-
 
1844
        {
-
 
1845
        pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
-
 
1846
        pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4;  // Überlauf verhindern
-
 
1847
        }
-
 
1848
       
-
 
1849
        if(IntegralFaktor) // MartinR : ACC-Mode
-
 
1850
         {
-
 
1851
          SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
-
 
1852
          if(SummeNick >  (STICK_GAIN * 8000L)) SummeNick =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
-
 
1853
      if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
1854
          pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick
-
 
1855
          SummeNickHH = 0 ;
-
 
1856
         
-
 
1857
          SummeRoll += IntegralRollMalFaktor - StickRoll;
-
 
1858
          if(SummeRoll >  (STICK_GAIN * 8000L)) SummeRoll =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
-
 
1859
      if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
-
 
1860
          pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll
-
 
1861
          SummeRollHH = 0;
-
 
1862
         
-
 
1863
         }
-
 
1864
    else // MartinR : HH-Mode
-
 
1865
         {
-
 
1866
          SummeNickHH += DiffNick; // I-Anteil bei HH
-
 
1867
      if(SummeNickHH >  (STICK_GAIN * 8000L)) SummeNickHH =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
-
 
1868
      if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
1869
          pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
-
 
1870
          SummeNick = 0;
-
 
1871
         
-
 
1872
          SummeRollHH += DiffRoll;  // I-Anteil bei HH
-
 
1873
      if(SummeRollHH >  (STICK_GAIN * 8000L)) SummeRollHH =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
-
 
1874
      if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
-
 
1875
          pd_ergebnis_roll += SummeRollHH / KiHH;       // MartinR: PI-Regler für Roll bei HH
-
 
1876
          SummeRoll = 0;
-
 
1877
     } 
-
 
1878
        // MartinR : geändert Ende
-
 
1879
       
-
 
1880
       
-
 
1881
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-
 
1882
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
-
 
1883
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
Line -... Line 1884...
-
 
1884
 
-
 
1885
        //tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-
 
1886
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
-
 
1887
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;       
-
 
1888
       
-
 
1889
       
-
 
1890
// MartinR: alt
1660
 
1891
/*
1661
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1892
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1662
// Nick-Achse
1893
// Nick-Achse
1663
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1894
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1895
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
-
 
1896
        // MartinR : so war es Anfang
1664
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1897
        /+
1665
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1898
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1666
    else  SummeNick += DiffNick; // I-Anteil bei HH
1899
    else  SummeNick += DiffNick; // I-Anteil bei HH
1667
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
1900
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
-
 
1901
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-
 
1902
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick
1668
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1903
    // Motor Vorn
-
 
1904
       
-
 
1905
        +/
-
 
1906
        // MartinR : so war es Ende
-
 
1907
       
1669
 
1908
        // MartinR : geändert Anfang
-
 
1909
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen
-
 
1910
        if(IntegralFaktor) // MartinR : ACC-Mode
-
 
1911
         {
-
 
1912
          SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1670
    if(EE_Parameter.Gyro_Stability <= 8)        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8; // PI-Regler für Nick
1913
          if(SummeNick >  (STICK_GAIN * 8000L)) SummeNick =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
1671
    else                                                                        pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern
1914
      if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
1915
          //pd_ergebnis_nick = DiffNick + (SummeNick / Ki);
-
 
1916
          pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick
-
 
1917
          SummeNickHH = 0 ;
-
 
1918
         }
-
 
1919
    else // MartinR : HH-Mode
-
 
1920
         {
-
 
1921
          SummeNickHH += DiffNick; // I-Anteil bei HH
-
 
1922
      if(SummeNickHH >  (STICK_GAIN * 8000L)) SummeNickHH =  (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf
-
 
1923
      if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf
-
 
1924
          pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH
-
 
1925
          SummeNick = 0;
-
 
1926
     } 
-
 
1927
        // MartinR : geändert Ende
1672
    pd_ergebnis_nick +=  SummeNick / Ki;
1928
       
1673
 
1929
       
1674
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1930
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1675
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
1931
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
Line 1676... Line 1932...
1676
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
1932
    if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int;
1677
 
1933
 
1678
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1934
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1679
// Roll-Achse
1935
// Roll-Achse
-
 
1936
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1937
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
1680
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1938
        // MartinR : so war es Anfang
1681
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
1939
        /+
1682
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
1940
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
1683
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1941
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
-
 
1942
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
-
 
1943
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
-
 
1944
    pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki;   // PI-Regler für Roll
1684
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1945
        +/
-
 
1946
        // MartinR : so war es Ende
1685
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1947
       
-
 
1948
        // MartinR : geändert Anfang
-
 
1949
        pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;
-
 
1950
        if(IntegralFaktor) // MartinR : ACC-Mode
-
 
1951
         {
-
 
1952
         SummeRoll += IntegralRollMalFaktor - StickRoll;
-
 
1953
         if(SummeRoll >  (STICK_GAIN * 8000L)) SummeRoll =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
1686
 
1954
     if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
1687
    if(EE_Parameter.Gyro_Stability <= 8)        pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8;        // PI-Regler für Roll
1955
         //tmp_int = SummeRoll / Ki;
-
 
1956
         //pd_ergebnis_roll = DiffRoll + tmp_int;       // MartinR: PI-Regler im ACC-Mode
-
 
1957
         pd_ergebnis_roll += SummeRoll / Ki;    // PI-Regler für Roll
-
 
1958
         //SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH
-
 
1959
         // MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht ändern!
-
 
1960
         SummeRollHH = 0;
-
 
1961
         }
-
 
1962
    else // MartinR : HH-Mode
-
 
1963
         {               
-
 
1964
          SummeRollHH += DiffRoll;  // I-Anteil bei HH
-
 
1965
      if(SummeRollHH >  (STICK_GAIN * 8000L)) SummeRollHH =  (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf
-
 
1966
      if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf
-
 
1967
          pd_ergebnis_roll += SummeRollHH / KiHH;       // MartinR: PI-Regler für Roll bei HH
-
 
1968
          SummeRoll = 0;
Line 1688... Line 1969...
1688
        else                                                                    pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4;  // Überlauf verhindern
1969
         }
1689
    pd_ergebnis_roll += SummeRoll / Ki;
1970
        // MartinR : geändert Ende
1690
       
1971
       
-
 
1972
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
-
 
1973
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
Line 1691... Line 1974...
1691
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1974
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
1692
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
1975
       
1693
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
1976
        */
1694
 
1977
 
Line 1714... Line 1997...
1714
                        if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
1997
                        if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil;
1715
                        else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
1998
                        else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil;
1716
                        else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
1999
                        else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L;
Line 1717... Line 2000...
1717
 
2000
 
1718
                        if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
2001
                        if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2;      // MotorSmoothing
-
 
2002
                        //else tmp_int = 2 * tmp_int - tmp_motorwert[i];                // MotorSmoothing  // MartinR: so war es
Line 1719... Line 2003...
1719
                        else tmp_int = 2 * tmp_int - tmp_motorwert[i];                                                              // MotorSmoothing
2003
                        //else tmp_int = tmp_int;               // MartinR: Entsprechend Vorschlag von MartinW geändert
1720
 
2004
 
1721
                        LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
2005
                        LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4);
1722
                        Motor[i].SetPoint = tmp_int / 4;
2006
                        Motor[i].SetPoint = tmp_int / 4;