Rev 604 | Rev 623 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 604 | Rev 614 | ||
---|---|---|---|
Line 117... | Line 117... | ||
117 | unsigned char Parameter_AchsKopplung1 = 0; |
117 | unsigned char Parameter_AchsKopplung1 = 0; |
118 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
118 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
119 | unsigned char Parameter_DynamicStability = 100; |
119 | unsigned char Parameter_DynamicStability = 100; |
120 | struct mk_param_struct EE_Parameter; |
120 | struct mk_param_struct EE_Parameter; |
121 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
121 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
- | 122 | int MaxStickNick = 0,MaxStickRoll = 0; |
|
Line 122... | Line 123... | ||
122 | 123 | ||
123 | void Piep(unsigned char Anzahl) |
124 | void Piep(unsigned char Anzahl) |
124 | { |
125 | { |
125 | while(Anzahl--) |
126 | while(Anzahl--) |
Line 581... | Line 582... | ||
581 | if(!NewPpmData-- || Notlandung) |
582 | if(!NewPpmData-- || Notlandung) |
582 | { |
583 | { |
583 | int tmp_int; |
584 | int tmp_int; |
584 | static int stick_nick,stick_roll; |
585 | static int stick_nick,stick_roll; |
585 | ParameterZuordnung(); |
586 | ParameterZuordnung(); |
586 | StickNick = (StickNick * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 8; |
587 | StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
587 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
588 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
588 | StickRoll = (StickRoll * 7 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 8; |
589 | StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
589 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
590 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
Line 590... | Line 591... | ||
590 | 591 | ||
591 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
592 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
Line -... | Line 593... | ||
- | 593 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
|
- | 594 | ||
- | 595 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
|
- | 596 | MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
|
- | 597 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
|
- | 598 | MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
|
592 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
599 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
593 | 600 | ||
Line 594... | Line 601... | ||
594 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
601 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
595 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
602 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
Line 627... | Line 634... | ||
627 | } |
634 | } |
Line 628... | Line 635... | ||
628 | 635 | ||
629 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
636 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
630 | if(GyroFaktor < 0) GyroFaktor = 0; |
637 | if(GyroFaktor < 0) GyroFaktor = 0; |
631 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
- | |
632 | /* |
- | |
633 | // greift in den Stick ein, um ungewolltes überschlagen zu verhindern |
- | |
634 | if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
- | |
635 | { |
- | |
636 | if(IntegralNick > 60000) |
- | |
637 | { |
- | |
638 | StickNick -= 8 * EE_Parameter.Stick_P; |
- | |
639 | if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
- | |
640 | } |
- | |
641 | else |
- | |
642 | if(IntegralNick < -60000) |
- | |
643 | { |
- | |
644 | StickNick += 8 * EE_Parameter.Stick_P; |
- | |
645 | if(IntegralNick > 80000) StickNick -= 16 * EE_Parameter.Stick_P; |
- | |
646 | } |
- | |
647 | if(IntegralRoll > 60000) |
- | |
648 | { |
- | |
649 | StickRoll -= 8 * EE_Parameter.Stick_P; |
- | |
650 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
- | |
651 | } |
- | |
652 | else |
- | |
653 | if(IntegralRoll < -60000) |
- | |
654 | { |
- | |
655 | StickRoll += 8 * EE_Parameter.Stick_P; |
- | |
656 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
- | |
657 | } |
- | |
658 | } |
- | |
659 | */ |
638 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
660 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
639 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
661 | // Looping? |
640 | // Looping? |
662 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
641 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
663 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
642 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
Line 747... | Line 726... | ||
747 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
726 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
748 | if(!Looping_Nick && !Looping_Roll) |
727 | if(!Looping_Nick && !Looping_Roll) |
749 | { |
728 | { |
750 | long tmp_long, tmp_long2; |
729 | long tmp_long, tmp_long2; |
751 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
730 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
752 | tmp_long /= 16; |
- | |
753 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
731 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
- | 732 | tmp_long /= 16; |
|
754 | tmp_long2 /= 16; |
733 | tmp_long2 /= 16; |
- | 734 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
|
- | 735 | { |
|
- | 736 | tmp_long /= 3; |
|
- | 737 | tmp_long2 /= 3; |
|
- | 738 | } |
|
- | 739 | ||
755 | #define AUSGLEICH 32 //(Parameter_UserParam1 / 2) |
740 | #define AUSGLEICH 32 |
756 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
741 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
757 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
742 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
758 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
743 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
759 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
744 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
- | 745 | ||
760 | Mess_IntegralNick -= tmp_long; |
746 | Mess_IntegralNick -= tmp_long; |
761 | Mess_IntegralRoll -= tmp_long2; |
747 | Mess_IntegralRoll -= tmp_long2; |
762 | } |
748 | } |
763 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
749 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 776... | Line 762... | ||
776 | IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
762 | IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
777 | #define MAX_I 0//(Poti2/10) |
763 | #define MAX_I 0//(Poti2/10) |
778 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
764 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
779 | IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
765 | IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
780 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
766 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
781 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
- | |
782 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
767 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
783 | IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
768 | IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
784 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
769 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
- | 770 | ||
- | 771 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
|
785 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
772 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
Line -... | Line 773... | ||
- | 773 | ||
- | 774 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
|
786 | 775 | { |
|
787 | // Mess_IntegralNick -= ausgleichNick; |
776 | LageKorrekturNick /= 2; |
- | 777 | LageKorrekturNick /= 2; |
|
Line 788... | Line 778... | ||
788 | // Mess_IntegralRoll -= ausgleichRoll; |
778 | } |
789 | 779 | ||
790 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
780 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
791 | // Gyro-Drift ermitteln |
781 | // Gyro-Drift ermitteln |
Line 881... | Line 871... | ||
881 | } else last_r_n = 0; |
871 | } else last_r_n = 0; |
882 | } else |
872 | } else |
883 | { |
873 | { |
884 | cnt = 0; |
874 | cnt = 0; |
885 | } |
875 | } |
886 | DebugOut.Analog[27] = ausgleichRoll; |
- | |
- | 876 | ||
887 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
877 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
888 | //if(cnt > 1) beeptime = 50; |
878 | //if(cnt > 1) beeptime = 50; |
889 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
879 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
890 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
880 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
- | 881 | DebugOut.Analog[27] = ausgleichRoll; |
|
891 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
882 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
892 | DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
883 | DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll); |
893 | } |
884 | } |
894 | else |
885 | else |
895 | { |
886 | { |
Line 1092... | Line 1083... | ||
1092 | // Nick-Achse |
1083 | // Nick-Achse |
1093 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1084 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1094 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1085 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1095 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1086 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1096 | else SummeNick += DiffNick; // I-Anteil bei HH |
1087 | else SummeNick += DiffNick; // I-Anteil bei HH |
1097 | if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ; |
1088 | // if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ; |
1098 | if(SummeNick > 16000) SummeNick = 16000; |
1089 | if(SummeNick > 16000) SummeNick = 16000; |
1099 | if(SummeNick < -16000) SummeNick = -16000; |
1090 | if(SummeNick < -16000) SummeNick = -16000; |
1100 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1091 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1101 | // Motor Vorn |
1092 | // Motor Vorn |
1102 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1093 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
Line 1118... | Line 1109... | ||
1118 | // Roll-Achse |
1109 | // Roll-Achse |
1119 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1110 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1120 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1111 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1121 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1112 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1122 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1113 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1123 | if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ; |
1114 | // if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ; |
1124 | if(SummeRoll > 16000) SummeRoll = 16000; |
1115 | if(SummeRoll > 16000) SummeRoll = 16000; |
1125 | if(SummeRoll < -16000) SummeRoll = -16000; |
1116 | if(SummeRoll < -16000) SummeRoll = -16000; |
1126 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1117 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1127 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1118 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1128 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1119 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |