Subversion Repositories FlightCtrl

Rev

Rev 469 | Rev 498 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 469 Rev 492
Line 45... Line 45...
45
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
45
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
46
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
46
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
47
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
47
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
48
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
48
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
49
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
49
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
50
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-
 
51
// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
50
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
52
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
53
// +  POSSIBILITY OF SUCH DAMAGE. 
52
// +  POSSIBILITY OF SUCH DAMAGE. 
54
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
Line 55... Line 54...
55
 
54
 
Line 80... Line 79...
80
unsigned char HoehenReglerAktiv = 0;
79
unsigned char HoehenReglerAktiv = 0;
81
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
80
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
Line 82... Line 81...
82
 
81
 
83
float GyroFaktor;
82
float GyroFaktor;
84
float IntegralFaktor;
-
 
85
 
83
float IntegralFaktor;
86
volatile int  DiffNick,DiffRoll;
84
volatile int  DiffNick,DiffRoll;
87
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
85
int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;
88
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
86
volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;
89
unsigned char MotorWert[5];
87
unsigned char MotorWert[5];
Line 114... Line 112...
114
unsigned char Parameter_ServoNickControl = 100;
112
unsigned char Parameter_ServoNickControl = 100;
115
unsigned char Parameter_LoopGasLimit = 70;
113
unsigned char Parameter_LoopGasLimit = 70;
116
unsigned char Parameter_AchsKopplung1 = 0;
114
unsigned char Parameter_AchsKopplung1 = 0;
117
unsigned char Parameter_AchsGegenKopplung1 = 0;
115
unsigned char Parameter_AchsGegenKopplung1 = 0;
118
struct mk_param_struct EE_Parameter;
116
struct mk_param_struct EE_Parameter;
-
 
117
signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;
Line 119... Line 118...
119
 
118
 
120
void Piep(unsigned char Anzahl)
119
void Piep(unsigned char Anzahl)
121
{
120
{
122
 while(Anzahl--)
121
 while(Anzahl--)
Line 171... Line 170...
171
    KompassStartwert = KompassValue;
170
    KompassStartwert = KompassValue;
172
    GPS_Neutral();
171
    GPS_Neutral();
173
    beeptime = 50;  
172
    beeptime = 50;  
174
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
173
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
175
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
174
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
-
 
175
    ExternHoehenValue = 0;
176
}
176
}
Line 177... Line 177...
177
 
177
 
178
//############################################################################
178
//############################################################################
179
// Bearbeitet die Messwerte
179
// Bearbeitet die Messwerte
Line 265... Line 265...
265
//++++++++++++++++++++++++++++++++++++++++++++++++
265
//++++++++++++++++++++++++++++++++++++++++++++++++
266
// ADC einschalten
266
// ADC einschalten
267
    ANALOG_ON; 
267
    ANALOG_ON; 
268
//++++++++++++++++++++++++++++++++++++++++++++++++
268
//++++++++++++++++++++++++++++++++++++++++++++++++
269
DebugOut.Analog[11] = MesswertRoll;
269
DebugOut.Analog[11] = MesswertRoll;
-
 
270
//DebugOut.Analog[11] = AdWertNick;
Line 270... Line 271...
270
 
271
 
271
    Integral_Gier  = Mess_Integral_Gier;
272
    Integral_Gier  = Mess_Integral_Gier;
272
    IntegralNick = Mess_IntegralNick;
273
    IntegralNick = Mess_IntegralNick;
273
    IntegralRoll = Mess_IntegralRoll;
274
    IntegralRoll = Mess_IntegralRoll;
Line 539... Line 540...
539
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
540
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
540
// neue Werte von der Funke
541
// neue Werte von der Funke
541
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
542
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
542
 if(!NewPpmData-- || Notlandung)  
543
 if(!NewPpmData-- || Notlandung)  
543
  {
544
  {
-
 
545
    int tmp_int;
544
    ParameterZuordnung();
546
    ParameterZuordnung();
545
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
547
    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;
546
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
548
    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
547
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
549
    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;
548
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
550
    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
549
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
551
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
550
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
552
    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;
551
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
553
    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;
Line -... Line 554...
-
 
554
 
-
 
555
#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)
-
 
556
if(DubWiseKeys[1]) beeptime = 10;
-
 
557
    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else
-
 
558
    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;
-
 
559
    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;
-
 
560
    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else
-
 
561
    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;
-
 
562
    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;
-
 
563
 
-
 
564
    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else
-
 
565
    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;
-
 
566
    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;
-
 
567
    if(DubWiseKeys[0] & 16) ExternHoehenValue--;
-
 
568
 
-
 
569
    StickNick += ExternStickNick / 8;
-
 
570
    StickRoll += ExternStickRoll / 8;
-
 
571
    StickGier += ExternStickGier;
552
 
572
 
553
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
573
    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;
554
    if(GyroFaktor < 0) GyroFaktor = 0;
574
    if(GyroFaktor < 0) GyroFaktor = 0;
555
    if(IntegralFaktor < 0) IntegralFaktor = 0;
575
    if(IntegralFaktor < 0) IntegralFaktor = 0;
556
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
576
    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern
Line 672... Line 692...
672
   long tmp_long, tmp_long2;
692
   long tmp_long, tmp_long2;
673
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
693
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
674
    tmp_long /= 16;
694
    tmp_long /= 16;
675
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
695
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
676
    tmp_long2 /= 16;
696
    tmp_long2 /= 16;
677
 #define AUSGLEICH 32
697
 #define AUSGLEICH 32 //(Parameter_UserParam1 / 2)
678
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
698
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
679
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
699
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
680
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
700
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
681
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
701
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
682
    Mess_IntegralNick -= tmp_long;
702
    Mess_IntegralNick -= tmp_long;
Line 694... Line 714...
694
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
714
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
695
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
715
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
696
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
716
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
697
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
717
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
698
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
718
    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;
699
#define MAX_I 1//(Poti2/10)
719
#define MAX_I 0//(Poti2/10)
700
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
720
// Nick ++++++++++++++++++++++++++++++++++++++++++++++++
701
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
721
    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);
702
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
722
    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;
703
    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
723
/*    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)
704
     {
724
     {
705
       tmp_long = IntegralFehlerNick / 700L;
725
       tmp_long = IntegralFehlerNick / 256L;
706
       if(tmp_long > MAX_I) tmp_long = MAX_I;
726
       if(tmp_long > MAX_I) tmp_long = MAX_I;
707
       if(tmp_long <-MAX_I) tmp_long =-MAX_I;
727
       if(tmp_long <-MAX_I) tmp_long =-MAX_I;
708
       I_LageNick = tmp_long;    
728
       I_LageNick = tmp_long;    
709
     }
729
     }
710
     else
730
     else
711
     {
731
     {
712
      I_LageNick /= 2;
732
      I_LageNick /= 2;
713
     }
733
     }
-
 
734
*/
714
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++     
735
// Roll ++++++++++++++++++++++++++++++++++++++++++++++++     
715
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
736
    IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll);
716
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
737
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
717
    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
738
/*    if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000)
718
     {
739
     {
719
      tmp_long2 = IntegralFehlerRoll / 700L;
740
      tmp_long2 = IntegralFehlerRoll / 256L;
720
      if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
741
      if(tmp_long2 > MAX_I) tmp_long2 = MAX_I;
721
      if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
742
      if(tmp_long2 <-MAX_I) tmp_long2 =-MAX_I;
722
      I_LageRoll = tmp_long2;    
743
      I_LageRoll = tmp_long2;    
723
     }
744
     }
724
     else
745
     else
725
     {
746
     {
726
      I_LageRoll /=2;
747
      I_LageRoll /=2;
727
     }
748
     }
728
 
749
*/
729
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
-
 
730
   MittelIntegralNick_Alt = MittelIntegralNick;      
-
 
731
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
-
 
732
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
-
 
733
    Mess_IntegralNick -= ausgleichNick;
750
    Mess_IntegralNick -= ausgleichNick;
734
    Mess_IntegralRoll -= ausgleichRoll;
751
    Mess_IntegralRoll -= ausgleichRoll;
Line 735... Line 752...
735
 
752
 
736
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
753
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
737
// Gyro-Drift ermitteln
754
// Gyro-Drift ermitteln
738
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
755
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
739
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
756
    MittelIntegralNick2 /= ABGLEICH_ANZAHL;
740
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
757
    MittelIntegralRoll2 /= ABGLEICH_ANZAHL;
741
    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick);
758
//    tmp_long  = (long)(MittelIntegralNick2 - (long)IntegralAccNick); 
-
 
759
//    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll); 
-
 
760
    tmp_long  = IntegralNick2 - IntegralNick;
742
    tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll);
761
    tmp_long2 = IntegralRoll2 - IntegralRoll;
Line 743... Line 762...
743
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
762
    //DebugOut.Analog[25] = MittelIntegralRoll2 / 26;
744
 
763
 
745
    IntegralFehlerNick = tmp_long;
764
    IntegralFehlerNick = tmp_long;
Line 759... Line 778...
759
DebugOut.Analog[22] = MittelIntegralRoll / 26;
778
DebugOut.Analog[22] = MittelIntegralRoll / 26;
760
//DebugOut.Analog[28] = ausgleichNick;
779
//DebugOut.Analog[28] = ausgleichNick;
761
DebugOut.Analog[29] = ausgleichRoll;
780
DebugOut.Analog[29] = ausgleichRoll;
762
DebugOut.Analog[30] = I_LageRoll * 10;
781
DebugOut.Analog[30] = I_LageRoll * 10;
Line 763... Line 782...
763
 
782
 
764
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL/2)
783
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
765
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL*4)
-
 
-
 
784
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
766
 
785
#define BEWEGUNGS_LIMIT 20000
767
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
786
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
-
 
787
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
-
 
788
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
768
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
789
        {
769
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
790
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
770
         {
791
         {
771
           if(last_n_p)
792
           if(last_n_p)
772
           {
793
           {
773
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
794
            cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
774
            ausgleichNick = IntegralFehlerNick / 12;
795
            ausgleichNick = IntegralFehlerNick / 8;
775
            if(ausgleichNick > 5000) ausgleichNick = 5000;
796
            if(ausgleichNick > 5000) ausgleichNick = 5000;
776
//            Mess_IntegralNick -= ausgleichNick;
797
            Mess_IntegralNick -= ausgleichNick;
777
           }
798
           }
778
           else last_n_p = 1;
799
           else last_n_p = 1;
779
         } else  last_n_p = 0;
800
         } else  last_n_p = 0;
780
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
801
        if(IntegralFehlerNick < -FEHLER_LIMIT2)
781
         {
802
         {
782
           if(last_n_n)
803
           if(last_n_n)
783
            {
804
            {
784
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
805
             cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2;
785
             ausgleichNick = IntegralFehlerNick / 12;
806
             ausgleichNick = IntegralFehlerNick / 8;
786
             if(ausgleichNick < -5000) ausgleichNick = -5000;
807
             if(ausgleichNick < -5000) ausgleichNick = -5000;
787
//             Mess_IntegralNick -= ausgleichNick;
808
             Mess_IntegralNick -= ausgleichNick;
788
            }
809
            }
789
           else last_n_n = 1;
810
           else last_n_n = 1;
790
         } else  last_n_n = 0;
-
 
-
 
811
         } else  last_n_n = 0;
791
 
812
        } else cnt = 0;
-
 
813
        if(cnt > 4) cnt = 4;
792
        if(cnt > 5) cnt = 5;
814
//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
793
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
815
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
Line 794... Line 816...
794
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
816
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
795
 
817
 
-
 
818
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
796
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
819
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
-
 
820
 
-
 
821
ausgleichRoll = 0;
797
        cnt = 1;// + labs(IntegralFehlerRoll) / 4096;
822
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
798
ausgleichRoll = 0;
823
        {
799
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
824
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
800
         {
825
         {
801
           if(last_r_p)
826
           if(last_r_p)
802
           {
827
           {
803
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
828
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
804
            ausgleichRoll = IntegralFehlerRoll / 12;
829
            ausgleichRoll = IntegralFehlerRoll / 8;
-
 
830
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
805
            if(ausgleichRoll > 5000) ausgleichRoll = 5000;
831
            Mess_IntegralRoll -= ausgleichRoll;
806
//            Mess_IntegralRoll -= ausgleichRoll;
832
//            beeptime = 50;
807
           }
833
           }
808
           else last_r_p = 1;
834
           else last_r_p = 1;
809
         } else  last_r_p = 0;
835
         } else  last_r_p = 0;
810
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
836
        if(IntegralFehlerRoll < -FEHLER_LIMIT2)
811
         {
837
         {
812
           if(last_r_n)
838
           if(last_r_n)
813
           {
839
           {
814
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
840
            cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2;
815
            ausgleichRoll = IntegralFehlerRoll / 12;
841
            ausgleichRoll = IntegralFehlerRoll / 8;
-
 
842
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
816
            if(ausgleichRoll < -5000) ausgleichRoll = -5000;
843
            Mess_IntegralRoll -= ausgleichRoll;
817
//            Mess_IntegralRoll -= ausgleichRoll;
844
//            beeptime = 50;
818
           }
845
           }
-
 
846
           else last_r_n = 1;
-
 
847
         } else  last_r_n = 0;
-
 
848
        } else
-
 
849
        {
819
           else last_r_n = 1;
850
         beeptime = 50;
820
         } else  last_r_n = 0;
851
         cnt = 0;
821
 
852
        }
-
 
853
DebugOut.Analog[27] = ausgleichRoll;
-
 
854
        if(cnt > 4) cnt = 4;
822
DebugOut.Analog[27] = ausgleichRoll;
855
//        if(cnt > Poti2 / 40) cnt =  Poti2 / 40;
823
        if(cnt > 5) cnt = 5;
856
 
-
 
857
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
824
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
858
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
Line 825... Line 859...
825
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
859
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
-
 
860
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
-
 
861
 
-
 
862
  }
-
 
863
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
826
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
864
   MittelIntegralNick_Alt = MittelIntegralNick;      
827
 
865
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
828
  }
866
// +++++++++++++++++++++++++++++++++++++++++++++++++++++     
829
    IntegralAccNick = 0;
867
    IntegralAccNick = 0;
830
    IntegralAccRoll = 0;
868
    IntegralAccRoll = 0;
Line 961... Line 999...
961
      else  
999
      else  
962
        HoehenReglerAktiv = 1;
1000
        HoehenReglerAktiv = 1;
963
    }
1001
    }
964
    else
1002
    else
965
    {
1003
    {
966
     SollHoehe = Parameter_MaxHoehe * EE_Parameter.Hoehe_Verstaerkung - 20;
1004
     SollHoehe = ((int) ExternHoehenValue + (int) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung - 20;
967
     HoehenReglerAktiv = 1;
1005
     HoehenReglerAktiv = 1;
968
    }
1006
    }
Line 969... Line 1007...
969
 
1007
 
970
    if(Notlandung) SollHoehe = 0;
1008
    if(Notlandung) SollHoehe = 0;