Rev 123 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 123 | Rev 124 | ||
---|---|---|---|
Line 74... | Line 74... | ||
74 | volatile int KompassRichtung = 0; |
74 | volatile int KompassRichtung = 0; |
75 | unsigned char MAX_GAS,MIN_GAS; |
75 | unsigned char MAX_GAS,MIN_GAS; |
76 | unsigned char Notlandung = 0; |
76 | unsigned char Notlandung = 0; |
77 | unsigned char HoehenReglerAktiv = 0; |
77 | unsigned char HoehenReglerAktiv = 0; |
78 | static int SignalSchlecht = 0; |
78 | static int SignalSchlecht = 0; |
- | 79 | //Salvo 2.9.2007 Ersatzkompass |
|
- | 80 | volatile long GyroKomp_Int,GyroKomp_Int2; |
|
79 | 81 | // Salvo End |
|
80 | float GyroFaktor; |
82 | float GyroFaktor; |
81 | float IntegralFaktor; |
83 | float IntegralFaktor; |
Line 82... | Line 84... | ||
82 | 84 | ||
83 | volatile int DiffNick,DiffRoll; |
85 | volatile int DiffNick,DiffRoll; |
Line 168... | Line 170... | ||
168 | HoeheD = 0; |
170 | HoeheD = 0; |
169 | Mess_Integral_Hoch = 0; |
171 | Mess_Integral_Hoch = 0; |
170 | KompassStartwert = KompassValue; |
172 | KompassStartwert = KompassValue; |
171 | GPS_Neutral(); |
173 | GPS_Neutral(); |
172 | beeptime = 50; |
174 | beeptime = 50; |
- | 175 | //Salvo 2.9.2007 Ersatzkompass |
|
- | 176 | GyroKomp_Int = 0; |
|
- | 177 | GyroKomp_Int2 = 0; |
|
- | 178 | // Salvo End |
|
173 | } |
179 | } |
Line 174... | Line 180... | ||
174 | 180 | ||
175 | //############################################################################ |
181 | //############################################################################ |
176 | // Bildet den Mittelwert aus den Messwerten |
182 | // Bildet den Mittelwert aus den Messwerten |
Line 596... | Line 602... | ||
596 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
602 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
597 | // Gyro-Drift kompensieren |
603 | // Gyro-Drift kompensieren |
598 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
604 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
599 | #define DRIFT_FAKTOR 3 |
605 | #define DRIFT_FAKTOR 3 |
600 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
606 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
601 | { |
607 | { |
602 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
608 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
603 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
609 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
604 | ZaehlMessungen = 0; |
610 | ZaehlMessungen = 0; |
605 | // Salvo 1.9.2007 ************************* |
611 | // Salvo 1.9.2007 ************************* |
606 | // Abgleich Roll und Nick Gyro nur, wenn nahezu waagrechte Lage |
612 | // Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen |
607 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
613 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
608 | w = (abs(Mittelwert_AccNick)); |
614 | w = (abs(Mittelwert_AccNick)); |
609 | v = (abs(Mittelwert_AccRoll)); |
615 | v = (abs(Mittelwert_AccRoll)); |
610 | ANALOG_ON; // ADC einschalten |
616 | ANALOG_ON; // ADC einschalten |
611 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
617 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
612 | { |
618 | { |
613 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
619 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
614 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
620 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
615 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
621 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
616 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
622 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
- | 623 | } |
|
- | 624 | else if ((w < 2*ACC_WAAGRECHT_LIMIT) && (v < 2*ACC_WAAGRECHT_LIMIT)) // langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
|
- | 625 | { |
|
- | 626 | if(IntegralFehlerNick > 500*2/DRIFT_FAKTOR) AdNeutralNick++; |
|
- | 627 | if(IntegralFehlerNick < -500*2/DRIFT_FAKTOR) AdNeutralNick--; |
|
- | 628 | if(IntegralFehlerRoll > 500*2/DRIFT_FAKTOR) AdNeutralRoll++; |
|
- | 629 | if(IntegralFehlerRoll < -500*2/DRIFT_FAKTOR) AdNeutralRoll--; |
|
- | 630 | } |
|
- | 631 | else // noch langsamer kompensieren, weil ACC Werte unsicherer sind, was die Neigung angeht |
|
- | 632 | { |
|
- | 633 | if(IntegralFehlerNick > 500*4/DRIFT_FAKTOR) AdNeutralNick++; |
|
- | 634 | if(IntegralFehlerNick < -500*4/DRIFT_FAKTOR) AdNeutralNick--; |
|
- | 635 | if(IntegralFehlerRoll > 500*4/DRIFT_FAKTOR) AdNeutralRoll++; |
|
- | 636 | if(IntegralFehlerRoll < -500*4/DRIFT_FAKTOR) AdNeutralRoll--; |
|
617 | } |
637 | } |
618 | // Salvo End |
638 | // Salvo End |
Line 619... | Line 639... | ||
619 | 639 | ||
620 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
640 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
621 | // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
641 | // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
622 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
642 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
623 | if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
643 | if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
624 | { |
644 | { |
625 | if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
645 | if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
626 | if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
646 | if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
627 | } |
647 | } |
628 | else |
648 | else |
629 | { |
649 | { |
630 | Mess_Integral_Gier2 = 0; |
650 | Mess_Integral_Gier2 = 0; |
631 | } |
651 | } |
632 | // Salvo End *********************** |
652 | // Salvo End *********************** |
633 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
653 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
634 | Mess_IntegralNick2 = IntegralNick; |
654 | Mess_IntegralNick2 = IntegralNick; |
635 | Mess_IntegralRoll2 = IntegralRoll; |
655 | Mess_IntegralRoll2 = IntegralRoll; |
636 | Mess_Integral_Gier2 = Integral_Gier; |
656 | Mess_Integral_Gier2 = Integral_Gier; |
637 | ANALOG_ON; // ADC einschalten |
657 | ANALOG_ON; // ADC einschalten |
638 | } |
658 | } |
639 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
659 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
640 | // Integrale auf ACC-Signal abgleichen |
660 | // Integrale auf ACC-Signal abgleichen |
641 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
661 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 656... | Line 676... | ||
656 | Mess_IntegralNick -= tmp_long; |
676 | Mess_IntegralNick -= tmp_long; |
657 | Mess_IntegralRoll -= tmp_long2; |
677 | Mess_IntegralRoll -= tmp_long2; |
658 | } |
678 | } |
659 | else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
679 | else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
660 | { |
680 | { |
661 | Mess_IntegralNick -= tmp_long/8; |
681 | Mess_IntegralNick -= tmp_long/2; //Vorher 8 |
662 | Mess_IntegralRoll -= tmp_long/8; |
682 | Mess_IntegralRoll -= tmp_long2/2; |
663 | } |
683 | } |
664 | else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
684 | else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
665 | { |
685 | { |
666 | Mess_IntegralNick -= tmp_long/16; |
686 | Mess_IntegralNick -= tmp_long/4; |
667 | Mess_IntegralRoll -= tmp_long/16; |
687 | Mess_IntegralRoll -= tmp_long2/4; |
668 | } |
688 | } |
669 | else |
689 | else |
670 | { |
690 | { |
671 | Mess_IntegralNick -= tmp_long/32; |
691 | Mess_IntegralNick -= tmp_long/8; |
672 | Mess_IntegralRoll -= tmp_long2/32; |
692 | Mess_IntegralRoll -= tmp_long2/8; |
673 | } |
693 | } |
674 | // Salvo End *********************** |
694 | // Salvo End *********************** |
675 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
695 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
676 | // Gieren |
696 | // Gieren |
677 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 744... | Line 764... | ||
744 | DebugOut.Analog[4] = MesswertGier; |
764 | DebugOut.Analog[4] = MesswertGier; |
745 | DebugOut.Analog[5] = HoehenWert; |
765 | DebugOut.Analog[5] = HoehenWert; |
746 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
766 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
747 | DebugOut.Analog[7] = GasMischanteil; |
767 | DebugOut.Analog[7] = GasMischanteil; |
748 | DebugOut.Analog[8] = KompassValue; |
768 | DebugOut.Analog[8] = KompassValue; |
749 | DebugOut.Analog[9] = tmp_long; |
769 | DebugOut.Analog[9] = GyroKomp_Int; |
750 | DebugOut.Analog[10] = tmp_long2; |
770 | DebugOut.Analog[10] = GyroKomp_Int2; |
- | 771 | ||
751 | // DebugOut.Analog[9] = SollHoehe; |
772 | // DebugOut.Analog[9] = SollHoehe; |
752 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
773 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
753 | // DebugOut.Analog[11] = KompassStartwert; |
774 | // DebugOut.Analog[11] = KompassStartwert; |
754 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
775 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
755 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
776 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |