Subversion Repositories FlightCtrl

Rev

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

Rev 167 Rev 182
Line 446... Line 446...
446 
        n = Get_Rel_Position();
 446 
        n = Get_Rel_Position();
447 
        if (n == 0)    
 447 
        if (n == 0)    
448 
        {
 448 
        {
449 
                ROT_ON; //led blitzen lassen
 449 
                ROT_ON; //led blitzen lassen
450 
        }
 450 
        }
451 
 
 - 
 
452 
//******PROVISORISCH***************
 451 
//******PROVISORISCH***************
453 
    GRN_ON;
 452 
    GRN_ON;
Line 454... Line 453...
454 
 
 453 
 
455 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
454 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
Line 639... Line 638...
639 
// Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen
 638 
// Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen
640 
                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 639 
                   ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
641 
                        w = (abs(Mittelwert_AccNick));
 640 
                        w = (abs(Mittelwert_AccNick));
642 
                        v = (abs(Mittelwert_AccRoll));
 641 
                        v = (abs(Mittelwert_AccRoll));
643 
                ANALOG_ON;      // ADC einschalten
 642 
                ANALOG_ON;      // ADC einschalten
644 
                        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 - 
 
645 
                        {              
 - 
 
646 
                 if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
 - 
 
647 
                 if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
 - 
 
648 
                 if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
 - 
 
649 
                 if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
 - 
 
650 
                        }
 - 
 
651 
                        else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 - 
 
652 
                        {
 - 
 
653 
                 if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
 643 
                        // Abgleich Roll und Nick Gyro nur wenn auch keine wilden Manoever stattfinden 26.9.2007
654 
                 if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
 644 
                        if ((abs(StickNick - GPS_Nick) < 30) && (abs(StickRoll - GPS_Roll) < 30))
655 
                 if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
 - 
 
656 
                 if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
 - 
 
657 
                        }
 - 
 
658 
                        else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
 - 
 
659 
                        {
 645 
                        {
- 
 
 646 
                                if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
- 
 
 647 
                                {              
- 
 
 648 
                         if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
- 
 
 649 
                         if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
- 
 
 650 
                         if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
- 
 
 651 
                         if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
- 
 
 652 
                                }
- 
 
 653 
                                else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
- 
 
 654 
                                {
- 
 
 655 
                         if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR)   AdNeutralNick++;
- 
 
 656 
                         if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR)  AdNeutralNick--;
- 
 
 657 
                         if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR)   AdNeutralRoll++;
- 
 
 658 
                         if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR)  AdNeutralRoll--;
- 
 
 659 
                                }
- 
 
 660 
                                else  // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht
- 
 
 661 
                                {
660 
                 if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
 662 
                         if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR)   AdNeutralNick++;
661 
                 if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
 663 
                         if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR)  AdNeutralNick--;
662 
                 if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
 664 
                         if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR)   AdNeutralRoll++;
663 
                 if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
 665 
                         if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR)  AdNeutralRoll--;
- 
 
 666 
                                }
664 
                        }
 667 
                        }
665 
// Salvo End
 668 
// Salvo End
Line 666... Line 669...
666 
 
 669 
 
667 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 670 
                        ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
Line 681... Line 684...
681 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 684 
        if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
682 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 685 
        if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
Line 683... Line 686...
683 
 
 686 
 
684 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 687 
 ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
- 
 
 688 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
685 
 // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht *****************
 689 
 
686 
        w = (abs(Mittelwert_AccNick));
 690 
        w = (abs(Mittelwert_AccNick));
687 
        v = (abs(Mittelwert_AccRoll));
 691 
        v = (abs(Mittelwert_AccRoll));
688 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
 692 
        if ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT))
689 
        {
 693 
        {
Line 721... Line 725...
721 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
 725 
// Salvo Gewolltes Gieren ignorieren 30.8.2007 **********************
722 
    Mess_Integral_Gier2  -= tmp_int;  
 726 
    Mess_Integral_Gier2  -= tmp_int;  
723 
// Salvo End *************************
 727 
// Salvo End *************************
724 
 ANALOG_ON;     // ADC einschalten
 728 
 ANALOG_ON;     // ADC einschalten
Line 725... Line 729...
725 
 
 729 
 
726 
// Salvo Ersatzkompass  8.9.2007 **********************
 730 
// Salvo Ersatzkompass  26.9.2007 **********************
727 
        if ((Kompass_Neuer_Wert > 0))
 731 
        if ((Kompass_Neuer_Wert > 0))
728 
        {
 732 
        {
729 
           Kompass_Neuer_Wert = 0;
 733 
           Kompass_Neuer_Wert = 0;
730 
           w = (abs(Mittelwert_AccNick));
 734 
           w = (abs(Mittelwert_AccNick));
731 
           v = (abs(Mittelwert_AccRoll));
 735 
           v = (abs(Mittelwert_AccRoll));
732 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
 736 
           if  ((w  < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok
733 
           {
 737 
           {
734 
                if  ((abs(KompassValue - Kompass_Value_Old)) < 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
 738 
                if  ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen
735 
                 {
 739 
                 {
736 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
 740 
                  ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
737 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
741 
                  magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet
738 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
 742 
                  GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT;
Line 752... Line 756...
752 
                  else if ((w < 0) && (w < -180))
 756 
                  else if ((w < 0) && (w < -180))
753 
                  {
 757 
                  {
754 
                   ++GyroKomp_Int;
 758 
                   ++GyroKomp_Int;
755 
                  }
 759 
                  }
756 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
 760 
                  if (GyroKomp_Int < 0)  GyroKomp_Int = GyroKomp_Int + 360;
757 
 
 - 
 
758 
                   GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
 761 
                  GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern
759 
                  ANALOG_ON;    // ADC einschalten
 762 
                  ANALOG_ON;    // ADC einschalten
760 
                 }
 763 
                 }
761 
           }
 764 
           }
762 
           else magkompass_ok = 0;
 765 
           else magkompass_ok = 0;
763 
        }
 766 
        }