Rev 1839 | Rev 1855 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1839 | Rev 1840 | ||
---|---|---|---|
Line 75... | Line 75... | ||
75 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
75 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
76 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
76 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
77 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
77 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
78 | long SummeNick=0,SummeRoll=0; |
78 | long SummeNick=0,SummeRoll=0; |
79 | volatile long Mess_Integral_Hoch = 0; |
79 | volatile long Mess_Integral_Hoch = 0; |
80 | int KompassValue = 0; |
80 | int KompassValue = -1; |
81 | int KompassSollWert = 0; |
81 | int KompassSollWert = 0; |
82 | int KompassRichtung = 0; |
82 | int KompassRichtung = 0; |
83 | char CalculateCompassTimer = 100; |
83 | char CalculateCompassTimer = 100; |
84 | unsigned char KompassFusion = 32; |
84 | unsigned char KompassFusion = 32; |
85 | unsigned int KompassSignalSchlecht = 50; |
85 | unsigned int KompassSignalSchlecht = 50; |
Line 98... | Line 98... | ||
98 | volatile unsigned char SenderOkay = 0; |
98 | volatile unsigned char SenderOkay = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
100 | char MotorenEin = 0,StartTrigger = 0; |
100 | char MotorenEin = 0,StartTrigger = 0; |
101 | long HoehenWert = 0; |
101 | long HoehenWert = 0; |
102 | long SollHoehe = 0; |
102 | long SollHoehe = 0; |
- | 103 | int CompassGierSetpoint = 0; |
|
103 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
104 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
104 | //float Ki = FAKTOR_I; |
105 | //float Ki = FAKTOR_I; |
105 | int Ki = 10300 / 33; |
106 | int Ki = 10300 / 33; |
106 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
107 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
107 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
108 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
Line 370... | Line 371... | ||
370 | //############################################################################ |
371 | //############################################################################ |
371 | { |
372 | { |
372 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
373 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
373 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
374 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
374 | signed long winkel_nick, winkel_roll; |
375 | signed long winkel_nick, winkel_roll; |
375 | unsigned char i; |
- | |
376 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
376 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
377 | MesswertNick = (signed int) AdWertNickFilter / 8; |
377 | MesswertNick = (signed int) AdWertNickFilter / 8; |
378 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
378 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
379 | RohMesswertNick = MesswertNick; |
379 | RohMesswertNick = MesswertNick; |
380 | RohMesswertRoll = MesswertRoll; |
380 | RohMesswertRoll = MesswertRoll; |
Line 704... | Line 704... | ||
704 | { |
704 | { |
705 | SummeNick = 0; |
705 | SummeNick = 0; |
706 | SummeRoll = 0; |
706 | SummeRoll = 0; |
707 | sollGier = 0; |
707 | sollGier = 0; |
708 | Mess_Integral_Gier = 0; |
708 | Mess_Integral_Gier = 0; |
709 | NeueKompassRichtungMerken = 250; |
- | |
710 | } else FC_StatusFlags |= FC_STATUS_FLY; |
709 | } else FC_StatusFlags |= FC_STATUS_FLY; |
Line 711... | Line 710... | ||
711 | 710 | ||
712 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
711 | if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0) |
713 | { |
712 | { |
Line 801... | Line 800... | ||
801 | Mess_IntegralRoll2 = IntegralRoll; |
800 | Mess_IntegralRoll2 = IntegralRoll; |
802 | SummeNick = 0; |
801 | SummeNick = 0; |
803 | SummeRoll = 0; |
802 | SummeRoll = 0; |
804 | FC_StatusFlags |= FC_STATUS_START; |
803 | FC_StatusFlags |= FC_STATUS_START; |
805 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
804 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
- | 805 | NeueKompassRichtungMerken = 100; // 2 sekunden |
|
806 | } |
806 | } |
807 | else |
807 | else |
808 | { |
808 | { |
809 | beeptime = 1500; // indicate missing calibration |
809 | beeptime = 1500; // indicate missing calibration |
810 | } |
810 | } |
Line 861... | Line 861... | ||
861 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
861 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
862 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
862 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
863 | } |
863 | } |
Line 864... | Line 864... | ||
864 | 864 | ||
865 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
865 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
866 | if(StickGier > 2) StickGier -= 2; else |
866 | if(StickGier > 4) StickGier -= 4; else |
Line 867... | Line 867... | ||
867 | if(StickGier < -2) StickGier += 2; else StickGier = 0; |
867 | if(StickGier < -4) StickGier += 4; else StickGier = 0; |
868 | 868 | ||
869 | StickNick -= (GPS_Nick + GPS_Nick2); |
869 | StickNick -= (GPS_Nick + GPS_Nick2); |
Line 1198... | Line 1198... | ||
1198 | } // ZaehlMessungen >= ABGLEICH_ANZAHL |
1198 | } // ZaehlMessungen >= ABGLEICH_ANZAHL |
Line 1199... | Line 1199... | ||
1199 | 1199 | ||
1200 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1200 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1201 | // Gieren |
1201 | // Gieren |
1202 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1202 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1203 | if(abs(StickGier) > 15) // war 35 |
1203 | if(abs(StickGier) > 3) // war 15 |
1204 | { |
1204 | { |
1205 | // KompassSignalSchlecht = 1000; |
1205 | // KompassSignalSchlecht = 1000; |
1206 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
1206 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
1207 | { |
1207 | { |
1208 | NeueKompassRichtungMerken = 250; |
1208 | NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen |
1209 | }; |
1209 | }; |
1210 | } |
1210 | } |
1211 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1211 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
- | 1212 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
|
1212 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1213 | tmp_int += CompassGierSetpoint; |
1213 | sollGier = tmp_int; |
1214 | sollGier = tmp_int; |
1214 | Mess_Integral_Gier -= tmp_int; |
1215 | Mess_Integral_Gier -= tmp_int; |
1215 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1216 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
Line 1216... | Line 1217... | ||
1216 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1217 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1217 | 1218 | ||
1218 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1219 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1219 | // Kompass |
1220 | // Kompass |
1220 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1221 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1221 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1222 | if(KompassValue >= 0 && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1222 | { |
1223 | { |
1223 | if(CalculateCompassTimer-- == 1) |
1224 | if(CalculateCompassTimer-- == 1) |
1224 | { |
1225 | { |
1225 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
- | |
1226 | CalculateCompassTimer = 13; // falls keine Navi-Daten |
1226 | int w,v,r,fehler,korrektur; // wird von der SPI-Routine auf 1 gesetzt |
1227 | 1227 | CalculateCompassTimer = 13; // falls keine Navi-Daten |
|
1228 | // max. Korrekturwert schätzen |
1228 | // max. Korrekturwert schätzen |
1229 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1229 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1230 | v = abs(IntegralRoll /512); |
1230 | v = abs(IntegralRoll /512); |
1231 | if(v > w) w = v; // grösste Neigung ermitteln |
1231 | if(v > w) w = v; // grösste Neigung ermitteln |
1232 | korrektur = w / 4 + 1; |
1232 | korrektur = w / 4 + 1; |
1233 | // Kompassfehlerwert bestimmen |
1233 | // Kompassfehlerwert bestimmen |
Line 1234... | Line 1234... | ||
1234 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1234 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1235 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1235 | // GIER_GRAD_FAKTOR ist ca. 1200 |
- | 1236 | ||
- | 1237 | // Kompasswert einloggen |
|
1236 | 1238 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
|
1237 | // Kompasswert einloggen |
1239 | else |
1238 | if(!KompassSignalSchlecht && w < 25) |
1240 | if(w < 25) |
1239 | { |
1241 | { |
1240 | GierGyroFehler += fehler; |
1242 | GierGyroFehler += fehler; |
Line 1248... | Line 1250... | ||
1248 | } |
1250 | } |
1249 | } |
1251 | } |
1250 | // Kompass fusionieren |
1252 | // Kompass fusionieren |
1251 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
1253 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
Line 1252... | Line -... | ||
1252 | - | ||
1253 | DebugOut.Analog[16] = fehler; |
- | |
1254 | DebugOut.Analog[17] = korrektur; |
- | |
1255 | DebugOut.Analog[18] = KompassFusion; |
- | |
1256 | DebugOut.Analog[19] = KompassSignalSchlecht; |
- | |
1257 | DebugOut.Analog[24] = FromNaviCtrl_Value.Kalman_K; |
- | |
1258 | 1254 | ||
1259 | // MK drehen |
- | |
1260 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
- | |
1261 | else |
1255 | // MK Gieren |
1262 | if(!NeueKompassRichtungMerken) |
1256 | if(!NeueKompassRichtungMerken) |
1263 | { |
1257 | { |
- | 1258 | r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
|
1264 | r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180; |
1259 | DebugOut.Analog[19] = r; |
1265 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1260 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1266 | Mess_Integral_Gier += v; |
1261 | // if(abs(r) > 20) v *= 2; // über 20° -> doppelt so schnell |
1267 | DebugOut.Analog[25] = v; |
- | |
1268 | //DebugOut.Analog[26] = r; |
1262 | CompassGierSetpoint = v / 16; |
- | 1263 | } |
|
- | 1264 | else CompassGierSetpoint = 0; |
|
- | 1265 | ||
- | 1266 | /* |
|
- | 1267 | DebugOut.Analog[16] = fehler; |
|
- | 1268 | DebugOut.Analog[17] = korrektur; |
|
- | 1269 | */ |
|
- | 1270 | DebugOut.Analog[25] = CompassGierSetpoint; |
|
Line 1269... | Line 1271... | ||
1269 | } |
1271 | DebugOut.Analog[18] = KompassFusion; |
1270 | 1272 | ||
1271 | /* |
1273 | /* |
1272 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
1274 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
Line 1289... | Line 1291... | ||
1289 | } |
1291 | } |
1290 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1292 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
1291 | */ |
1293 | */ |
1292 | } // CalculateCompassTimer |
1294 | } // CalculateCompassTimer |
1293 | } |
1295 | } |
- | 1296 | else CompassGierSetpoint = 0; |
|
1294 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1297 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1295 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1298 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1296 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1299 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1297 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
1300 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |