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; |