Rev 124 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 124 | Rev 136 | ||
|---|---|---|---|
| Line 76... | Line 76... | ||
| 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 |
79 | //Salvo 2.9.2007 Ersatzkompass |
| 80 | volatile long GyroKomp_Int,GyroKomp_Int2; |
80 | volatile long GyroKomp_Int,GyroKomp_Int2; |
| - | 81 | volatile int GyroKomp_Inc_Grad; // Gyroincrements / Grad |
|
| - | 82 | volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
|
| 81 | // Salvo End |
83 | // Salvo End |
| 82 | float GyroFaktor; |
84 | float GyroFaktor; |
| 83 | float IntegralFaktor; |
85 | float IntegralFaktor; |
| Line 84... | Line 86... | ||
| 84 | 86 | ||
| Line 173... | Line 175... | ||
| 173 | GPS_Neutral(); |
175 | GPS_Neutral(); |
| 174 | beeptime = 50; |
176 | beeptime = 50; |
| 175 | //Salvo 2.9.2007 Ersatzkompass |
177 | //Salvo 2.9.2007 Ersatzkompass |
| 176 | GyroKomp_Int = 0; |
178 | GyroKomp_Int = 0; |
| 177 | GyroKomp_Int2 = 0; |
179 | GyroKomp_Int2 = 0; |
| - | 180 | GyroKomp_Inc_Grad = GYROKOMP_INC_GRAD_DEFAULT; |
|
| 178 | // Salvo End |
181 | // Salvo End |
| 179 | } |
182 | } |
| Line 180... | Line 183... | ||
| 180 | 183 | ||
| 181 | //############################################################################ |
184 | //############################################################################ |
| Line 603... | Line 606... | ||
| 603 | // Gyro-Drift kompensieren |
606 | // Gyro-Drift kompensieren |
| 604 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
607 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 605 | #define DRIFT_FAKTOR 3 |
608 | #define DRIFT_FAKTOR 3 |
| 606 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
609 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
| 607 | { |
610 | { |
| - | 611 | // Salvo 8.9.2007 Ersatzkompass ******* |
|
| - | 612 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
| - | 613 | if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) |
|
| - | 614 | { |
|
| - | 615 | // GyroKomp_Int = GyroKomp_Int- (long)(GYROKOMP_INC_GRAD_DEFAULT *360); |
|
| - | 616 | GyroKomp_Int = 0; |
|
| - | 617 | } |
|
| - | 618 | ANALOG_ON; // ADC einschalten |
|
| - | 619 | // Salvo End |
|
| - | 620 | ||
| 608 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
621 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
| 609 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
622 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
| 610 | ZaehlMessungen = 0; |
623 | ZaehlMessungen = 0; |
| 611 | // Salvo 1.9.2007 ************************* |
624 | // Salvo 1.9.2007 ************************* |
| 612 | // Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen |
625 | // Abgleich Roll und Nick Gyro vollsteandig nur, wenn nahezu waagrechte Lage, ansonsten abschwaechen |
| Line 707... | Line 720... | ||
| 707 | // Salvo Gewolltes Gieren ignorieren 30.8.2007 ********************** |
720 | // Salvo Gewolltes Gieren ignorieren 30.8.2007 ********************** |
| 708 | Mess_Integral_Gier2 -= tmp_int; |
721 | Mess_Integral_Gier2 -= tmp_int; |
| 709 | // Salvo End ************************* |
722 | // Salvo End ************************* |
| 710 | ANALOG_ON; // ADC einschalten |
723 | ANALOG_ON; // ADC einschalten |
| Line -... | Line 724... | ||
| - | 724 | ||
| - | 725 | // Salvo Ersatzkompass 8.9.2007 ********************** |
|
| - | 726 | if (Kompass_Neuer_Wert > 0) |
|
| - | 727 | { |
|
| - | 728 | w = (abs(Mittelwert_AccNick)); |
|
| - | 729 | v = (abs(Mittelwert_AccRoll)); |
|
| - | 730 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass nur mit Magnetkompass aktualisieren wenn alle sok |
|
| - | 731 | { |
|
| - | 732 | if ((abs(KompassValue - Kompass_Value_Old)) < 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
|
| - | 733 | { |
|
| - | 734 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
| - | 735 | GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
|
| - | 736 | w = KompassValue - GyroKomp_Int; |
|
| - | 737 | if ((w > 0) && (w < 180)) |
|
| - | 738 | { |
|
| - | 739 | ++GyroKomp_Int; |
|
| - | 740 | } |
|
| - | 741 | else if ((w > 0) && (w >= 180)) |
|
| - | 742 | { |
|
| - | 743 | --GyroKomp_Int; |
|
| - | 744 | } |
|
| - | 745 | else if ((w < 0) && (w >= -180)) |
|
| - | 746 | { |
|
| - | 747 | --GyroKomp_Int; |
|
| - | 748 | } |
|
| - | 749 | else if ((w < 0) && (w < -180)) |
|
| - | 750 | { |
|
| - | 751 | ++GyroKomp_Int; |
|
| - | 752 | } |
|
| - | 753 | if (GyroKomp_Int < 0) GyroKomp_Int = GyroKomp_Int + 360; |
|
| - | 754 | ||
| - | 755 | GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
|
| - | 756 | ANALOG_ON; // ADC einschalten |
|
| - | 757 | } |
|
| - | 758 | } |
|
| - | 759 | Kompass_Neuer_Wert = 0; |
|
| - | 760 | } |
|
| - | 761 | ||
| - | 762 | // Salvo End ************************* |
|
| - | 763 | ||
| 711 | 764 | ||
| 712 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
765 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 713 | // Kompass |
766 | // Kompass |
| 714 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
767 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 715 | //KompassValue = 12; |
768 | //KompassValue = 12; |
| Line 764... | Line 817... | ||
| 764 | DebugOut.Analog[4] = MesswertGier; |
817 | DebugOut.Analog[4] = MesswertGier; |
| 765 | DebugOut.Analog[5] = HoehenWert; |
818 | DebugOut.Analog[5] = HoehenWert; |
| 766 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
819 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
| 767 | DebugOut.Analog[7] = GasMischanteil; |
820 | DebugOut.Analog[7] = GasMischanteil; |
| 768 | DebugOut.Analog[8] = KompassValue; |
821 | DebugOut.Analog[8] = KompassValue; |
| - | 822 | DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
| - | 823 | DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT; |
|
| 769 | DebugOut.Analog[9] = GyroKomp_Int; |
824 | DebugOut.Analog[11] = GyroKomp_Inc_Grad; |
| 770 | DebugOut.Analog[10] = GyroKomp_Int2; |
825 | DebugOut.Analog[12] = GyroKomp_Value; |
| Line 771... | Line 826... | ||
| 771 | 826 | ||
| 772 | // DebugOut.Analog[9] = SollHoehe; |
827 | // DebugOut.Analog[9] = SollHoehe; |
| 773 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
828 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
| 774 | // DebugOut.Analog[11] = KompassStartwert; |
829 | // DebugOut.Analog[11] = KompassStartwert; |