Rev 122 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 122 | Rev 123 | ||
---|---|---|---|
Line 87... | Line 87... | ||
87 | volatile unsigned char SenderOkay = 0; |
87 | volatile unsigned char SenderOkay = 0; |
88 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
88 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
89 | char MotorenEin = 0; |
89 | char MotorenEin = 0; |
90 | int HoehenWert = 0; |
90 | int HoehenWert = 0; |
91 | int SollHoehe = 0; |
91 | int SollHoehe = 0; |
- | 92 | int w,v; |
|
Line 92... | Line 93... | ||
92 | 93 | ||
93 | float Kp = FAKTOR_P; |
94 | float Kp = FAKTOR_P; |
Line 94... | Line 95... | ||
94 | float Ki = FAKTOR_I; |
95 | float Ki = FAKTOR_I; |
Line 599... | Line 600... | ||
599 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
600 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
600 | { |
601 | { |
601 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
602 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
602 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
603 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
603 | ZaehlMessungen = 0; |
604 | ZaehlMessungen = 0; |
- | 605 | // Salvo 1.9.2007 ************************* |
|
- | 606 | // Abgleich Roll und Nick Gyro nur, wenn nahezu waagrechte Lage |
|
- | 607 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
|
- | 608 | w = (abs(Mittelwert_AccNick)); |
|
- | 609 | v = (abs(Mittelwert_AccRoll)); |
|
- | 610 | ANALOG_ON; // ADC einschalten |
|
- | 611 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
|
- | 612 | { |
|
604 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
613 | if(IntegralFehlerNick > 500/DRIFT_FAKTOR) AdNeutralNick++; |
605 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
614 | if(IntegralFehlerNick < -500/DRIFT_FAKTOR) AdNeutralNick--; |
606 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
615 | if(IntegralFehlerRoll > 500/DRIFT_FAKTOR) AdNeutralRoll++; |
607 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
616 | if(IntegralFehlerRoll < -500/DRIFT_FAKTOR) AdNeutralRoll--; |
- | 617 | } |
|
- | 618 | // Salvo End |
|
- | 619 | ||
608 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
620 | // Salvo 31.8.2007 Abgleich Giergyro nur wenn Kompass aktiv und ok ist *********************** |
609 | // Ohne Kompass wird die Gyrodrift durch die Driftkompensation nur verschlimmert |
621 | // Ohne Kompass wird die Pseudo-Gyrodrift durch die Driftkompensation nur verschlimmert |
610 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
622 | // Ohne Driftkompensation ist die Gierachse wesentlich stabiler |
611 | if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
623 | if ((EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) && (!SignalSchlecht)) |
612 | { |
624 | { |
613 | if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
625 | if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; |
614 | if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
626 | if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; |
Line 628... | Line 640... | ||
628 | // Integrale auf ACC-Signal abgleichen |
640 | // Integrale auf ACC-Signal abgleichen |
629 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
641 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
630 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16; |
642 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick) / 16; |
631 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16; |
643 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll) / 16; |
632 | #define AUSGLEICH 500 |
644 | #define AUSGLEICH 500 |
633 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
645 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
634 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
646 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
635 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
647 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
636 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
648 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
- | 649 | ||
637 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
650 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
- | 651 | // Salvo 1.9.2007 Volle Korrektur nur wenn waagrechte Lage, sonst abgeschawecht ***************** |
|
- | 652 | w = (abs(Mittelwert_AccNick)); |
|
- | 653 | v = (abs(Mittelwert_AccRoll)); |
|
- | 654 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) |
|
- | 655 | { |
|
638 | Mess_IntegralNick -= tmp_long; |
656 | Mess_IntegralNick -= tmp_long; |
639 | Mess_IntegralRoll -= tmp_long2; |
657 | Mess_IntegralRoll -= tmp_long2; |
- | 658 | } |
|
- | 659 | else if ((w < 2 * ACC_WAAGRECHT_LIMIT) && (v < 2 * ACC_WAAGRECHT_LIMIT)) |
|
- | 660 | { |
|
- | 661 | Mess_IntegralNick -= tmp_long/8; |
|
- | 662 | Mess_IntegralRoll -= tmp_long/8; |
|
- | 663 | } |
|
- | 664 | else if ((w < 4 * ACC_WAAGRECHT_LIMIT) && (v < 4 * ACC_WAAGRECHT_LIMIT)) |
|
- | 665 | { |
|
- | 666 | Mess_IntegralNick -= tmp_long/16; |
|
- | 667 | Mess_IntegralRoll -= tmp_long/16; |
|
- | 668 | } |
|
- | 669 | else |
|
- | 670 | { |
|
- | 671 | Mess_IntegralNick -= tmp_long/32; |
|
- | 672 | Mess_IntegralRoll -= tmp_long2/32; |
|
- | 673 | } |
|
- | 674 | // Salvo End *********************** |
|
640 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
675 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
641 | // Gieren |
676 | // Gieren |
642 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
677 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
643 | sollGier = StickGier; |
678 | sollGier = StickGier; |
644 | if(abs(StickGier) > 35) |
679 | if(abs(StickGier) > 35) |
Line 658... | Line 693... | ||
658 | // Kompass |
693 | // Kompass |
659 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
694 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
660 | //KompassValue = 12; |
695 | //KompassValue = 12; |
661 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
696 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
662 | { |
697 | { |
663 | int w,v; |
- | |
664 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
698 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
665 | v = abs(IntegralRoll /512); |
699 | v = abs(IntegralRoll /512); |
666 | if(v > w) w = v; // grösste Neigung ermitteln |
700 | if(v > w) w = v; // grösste Neigung ermitteln |
667 | if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
701 | if(w < 20 && NeueKompassRichtungMerken && !SignalSchlecht) |
668 | { |
702 | { |
Line 710... | Line 744... | ||
710 | DebugOut.Analog[4] = MesswertGier; |
744 | DebugOut.Analog[4] = MesswertGier; |
711 | DebugOut.Analog[5] = HoehenWert; |
745 | DebugOut.Analog[5] = HoehenWert; |
712 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
746 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
713 | DebugOut.Analog[7] = GasMischanteil; |
747 | DebugOut.Analog[7] = GasMischanteil; |
714 | DebugOut.Analog[8] = KompassValue; |
748 | DebugOut.Analog[8] = KompassValue; |
715 | DebugOut.Analog[9] = Mess_Integral_Gier2; |
749 | DebugOut.Analog[9] = tmp_long; |
- | 750 | DebugOut.Analog[10] = tmp_long2; |
|
716 | // DebugOut.Analog[9] = SollHoehe; |
751 | // DebugOut.Analog[9] = SollHoehe; |
717 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
752 | // DebugOut.Analog[10] = Mess_Integral_Gier / 128; |
718 | // DebugOut.Analog[11] = KompassStartwert; |
753 | // DebugOut.Analog[11] = KompassStartwert; |
719 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
754 | // DebugOut.Analog[10] = Parameter_Gyro_I; |
720 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |
755 | // DebugOut.Analog[10] = EE_Parameter.Gyro_I; |