Rev 2161 | Rev 2168 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2161 | Rev 2165 | ||
---|---|---|---|
Line 203... | Line 203... | ||
203 | signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
203 | signed char cosinus, sinus; // MartinR : extern für PAN-Funktion |
Line 204... | Line 204... | ||
204 | 204 | ||
205 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
205 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
206 | //int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es |
206 | //int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es |
- | 207 | int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
|
Line 207... | Line 208... | ||
207 | int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
208 | unsigned char stick_p; |
208 | 209 | ||
209 | unsigned int modell_fliegt = 0; |
210 | unsigned int modell_fliegt = 0; |
210 | volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
211 | volatile unsigned char FC_StatusFlags = 0, FC_StatusFlags2 = 0; |
Line 242... | Line 243... | ||
242 | 243 | ||
243 | DebugOut.Analog[16] = cosinus; // MartinR: zur Einstellung der Pan-Funktion |
244 | DebugOut.Analog[16] = cosinus; // MartinR: zur Einstellung der Pan-Funktion |
244 | DebugOut.Analog[17] = sinus; // MartinR: test zur Einstellung der Pan-Funktion |
245 | DebugOut.Analog[17] = sinus; // MartinR: test zur Einstellung der Pan-Funktion |
245 | DebugOut.Analog[18] = ServoPanValue; // MartinR: zur Einstellung der Pan-Funktion |
246 | DebugOut.Analog[18] = ServoPanValue; // MartinR: zur Einstellung der Pan-Funktion |
- | 247 | DebugOut.Analog[19] = ServoRollValue; // MartinR: Test |
|
246 | DebugOut.Analog[19] = ServoRollValue; // MartinR: Test |
248 | |
247 | DebugOut.Analog[20] = ServoNickValue; |
249 | DebugOut.Analog[20] = ServoNickValue; |
248 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
250 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
Line 249... | Line 251... | ||
249 | #ifdef WITH_REMAINCAPACITY // only include functions if DEBUG is defined in main.h |
251 | #ifdef WITH_REMAINCAPACITY // only include functions if DEBUG is defined in main.h |
Line 939... | Line 941... | ||
939 | modell_fliegt = 1; |
941 | modell_fliegt = 1; |
940 | MotorenEin = 1; |
942 | MotorenEin = 1; |
941 | sollGier = 0; |
943 | sollGier = 0; |
942 | Mess_Integral_Gier = 0; |
944 | Mess_Integral_Gier = 0; |
943 | // Mess_Integral_Gier2 = 0; |
945 | // Mess_Integral_Gier2 = 0; |
- | 946 | ||
944 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
947 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
945 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
948 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
946 | Mess_IntegralNick2 = IntegralNick; |
949 | Mess_IntegralNick2 = IntegralNick; |
947 | Mess_IntegralRoll2 = IntegralRoll; |
950 | Mess_IntegralRoll2 = IntegralRoll; |
948 | SummeNick = 0; |
951 | SummeNick = 0; |
949 | SummeRoll = 0; |
952 | SummeRoll = 0; |
950 | FC_StatusFlags |= FC_STATUS_START; |
953 | FC_StatusFlags |= FC_STATUS_START; |
951 | // ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
954 | // ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
952 | NeueKompassRichtungMerken = 100; // 2 sekunden |
955 | NeueKompassRichtungMerken = 100; // 2 sekunden |
- | 956 | ||
- | 957 | // MartinR: hinzugefügt Anfang |
|
- | 958 | stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; // aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR |
|
- | 959 | stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; // aktueller Stickwert wird als Neutralposition im HH verwendet, MartinR |
|
- | 960 | SummeNickHH = 0 ; // Zurücksetzen der Integratoren |
|
- | 961 | SummeRollHH = 0 ; // Zurücksetzen der Integratoren |
|
- | 962 | // MartinR: hinzugefügt Ende |
|
953 | } |
963 | } |
954 | else |
964 | else |
955 | { |
965 | { |
956 | beeptime = 1500; // indicate missing calibration |
966 | beeptime = 1500; // indicate missing calibration |
957 | } |
967 | } |
Line 993... | Line 1003... | ||
993 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
1003 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
994 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
1004 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
995 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
1005 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
996 | */ |
1006 | */ |
997 | // MartinR: geändert Anfang |
1007 | // MartinR: geändert Anfang |
998 | if(Parameter_UserParam1 > 50) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
- | |
999 | { |
- | |
1000 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4; |
- | |
1001 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ; |
- | |
1002 | //stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral); |
- | |
1003 | //stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral); |
- | |
1004 | } |
- | |
1005 | - | ||
1006 | else |
- | |
1007 | { |
- | |
1008 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
- | |
1009 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
- | |
1010 | stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
- | |
1011 | stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
- | |
1012 | } |
- | |
Line 1013... | Line 1008... | ||
1013 | 1008 | ||
1014 | if(IntegralFaktor) |
1009 | if(IntegralFaktor) // ACC-Mode |
- | 1010 | { |
|
- | 1011 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * stick_p) / 4; |
|
1015 | { |
1012 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * stick_p) / 4; |
1016 | //stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
1013 | stick_nick_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]]; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
1017 | //stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
1014 | stick_roll_neutral = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
1018 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
1015 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
Line 1019... | Line -... | ||
1019 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
- | |
1020 | - | ||
1021 | //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam |
1016 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
1022 | //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam |
1017 | |
1023 | } |
1018 | } |
- | 1019 | else // HH-Mode |
|
- | 1020 | { |
|
1024 | /*else // wenn HH , MartinR |
1021 | if(Parameter_UserParam1 > 49) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
1025 | { |
1022 | { |
- | 1023 | stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * Parameter_UserParam3); |
|
- | 1024 | stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * Parameter_UserParam3); |
|
- | 1025 | } |
|
- | 1026 | ||
1026 | //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
1027 | else |
1027 | //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
1028 | { |
- | 1029 | stick_nick = ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] - stick_nick_neutral) * stick_p); |
|
1028 | //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam |
1030 | stick_roll = ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] - stick_roll_neutral) * stick_p); |
1029 | //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
- | |
1030 | } |
1031 | } |
Line 1031... | Line 1032... | ||
1031 | */ |
1032 | } |
1032 | // MartinR: geändert Ende |
1033 | // MartinR: geändert Ende |
1033 | 1034 | ||
Line 1066... | Line 1067... | ||
1066 | StickNick -= GPS_Nick; |
1067 | StickNick -= GPS_Nick; |
1067 | StickRoll -= GPS_Roll; |
1068 | StickRoll -= GPS_Roll; |
1068 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127; |
1069 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 127; |
Line 1069... | Line 1070... | ||
1069 | 1070 | ||
1070 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
1071 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
1071 | IntegralFaktor = Parameter_Gyro_I; |
1072 | // IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen |
1072 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
1073 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
Line 1073... | Line 1074... | ||
1073 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
1074 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
1074 | 1075 | ||
Line 1191... | Line 1192... | ||
1191 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
1192 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
1192 | MittelIntegralRoll += IntegralRoll; |
1193 | MittelIntegralRoll += IntegralRoll; |
1193 | MittelIntegralNick2 += IntegralNick2; |
1194 | MittelIntegralNick2 += IntegralNick2; |
1194 | MittelIntegralRoll2 += IntegralRoll2; |
1195 | MittelIntegralRoll2 += IntegralRoll2; |
Line 1195... | Line -... | ||
1195 | - | ||
1196 | // if(Looping_Nick || Looping_Roll) // MartinR: so war es |
- | |
1197 | if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert |
- | |
1198 | { |
- | |
1199 | IntegralAccNick = 0; |
- | |
1200 | IntegralAccRoll = 0; |
- | |
1201 | MittelIntegralNick = 0; |
- | |
1202 | MittelIntegralRoll = 0; |
- | |
1203 | MittelIntegralNick2 = 0; |
- | |
1204 | MittelIntegralRoll2 = 0; |
- | |
1205 | Mess_IntegralNick2 = Mess_IntegralNick; |
- | |
1206 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
- | |
1207 | ZaehlMessungen = 0; |
- | |
1208 | LageKorrekturNick = 0; |
- | |
1209 | LageKorrekturRoll = 0; |
- | |
1210 | } |
- | |
1211 | 1196 | ||
1212 | if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR: |
1197 | if((!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: |
1213 | // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH |
1198 | // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH |
1214 | // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten! |
1199 | // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten! |
1215 | // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet |
1200 | // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet |
1216 | { |
- | |
1217 | 1201 | { |
|
1218 | IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1202 | IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1219 | IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1203 | IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1220 | Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1204 | Mess_IntegralNick = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1221 | Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1205 | Mess_IntegralRoll = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
- | 1206 | Mess_Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
|
1222 | Mess_Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1207 | sollGier = 0; |
1223 | Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1208 | Integral_Gier = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1224 | //Mess_Integral_Gier2 = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1209 | //Mess_Integral_Gier2 = 0; // MartinR: Reset der Integratoren beim Zurückschalten vom HH- in den ACC-Mode |
1225 | KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten |
1210 | KompassSollWert = KompassValue; // MartinR: aktuelle Ausrichtung beibehalten |
- | 1211 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten |
|
1226 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; // MartinR: aktuelle Ausrichtung beibehalten |
1212 | NeueKompassRichtungMerken = 1; // MartinR: aktuelle Ausrichtung beibehalten |
- | 1213 | } |
|
- | 1214 | ||
- | 1215 | // if(Looping_Nick || Looping_Roll) // MartinR: so war es |
|
- | 1216 | if(Looping_Nick || Looping_Roll || (!IntegralFaktor && (Parameter_UserParam1 < 50) && !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert |
|
- | 1217 | // MartinR: beim ACC-Loop oder beim zurückschalten von HH auf ACC |
|
- | 1218 | { |
|
- | 1219 | IntegralAccNick = 0; |
|
- | 1220 | IntegralAccRoll = 0; |
|
- | 1221 | MittelIntegralNick = 0; |
|
- | 1222 | MittelIntegralRoll = 0; |
|
- | 1223 | MittelIntegralNick2 = 0; |
|
- | 1224 | MittelIntegralRoll2 = 0; |
|
- | 1225 | Mess_IntegralNick2 = Mess_IntegralNick; |
|
- | 1226 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
|
- | 1227 | ZaehlMessungen = 0; |
|
- | 1228 | LageKorrekturNick = 0; |
|
- | 1229 | LageKorrekturRoll = 0; |
|
Line 1227... | Line 1230... | ||
1227 | } |
1230 | } |
1228 | 1231 | ||
Line 1229... | Line 1232... | ||
1229 | if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben |
1232 | if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 49)) IntegralFaktor = 0; // MartinR geändert und verschoben |