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; |