Subversion Repositories FlightCtrl

Rev

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

Rev 1022 Rev 1023
Line 643... Line 643...
643
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
643
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
644
// neue Werte von der Funke
644
// neue Werte von der Funke
645
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
645
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
646
 if(!NewPpmData-- || Notlandung)  
646
 if(!NewPpmData-- || Notlandung)  
647
  {
647
  {
648
    int tmp_int;
648
        static int chanNickPrev = 0;
-
 
649
        static int chanRollPrev = 0;
-
 
650
       
649
        static int stick_nick,stick_roll;
651
        static int stick_nick,stick_roll;
650
    ParameterZuordnung();
652
    ParameterZuordnung();
651
    stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4;
-
 
652
    stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;
-
 
653
    StickNick = stick_nick - (GPS_Nick + GPS_Nick2);
-
 
654
//    StickNick  = (StickNick  * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; 
-
 
655
 
653
       
656
    stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
-
 
657
    stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;
654
#define MAX_CHAN_VAL 125L
658
    StickRoll = stick_roll - (GPS_Roll + GPS_Roll2);
655
#define COS45        7071L              // cos( -45 ) * 10000
Line -... Line 656...
-
 
656
 
-
 
657
    long chanNick =  PPM_in[EE_Parameter.Kanalbelegung[K_NICK]];
-
 
658
    long chanRoll = -PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]];
-
 
659
 
-
 
660
        int  chanNickDiff;
-
 
661
        int  chanRollDiff;
-
 
662
       
-
 
663
        // Stick-Koordinatensystem um -45° (rechts) drehen
-
 
664
        chanNick *= COS45;
-
 
665
        chanRoll *= COS45;
-
 
666
       
-
 
667
        int chanNickTemp = ( chanNick - chanRoll ) / 10000L;
-
 
668
        int chanRollTemp = ( chanRoll + chanNick ) / 10000L;
-
 
669
 
-
 
670
        chanNick =  chanNickTemp;
-
 
671
        chanRoll = -chanRollTemp;
-
 
672
 
-
 
673
        if (chanNick >  MAX_CHAN_VAL)
-
 
674
                chanNick =  MAX_CHAN_VAL;
-
 
675
        if (chanNick < -MAX_CHAN_VAL)
-
 
676
                chanNick = -MAX_CHAN_VAL;
-
 
677
        if (chanRoll >  MAX_CHAN_VAL)
-
 
678
                chanRoll =  MAX_CHAN_VAL;
-
 
679
        if (chanRoll < -MAX_CHAN_VAL)
-
 
680
                chanRoll = -MAX_CHAN_VAL;      
-
 
681
                       
-
 
682
        chanNickDiff = ( ( chanNick - chanNickPrev ) / 3) * 3;
-
 
683
        chanRollDiff = ( ( chanRoll - chanRollPrev ) / 3) * 3;
-
 
684
       
-
 
685
        chanNickPrev = chanNick;
-
 
686
        chanRollPrev = chanRoll;
-
 
687
               
-
 
688
    stick_nick = (stick_nick * 3 + ( (int) chanNick ) * EE_Parameter.Stick_P) / 4;
-
 
689
    stick_nick += chanNickDiff * EE_Parameter.Stick_D;
-
 
690
    StickNick = stick_nick - GPS_Nick;
659
 
691
 
-
 
692
    stick_roll = (stick_roll * 3 + ( (int) chanRoll ) * EE_Parameter.Stick_P) / 4;
-
 
693
    stick_roll += chanRollDiff * EE_Parameter.Stick_D;
Line 660... Line 694...
660
//    StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4;
694
    StickRoll = stick_roll - GPS_Roll;
661
 
695
 
Line 662... Line 696...
662
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];
696
    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];