Rev 123 | Go to most recent revision | Show entire file | Regard 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 601... | Line 607... | ||
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)) |
Line 613... | Line 619... | ||
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--; |
617 | } |
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--; |
|
- | 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 |
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; |