Rev 153 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 153 | Rev 155 | ||
|---|---|---|---|
| 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 | uint8_t magkompass_ok=0; |
|
| - | 80 | ||
| 79 | //Salvo 2.9.2007 Ersatzkompass |
81 | //Salvo 2.9.2007 Ersatzkompass |
| 80 | volatile long GyroKomp_Int,GyroKomp_Int2; |
82 | 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 |
83 | volatile int GyroKomp_Value; // Der ermittelte Kompasswert aus Gyro und Magnetkompass |
| 83 | // Salvo End |
84 | // Salvo End |
| 84 | float GyroFaktor; |
85 | float GyroFaktor; |
| 85 | float IntegralFaktor; |
86 | float IntegralFaktor; |
| Line 175... | Line 176... | ||
| 175 | GPS_Neutral(); |
176 | GPS_Neutral(); |
| 176 | beeptime = 50; |
177 | beeptime = 50; |
| 177 | //Salvo 2.9.2007 Ersatzkompass |
178 | //Salvo 2.9.2007 Ersatzkompass |
| 178 | GyroKomp_Int = 0; |
179 | GyroKomp_Int = 0; |
| 179 | GyroKomp_Int2 = 0; |
180 | GyroKomp_Int2 = 0; |
| 180 | GyroKomp_Inc_Grad = GYROKOMP_INC_GRAD_DEFAULT; |
- | |
| 181 | // Salvo End |
181 | // Salvo End |
| 182 | } |
182 | } |
| Line 183... | Line 183... | ||
| 183 | 183 | ||
| 184 | //############################################################################ |
184 | //############################################################################ |
| Line 319... | Line 319... | ||
| 319 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
319 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
| 320 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
320 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
| 321 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
321 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
| 322 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
322 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
| 323 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
323 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
| 324 | EE_Parameter.NotGas = 90; // Wert : 0-250 // Gaswert bei Empangsverlust |
324 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
| 325 | EE_Parameter.NotGasZeit = 50; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
325 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
| 326 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
326 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
| 327 | EE_Parameter.I_Faktor = 5; |
327 | EE_Parameter.I_Faktor = 5; |
| 328 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
328 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
| 329 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
329 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
| 330 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
330 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
| Line 362... | Line 362... | ||
| 362 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
362 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
| 363 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
363 | EE_Parameter.KompassWirkung = 64; // Wert : 0-250 |
| 364 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
364 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
| 365 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
365 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
| 366 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
366 | EE_Parameter.UnterspannungsWarnung = 100; // Wert : 0-250 |
| 367 | EE_Parameter.NotGas = 90; // Wert : 0-250 // Gaswert bei Empangsverlust |
367 | EE_Parameter.NotGas = 100; // Wert : 0-250 // Gaswert bei Empangsverlust |
| 368 | EE_Parameter.NotGasZeit = 50; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
368 | EE_Parameter.NotGasZeit = 60; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
| 369 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
369 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
| 370 | EE_Parameter.I_Faktor = 5; |
370 | EE_Parameter.I_Faktor = 5; |
| 371 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
371 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
| 372 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
372 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
| 373 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
373 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
| Line 614... | Line 614... | ||
| 614 | // Gyro-Drift kompensieren |
614 | // Gyro-Drift kompensieren |
| 615 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
615 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 616 | #define DRIFT_FAKTOR 3 |
616 | #define DRIFT_FAKTOR 3 |
| 617 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
617 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
| 618 | { |
618 | { |
| 619 | // Salvo 8.9.2007 Ersatzkompass ******* |
619 | // Salvo 12.9.2007 Ersatzkompass ******* |
| 620 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
620 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| 621 | if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) |
621 | if (GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT >=360 ) GyroKomp_Int = 0; |
| 622 | { |
- | |
| 623 | // GyroKomp_Int = GyroKomp_Int- (long)(GYROKOMP_INC_GRAD_DEFAULT *360); |
- | |
| 624 | GyroKomp_Int = 0; |
- | |
| 625 | } |
- | |
| 626 | ANALOG_ON; // ADC einschalten |
622 | ANALOG_ON; // ADC einschalten |
| 627 | ROT_OFF; |
623 | ROT_OFF; |
| 628 | // Salvo End |
624 | // Salvo End |
| Line 629... | Line 625... | ||
| 629 | 625 | ||
| Line 660... | Line 656... | ||
| 660 | // Salvo End |
656 | // Salvo End |
| Line 661... | Line 657... | ||
| 661 | 657 | ||
| 662 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
658 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
| 663 | // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
659 | // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
| 664 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
660 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
| - | 661 | // if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
|
| - | 662 | // Salvo 13.9.2007 |
|
| 665 | if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
663 | // if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (magkompass_ok >0)) |
| 666 | { |
664 | // { |
| 667 | if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
665 | // if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
| 668 | if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
666 | // if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
| 669 | } |
- | |
| 670 | else |
- | |
| 671 | { |
667 | // } |
| 672 | Mess_Integral_Gier2 = 0; |
668 | // else Mess_Integral_Gier2 = 0; |
| 673 | } |
669 | |
| 674 | // Salvo End *********************** |
670 | // Salvo End *********************** |
| 675 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
671 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| 676 | Mess_IntegralNick2 = IntegralNick; |
672 | Mess_IntegralNick2 = IntegralNick; |
| 677 | Mess_IntegralRoll2 = IntegralRoll; |
673 | Mess_IntegralRoll2 = IntegralRoll; |
| Line 716... | Line 712... | ||
| 716 | // Salvo End *********************** |
712 | // Salvo End *********************** |
| 717 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
713 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 718 | // Gieren |
714 | // Gieren |
| 719 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
715 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 720 | sollGier = StickGier; |
716 | sollGier = StickGier; |
| 721 | if(abs(StickGier) > 35) |
717 | if(abs(StickGier) > 30) |
| 722 | { |
718 | { |
| 723 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
719 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
| 724 | } |
720 | } |
| 725 | tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo |
721 | tmp_int = EE_Parameter.Gier_P * (sollGier * abs(sollGier)) / 256; // expo |
| 726 | Mess_Integral_Gier -= tmp_int; |
722 | Mess_Integral_Gier -= tmp_int; |
| Line 732... | Line 728... | ||
| 732 | ANALOG_ON; // ADC einschalten |
728 | ANALOG_ON; // ADC einschalten |
| Line 733... | Line 729... | ||
| 733 | 729 | ||
| 734 | // Salvo Ersatzkompass 8.9.2007 ********************** |
730 | // Salvo Ersatzkompass 8.9.2007 ********************** |
| 735 | if (Kompass_Neuer_Wert > 0) |
731 | if (Kompass_Neuer_Wert > 0) |
| - | 732 | { |
|
| 736 | { |
733 | Kompass_Neuer_Wert = 0; |
| 737 | w = (abs(Mittelwert_AccNick)); |
734 | w = (abs(Mittelwert_AccNick)); |
| 738 | v = (abs(Mittelwert_AccRoll)); |
735 | v = (abs(Mittelwert_AccRoll)); |
| 739 | 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 |
| 740 | { |
737 | { |
| 741 | 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 |
| 742 | { |
739 | { |
| - | 740 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
| 743 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
741 | magkompass_ok = 1; // Flag dass Magnetkompass stabil arbeitet |
| 744 | GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
742 | GyroKomp_Int = (GyroKomp_Int )/GYROKOMP_INC_GRAD_DEFAULT; |
| 745 | w = KompassValue - GyroKomp_Int; |
743 | w = KompassValue - GyroKomp_Int; |
| 746 | if ((w > 0) && (w < 180)) |
744 | if ((w > 0) && (w < 180)) |
| 747 | { |
745 | { |
| Line 763... | Line 761... | ||
| 763 | 761 | ||
| 764 | GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
762 | GyroKomp_Int = (GyroKomp_Int%360) * GYROKOMP_INC_GRAD_DEFAULT; // An Magnetkompasswert annaehern |
| 765 | ANALOG_ON; // ADC einschalten |
763 | ANALOG_ON; // ADC einschalten |
| 766 | } |
764 | } |
| 767 | } |
765 | } |
| 768 | Kompass_Neuer_Wert = 0; |
766 | else magkompass_ok = 0; |
| Line 769... | Line 767... | ||
| 769 | } |
767 | } |
| Line 770... | Line 768... | ||
| 770 | 768 | ||
| 771 | // Salvo End ************************* |
769 | // Salvo End ************************* |
| 772 | 770 | ||
| 773 | - | ||
| 774 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
771 | |
| 775 | // Kompass |
772 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 776 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
773 | // Kompass |
| 777 | //KompassValue = 12; |
774 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 778 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
775 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
| - | 776 | { |
|
| - | 777 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
|
| - | 778 | v = abs(IntegralRoll /512); |
|
| 779 | { |
779 | if(v > w) w = v; // grösste Neigung ermitteln |
| - | 780 | // Salvo 12.9.2007 Kompassdaempfung abschalten weil Ersatzkompass verwendet wird |
|
| 780 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
781 | // w=0, v=0; |
| - | 782 | //Salvo End |
|
| 781 | v = abs(IntegralRoll /512); |
783 | // if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
| - | 784 | if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
|
| - | 785 | { |
|
| - | 786 | // Salvo 12.9.2007 Ersatzkompass nehmen statt Magnetkompass |
|
| - | 787 | // KompassStartwert = KompassValue; |
|
| - | 788 | // KompassStartwert = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
|
| 782 | if(v > w) w = v; // grösste Neigung ermitteln |
789 | // Salvo End ************************* |
| 783 | if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
790 | // Salvo 13.9.2007 |
| - | 791 | KompassStartwert = KompassValue; |
|
| - | 792 | // Salvo End |
|
| - | 793 | NeueKompassRichtungMerken = 0; |
|
| 784 | { |
794 | } |
| 785 | KompassStartwert = KompassValue; |
795 | // Salvo 13.9.2007 |
| 786 | NeueKompassRichtungMerken = 0; |
796 | w=0; |
| 787 | } |
797 | // Salvo End |
| 788 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
798 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| Line -... | Line 799... | ||
| - | 799 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
|
| - | 800 | if(w > 0) |
|
| - | 801 | { |
|
| 789 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
802 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
| - | 803 | ||
| 790 | if(w > 0) |
804 | // Salvo Kompasssteuerung ********************** |
| 791 | { |
805 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| 792 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
806 | // Salvo End |
| 793 | 807 | // Salvo 30.8.2007 Winkelbegrenzung ********************** |
|
| 794 | // Salvo 30.8.2007 Winkelbegrenzung ********************** |
808 | /* |
| 795 | if ((!SignalSchlecht) ) |
809 | if ((!SignalSchlecht) ) |
| 796 | { |
810 | { |
| - | 811 | if (abs(KompassRichtung) < 135 ) |
|
| 797 | if (abs(KompassRichtung) < 135 ) |
812 | { |
| Line 798... | Line 813... | ||
| 798 | { |
813 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| 799 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
814 | } |
| 800 | } |
815 | } |
| Line 827... | Line 842... | ||
| 827 | DebugOut.Analog[5] = HoehenWert; |
842 | DebugOut.Analog[5] = HoehenWert; |
| 828 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
843 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
| 829 | DebugOut.Analog[7] = GasMischanteil; |
844 | DebugOut.Analog[7] = GasMischanteil; |
| 830 | DebugOut.Analog[8] = KompassValue; |
845 | DebugOut.Analog[8] = KompassValue; |
| 831 | // ******provisorisch |
846 | // ******provisorisch |
| 832 | DebugOut.Analog[9] = cnt1; |
847 | // DebugOut.Analog[9] = cnt1; |
| 833 | // DebugOut.Analog[10] = cnt1; |
848 | // DebugOut.Analog[10] = cnt1; |
| 834 | // DebugOut.Analog[11] = cnt2; |
849 | // DebugOut.Analog[11] = cnt2; |
| 835 | DebugOut.Analog[10] = (gps_act_position.utm_east/10) % 10000; |
850 | // DebugOut.Analog[10] = (gps_act_position.utm_east/10) % 10000; |
| 836 | DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000; |
851 | // DebugOut.Analog[11] = (gps_act_position.utm_north/10) % 10000; |
| 837 | // ******provisorisch |
852 | // ******provisorisch |
| Line 838... | Line 853... | ||
| 838 | 853 | ||
| 839 | /* |
854 | |
| 840 | DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
855 | DebugOut.Analog[9] = GyroKomp_Int/GYROKOMP_INC_GRAD_DEFAULT; |
| - | 856 | DebugOut.Analog[10] = magkompass_ok; |
|
| - | 857 | DebugOut.Analog[11] = Mess_Integral_Gier2; |
|
| 841 | DebugOut.Analog[10] = GyroKomp_Int2/GYROKOMP_INC_GRAD_DEFAULT; |
858 | /* |
| 842 | DebugOut.Analog[11] = GyroKomp_Inc_Grad; |
859 | DebugOut.Analog[11] = GyroKomp_Inc_Grad; |
| 843 | DebugOut.Analog[12] = GyroKomp_Value; |
860 | DebugOut.Analog[12] = GyroKomp_Value; |
| 844 | */ |
861 | */ |
| 845 | // DebugOut.Analog[9] = SollHoehe; |
862 | // DebugOut.Analog[9] = SollHoehe; |