Subversion Repositories FlightCtrl

Rev

Rev 1022 | 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]];