Rev 1654 | Rev 1660 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1654 | Rev 1658 | ||
---|---|---|---|
Line 65... | Line 65... | ||
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
65 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
66 | int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch; |
67 | unsigned int NeutralAccX=0, NeutralAccY=0; |
67 | unsigned int NeutralAccX=0, NeutralAccY=0; |
68 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
68 | int NaviAccNick, NaviAccRoll,NaviCntAcc = 0; |
69 | int NeutralAccZ = 0; |
69 | int NeutralAccZ = 0; |
70 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
70 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0 , ControlHeading = 0; |
71 | long IntegralNick = 0,IntegralNick2 = 0; |
71 | long IntegralNick = 0,IntegralNick2 = 0; |
72 | long IntegralRoll = 0,IntegralRoll2 = 0; |
72 | long IntegralRoll = 0,IntegralRoll2 = 0; |
73 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
73 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
74 | long Integral_Gier = 0; |
74 | long Integral_Gier = 0; |
75 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
75 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
Line 146... | Line 146... | ||
146 | unsigned char Parameter_NaviOperatingRadius; |
146 | unsigned char Parameter_NaviOperatingRadius; |
147 | unsigned char Parameter_NaviWindCorrection; |
147 | unsigned char Parameter_NaviWindCorrection; |
148 | unsigned char Parameter_NaviSpeedCompensation; |
148 | unsigned char Parameter_NaviSpeedCompensation; |
149 | unsigned char Parameter_ExternalControl; |
149 | unsigned char Parameter_ExternalControl; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
- | 151 | unsigned char HeadFree = 0; |
|
Line 151... | Line 152... | ||
151 | 152 | ||
152 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
153 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
153 | int MaxStickNick = 0,MaxStickRoll = 0; |
154 | int MaxStickNick = 0,MaxStickRoll = 0; |
154 | unsigned int modell_fliegt = 0; |
155 | unsigned int modell_fliegt = 0; |
Line 166... | Line 167... | ||
166 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
167 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
167 | // Debugwerte zuordnen |
168 | // Debugwerte zuordnen |
168 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
169 | void CopyDebugValues(void) |
170 | void CopyDebugValues(void) |
170 | { |
171 | { |
171 | - | ||
172 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
172 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
173 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
173 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
174 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
174 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
175 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
175 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
176 | DebugOut.Analog[4] = MesswertGier; |
176 | DebugOut.Analog[4] = MesswertGier; |
Line 178... | Line 178... | ||
178 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
178 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
179 | DebugOut.Analog[8] = KompassValue; |
179 | DebugOut.Analog[8] = KompassValue; |
180 | DebugOut.Analog[9] = UBat; |
180 | DebugOut.Analog[9] = UBat; |
181 | DebugOut.Analog[10] = SenderOkay; |
181 | DebugOut.Analog[10] = SenderOkay; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
182 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
183 | DebugOut.Analog[16] = NeutralAccZ; |
183 | //DebugOut.Analog[16] = NeutralAccZ; |
184 | //DebugOut.Analog[16] = Motor[0].Temperature; |
184 | //DebugOut.Analog[16] = Motor[0].Temperature; |
185 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
185 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
186 | // DebugOut.Analog[18] = VarioMeter; |
186 | // DebugOut.Analog[18] = VarioMeter; |
187 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
187 | // DebugOut.Analog[19] = WinkelOut.CalcState; |
188 | DebugOut.Analog[20] = ServoNickValue; |
188 | DebugOut.Analog[20] = ServoNickValue; |
Line 327... | Line 327... | ||
327 | Mess_Integral_Gier = 0; |
327 | Mess_Integral_Gier = 0; |
328 | StartLuftdruck = Luftdruck; |
328 | StartLuftdruck = Luftdruck; |
329 | VarioMeter = 0; |
329 | VarioMeter = 0; |
330 | Mess_Integral_Hoch = 0; |
330 | Mess_Integral_Hoch = 0; |
331 | KompassStartwert = KompassValue; |
331 | KompassStartwert = KompassValue; |
- | 332 | ControlHeading = KompassValue / 15; |
|
332 | GPS_Neutral(); |
333 | GPS_Neutral(); |
333 | beeptime = 50; |
334 | beeptime = 50; |
334 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
335 | Umschlag180Nick = ((long) EE_Parameter.WinkelUmschlagNick * 2500L) + 15000L; |
335 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
336 | Umschlag180Roll = ((long) EE_Parameter.WinkelUmschlagRoll * 2500L) + 15000L; |
336 | ExternHoehenValue = 0; |
337 | ExternHoehenValue = 0; |
Line 416... | Line 417... | ||
416 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
417 | tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L; |
417 | tmpl *= Parameter_AchsKopplung1; // 90 |
418 | tmpl *= Parameter_AchsKopplung1; // 90 |
418 | tmpl /= 4096L; |
419 | tmpl /= 4096L; |
419 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
420 | tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L; |
420 | tmpl2 *= Parameter_AchsKopplung1; |
421 | tmpl2 *= Parameter_AchsKopplung1; |
421 | tmpl2 /= 4096L; |
422 | tmpl2 /= 4096L; |
422 | if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
423 | if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1; |
423 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
424 | //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256; |
424 | } |
425 | } |
425 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
426 | else tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0; |
426 | - | ||
427 | TrimRoll = tmpl - tmpl2 / 100L; |
427 | TrimRoll = tmpl - tmpl2 / 100L; |
428 | TrimNick = -tmpl2 + tmpl / 100L; |
428 | TrimNick = -tmpl2 + tmpl / 100L; |
429 | - | ||
430 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
429 | // Kompasswert begrenzen ++++++++++++++++++++++++++++++++++++++++++++++++ |
431 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
430 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
432 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
431 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
433 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
432 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
434 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
433 | Mess_IntegralRoll2 += MesswertRoll + TrimRoll; |
Line 597... | Line 596... | ||
597 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
596 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
598 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
597 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
599 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
598 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
600 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
599 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
601 | MAX_GAS = EE_Parameter.Gas_Max; |
600 | MAX_GAS = EE_Parameter.Gas_Max; |
602 | MIN_GAS = EE_Parameter.Gas_Min; |
601 | MIN_GAS = EE_Parameter.Gas_Min; |
- | 602 | if(Poti4 > 50) HeadFree = 1; else HeadFree = 0; |
|
- | 603 | if(HeadFree) {if(Parameter_AchsKopplung1 < 210) Parameter_AchsKopplung1 += 30;} |
|
603 | } |
604 | } |
Line 604... | Line 605... | ||
604 | 605 | ||
605 | //############################################################################ |
606 | //############################################################################ |
606 | // |
607 | // |
Line 803... | Line 804... | ||
803 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
804 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 804... | Line 805... | ||
804 | 805 | ||
805 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
806 | if(!NewPpmData-- || (FCFlags & FCFLAG_NOTLANDUNG)) |
806 | { |
807 | { |
- | 808 | static int stick_nick,stick_roll; |
|
- | 809 | ||
- | 810 | unsigned char angle = 180/15; |
|
- | 811 | signed char sintab[31] = { 0, 4, 8, 11, 14, 16, 16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16, 16}; |
|
- | 812 | //signed char costab[24] = {16, 16, 14, 11, 8, 4, 0, -4, -8, -11, -14, -16, -16, -16, -14, -11, -8, -4, 0, 4, 8, 11, 14, 16}; |
|
- | 813 | angle = (ControlHeading + (360/15) - ErsatzKompass / (GIER_GRAD_FAKTOR * 15)) % 24; |
|
- | 814 | ||
- | 815 | signed int cos_h, sin_h; |
|
- | 816 | cos_h = sintab[angle + 6]/2; |
|
- | 817 | sin_h = sintab[angle]/2; |
|
807 | static int stick_nick,stick_roll; |
818 | |
808 | ParameterZuordnung(); |
819 | ParameterZuordnung(); |
809 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
820 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
810 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
- | |
811 | StickNick = stick_nick - (GPS_Nick + GPS_Nick2); |
- | |
812 | 821 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
|
813 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
822 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
814 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
- | |
Line -... | Line 823... | ||
- | 823 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
- | 824 | ||
- | 825 | if(HeadFree) |
|
- | 826 | { |
|
- | 827 | StickNick = ((cos_h * stick_nick) + (sin_h * stick_roll))/8; |
|
- | 828 | StickRoll = ((cos_h * stick_roll) - (sin_h * stick_nick))/8; |
|
- | 829 | } |
|
- | 830 | else |
|
- | 831 | { |
|
- | 832 | StickNick = stick_nick; |
|
- | 833 | StickRoll = stick_roll; |
|
815 | StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); |
834 | } |
816 | 835 | ||
817 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
836 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
Line -... | Line 837... | ||
- | 837 | if(StickGier > 2) StickGier -= 2; else |
|
- | 838 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
|
818 | if(StickGier > 2) StickGier -= 2; else |
839 | |
Line 819... | Line 840... | ||
819 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
840 | StickNick -= (GPS_Nick + GPS_Nick2); |
820 | 841 | StickRoll -= (GPS_Roll + GPS_Roll2); |
|
821 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
842 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
Line 1172... | Line 1193... | ||
1172 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1193 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 1173... | Line 1194... | ||
1173 | 1194 | ||
1174 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1195 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1175 | // Kompass |
1196 | // Kompass |
1176 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1197 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1177... | Line 1198... | ||
1177 | //DebugOut.Analog[16] = KompassSignalSchlecht; |
1198 | DebugOut.Analog[18] = KompassSignalSchlecht; |
1178 | 1199 | ||
1179 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1200 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1180 | { |
1201 | { |
1181 | int w,v,r,fehler,korrektur; |
1202 | int w,v,r,fehler,korrektur; |
1182 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1203 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1183 | v = abs(IntegralRoll /512); |
1204 | v = abs(IntegralRoll /512); |
1184 | if(v > w) w = v; // grösste Neigung ermitteln |
1205 | if(v > w) w = v; // grösste Neigung ermitteln |
1185 | korrektur = w / 8 + 1; |
1206 | korrektur = w / 8 + 2; |
1186 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1207 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1187 | if(abs(MesswertGier) > 128) |
1208 | if(abs(MesswertGier) > 128) |
1188 | { |
1209 | { |
1189 | fehler = 0; |
1210 | // korrektur = korrektur = w / 16 + 2; // schnell konvergieren |
1190 | } |
1211 | } |
1191 | if(!KompassSignalSchlecht && w < 25) |
1212 | if(!KompassSignalSchlecht && w < 25) |
1192 | { |
1213 | { |
1193 | GierGyroFehler += fehler; |
1214 | GierGyroFehler += fehler; |
1194 | if(NeueKompassRichtungMerken) |
- | |
1195 | { |
- | |
1196 | // beeptime = 200; |
1215 | if(NeueKompassRichtungMerken) |
1197 | // KompassStartwert = KompassValue; |
1216 | { |
1198 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1217 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1199 | KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1218 | KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1200 | NeueKompassRichtungMerken = 0; |
1219 | NeueKompassRichtungMerken = 0; |
- | 1220 | } |
|
- | 1221 | } |
|
1201 | } |
1222 | DebugOut.Analog[16] = fehler; |
1202 | } |
1223 | DebugOut.Analog[17] = korrektur; |
1203 | ErsatzKompass += (fehler * 8) / korrektur; |
1224 | ErsatzKompass += (fehler * 16) / korrektur; |
1204 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
1225 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
1205 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1226 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1206 | if(w >= 0) |
1227 | if(w >= 0) |