Rev 720 | Rev 744 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 720 | Rev 723 | ||
---|---|---|---|
Line 189... | Line 189... | ||
189 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
189 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
190 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
190 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
191 | ExternHoehenValue = 0; |
191 | ExternHoehenValue = 0; |
192 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
192 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
193 | GierGyroFehler = 0; |
193 | GierGyroFehler = 0; |
- | 194 | SendVersionToNavi = 1; |
|
194 | } |
195 | } |
Line 195... | Line 196... | ||
195 | 196 | ||
196 | //############################################################################ |
197 | //############################################################################ |
197 | // Bearbeitet die Messwerte |
198 | // Bearbeitet die Messwerte |
Line 590... | Line 591... | ||
590 | if(!NewPpmData-- || Notlandung) |
591 | if(!NewPpmData-- || Notlandung) |
591 | { |
592 | { |
592 | int tmp_int; |
593 | int tmp_int; |
593 | static int stick_nick,stick_roll; |
594 | static int stick_nick,stick_roll; |
594 | ParameterZuordnung(); |
595 | ParameterZuordnung(); |
595 | StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
596 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
596 | StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
597 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
- | 598 | StickNick = stick_nick - GPS_Nick; |
|
- | 599 | // StickNick = (StickNick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
|
- | 600 | ||
597 | StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
601 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
598 | StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
602 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
- | 603 | StickRoll = stick_roll - GPS_Roll; |
|
- | 604 | ||
- | 605 | // StickRoll = (StickRoll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
|
Line 599... | Line 606... | ||
599 | 606 | ||
600 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
607 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
Line 601... | Line 608... | ||
601 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
608 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
602 | 609 | ||
603 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
610 | /* if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]) > MaxStickNick) |
604 | MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
611 | MaxStickNick = abs(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]); else MaxStickNick--; |
605 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
- | |
606 | MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
612 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > MaxStickRoll) |
607 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
613 | MaxStickRoll = abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]); else MaxStickRoll--; |
608 | 614 | */ |
|
Line 609... | Line 615... | ||
609 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
615 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
610 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
616 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
Line 642... | Line 648... | ||
642 | } |
648 | } |
Line 643... | Line 649... | ||
643 | 649 | ||
644 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
650 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
645 | if(GyroFaktor < 0) GyroFaktor = 0; |
651 | if(GyroFaktor < 0) GyroFaktor = 0; |
- | 652 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
- | 653 | ||
- | 654 | if(abs(StickNick) > MaxStickNick) MaxStickNick = abs(StickNick); else MaxStickNick--; |
|
- | 655 | if(abs(StickRoll) > MaxStickRoll) MaxStickRoll = abs(StickRoll); else MaxStickRoll--; |
|
- | 656 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
|
646 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
657 | |
647 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
658 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
648 | // Looping? |
659 | // Looping? |
649 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
660 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
650 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
661 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
Line 737... | Line 748... | ||
737 | long tmp_long, tmp_long2; |
748 | long tmp_long, tmp_long2; |
738 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
749 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
739 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
750 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
740 | tmp_long /= 16; |
751 | tmp_long /= 16; |
741 | tmp_long2 /= 16; |
752 | tmp_long2 /= 16; |
742 | if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
753 | if((MaxStickNick > 32) || (MaxStickRoll > 32)) |
743 | { |
754 | { |
744 | tmp_long /= 3; |
755 | tmp_long /= 3; |
745 | tmp_long2 /= 3; |
756 | tmp_long2 /= 3; |
746 | } |
757 | } |
747 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
758 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
Line 782... | Line 793... | ||
782 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
793 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
Line 783... | Line 794... | ||
783 | 794 | ||
784 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
795 | LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL; |
Line 785... | Line 796... | ||
785 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
796 | LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL; |
786 | 797 | ||
787 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
798 | if((MaxStickNick > 32) || (MaxStickRoll > 32) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
788 | { |
799 | { |
789 | LageKorrekturNick /= 2; |
800 | LageKorrekturNick /= 2; |
Line 950... | Line 961... | ||
950 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
961 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
951 | v = abs(IntegralRoll /512); |
962 | v = abs(IntegralRoll /512); |
952 | //v /= 4; |
963 | //v /= 4; |
953 | //w /= 4; |
964 | //w /= 4; |
954 | if(v > w) w = v; // grösste Neigung ermitteln |
965 | if(v > w) w = v; // grösste Neigung ermitteln |
955 | korrektur = w + 3; |
966 | korrektur = w + 8; |
956 | if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
967 | if(w < 25 && NeueKompassRichtungMerken && !KompassSignalSchlecht) |
957 | { |
968 | { |
958 | KompassStartwert = KompassValue; |
969 | KompassStartwert = KompassValue; |
959 | NeueKompassRichtungMerken = 0; |
970 | NeueKompassRichtungMerken = 0; |
960 | } |
971 | } |
Line 965... | Line 976... | ||
965 | if(!KompassSignalSchlecht) |
976 | if(!KompassSignalSchlecht) |
966 | { |
977 | { |
967 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
978 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
968 | GierGyroFehler += fehler; |
979 | GierGyroFehler += fehler; |
969 | // DebugOut.Analog[10] = fehler; |
980 | // DebugOut.Analog[10] = fehler; |
970 | ErsatzKompass += (fehler * 128) / korrektur; |
981 | ErsatzKompass += (fehler * 16) / korrektur; |
971 | v = (KompassRichtung * w) / 32; // nach Kompass ausrichten |
982 | v = (KompassRichtung * w) / 32; // nach Kompass ausrichten |
972 | w = Parameter_KompassWirkung; |
983 | w = Parameter_KompassWirkung; |
973 | if(v > w) v = w; // Begrenzen |
984 | if(v > w) v = w; // Begrenzen |
974 | else |
985 | else |
975 | if(v < -w) v = -w; |
986 | if(v < -w) v = -w; |
Line 1079... | Line 1090... | ||
1079 | } |
1090 | } |
Line 1080... | Line 1091... | ||
1080 | 1091 | ||
1081 | if(Notlandung) SollHoehe = 0; |
1092 | if(Notlandung) SollHoehe = 0; |
1082 | h = HoehenWert; |
1093 | h = HoehenWert; |
- | 1094 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
|
1083 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1095 | { |
1084 | { h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1096 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1085 | h = GasMischanteil - h; // vom Gas abziehen |
1097 | h = GasMischanteil - h; // vom Gas abziehen |
1086 | h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
1098 | h -= (HoeheD * Parameter_Luftdruck_D)/8; // D-Anteil |
1087 | tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
1099 | tmp_int = ((Mess_Integral_Hoch / 512) * (signed long) Parameter_Hoehe_ACC_Wirkung) / 32; |
1088 | if(tmp_int > 50) tmp_int = 50; |
1100 | if(tmp_int > 50) tmp_int = 50; |
Line 1125... | Line 1137... | ||
1125 | 1137 | ||
1126 | // if(GasMischanteil < 20) GierMischanteil = 0; |
1138 | // if(GasMischanteil < 20) GierMischanteil = 0; |
1127 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1139 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1128 | // Nick-Achse |
1140 | // Nick-Achse |
- | 1141 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1142 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
|
1129 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1143 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
1130 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1144 | // DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
- | 1145 | // if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
|
1131 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1146 | |
1132 | else SummeNick += DiffNick; // I-Anteil bei HH |
1147 | else SummeNick += DiffNick; // I-Anteil bei HH |
1133 | if(SummeNick > 16000) SummeNick = 16000; |
1148 | if(SummeNick > 16000) SummeNick = 16000; |
1134 | if(SummeNick < -16000) SummeNick = -16000; |
1149 | if(SummeNick < -16000) SummeNick = -16000; |
1135 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1150 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
Line 1150... | Line 1165... | ||
1150 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1165 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1151 | Motor_Hinten = motorwert; |
1166 | Motor_Hinten = motorwert; |
1152 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1167 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1153 | // Roll-Achse |
1168 | // Roll-Achse |
1154 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 1170 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
|
- | 1171 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
|
1155 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1172 | // DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1156 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1173 | // if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
- | 1174 | ||
1157 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1175 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1158 | if(SummeRoll > 16000) SummeRoll = 16000; |
1176 | if(SummeRoll > 16000) SummeRoll = 16000; |
1159 | if(SummeRoll < -16000) SummeRoll = -16000; |
1177 | if(SummeRoll < -16000) SummeRoll = -16000; |
1160 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1178 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1161 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1179 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1162 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1180 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1163 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1181 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1164 | // Motor Links |
1182 | // Motor Links |
1165 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1183 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1166 | #define GRENZE Poti1 |
- | |
Line 1167... | Line 1184... | ||
1167 | 1184 | ||
1168 | if ((motorwert < 0)) motorwert = 0; |
1185 | if ((motorwert < 0)) motorwert = 0; |
1169 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1186 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1170 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1187 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |