Rev 1937 | Rev 1942 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1937 | Rev 1941 | ||
---|---|---|---|
Line 77... | Line 77... | ||
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 = -1; |
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; |
86 | unsigned char MAX_GAS,MIN_GAS; |
86 | unsigned char MAX_GAS,MIN_GAS; |
87 | unsigned char HoehenReglerAktiv = 0; |
87 | unsigned char HoehenReglerAktiv = 0; |
Line 189... | Line 189... | ||
189 | DebugOut.Analog[5] = HoehenWert/5; |
189 | DebugOut.Analog[5] = HoehenWert/5; |
190 | DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az; |
190 | DebugOut.Analog[6] = AdWertAccHoch;//(Mess_Integral_Hoch / 512);// Aktuell_az; |
191 | DebugOut.Analog[8] = KompassValue; |
191 | DebugOut.Analog[8] = KompassValue; |
192 | DebugOut.Analog[9] = UBat; |
192 | DebugOut.Analog[9] = UBat; |
193 | DebugOut.Analog[10] = SenderOkay; |
193 | DebugOut.Analog[10] = SenderOkay; |
194 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
194 | DebugOut.Analog[11] = ErsatzKompassInGrad; |
195 | DebugOut.Analog[12] = Motor[0].SetPoint; |
195 | DebugOut.Analog[12] = Motor[0].SetPoint; |
196 | DebugOut.Analog[13] = Motor[1].SetPoint; |
196 | DebugOut.Analog[13] = Motor[1].SetPoint; |
197 | DebugOut.Analog[14] = Motor[2].SetPoint; |
197 | DebugOut.Analog[14] = Motor[2].SetPoint; |
198 | DebugOut.Analog[15] = Motor[3].SetPoint; |
198 | DebugOut.Analog[15] = Motor[3].SetPoint; |
199 | DebugOut.Analog[20] = ServoNickValue; |
199 | DebugOut.Analog[20] = ServoNickValue; |
Line 1267... | Line 1267... | ||
1267 | // max. Korrekturwert schätzen |
1267 | // max. Korrekturwert schätzen |
1268 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1268 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1269 | v = abs(IntegralRoll /512); |
1269 | v = abs(IntegralRoll /512); |
1270 | if(v > w) w = v; // grösste Neigung ermitteln |
1270 | if(v > w) w = v; // grösste Neigung ermitteln |
1271 | korrektur = w / 4 + 1; |
1271 | korrektur = w / 4 + 1; |
- | 1272 | ErsatzKompassInGrad = ErsatzKompass/GIER_GRAD_FAKTOR; |
|
1272 | // Kompassfehlerwert bestimmen |
1273 | // Kompassfehlerwert bestimmen |
1273 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1274 | fehler = ((540 + KompassValue - ErsatzKompassInGrad) % 360) - 180; |
1274 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1275 | // GIER_GRAD_FAKTOR ist ca. 1200 |
Line 1275... | Line 1276... | ||
1275 | 1276 | ||
1276 | // Kompasswert einloggen |
1277 | // Kompasswert einloggen |
1277 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1278 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
Line 1282... | Line 1283... | ||
1282 | if(NeueKompassRichtungMerken) |
1283 | if(NeueKompassRichtungMerken) |
1283 | { |
1284 | { |
1284 | if(--NeueKompassRichtungMerken == 0) |
1285 | if(--NeueKompassRichtungMerken == 0) |
1285 | { |
1286 | { |
1286 | //--> ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1287 | //--> ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1287 | KompassSollWert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1288 | KompassSollWert = ErsatzKompassInGrad; |
1288 | } |
1289 | } |
1289 | } |
1290 | } |
1290 | } |
1291 | } |
1291 | // Kompass fusionieren |
1292 | // Kompass fusionieren |
1292 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
1293 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
Line 1293... | Line 1294... | ||
1293 | 1294 | ||
1294 | // MK Gieren |
1295 | // MK Gieren |
1295 | if(!NeueKompassRichtungMerken) |
1296 | if(!NeueKompassRichtungMerken) |
1296 | { |
1297 | { |
1297 | r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1298 | r = ((540 + (KompassSollWert - ErsatzKompassInGrad)) % 360) - 180; |
1298 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1299 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1299 | CompassGierSetpoint = v / 16; |
1300 | CompassGierSetpoint = v / 16; |
1300 | } |
1301 | } |
1301 | else CompassGierSetpoint = 0; |
1302 | else CompassGierSetpoint = 0; |