Rev 1279 | Rev 1298 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1279 | Rev 1283 | ||
---|---|---|---|
Line 104... | Line 104... | ||
104 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
104 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
105 | unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
105 | unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
106 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
106 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
107 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
107 | unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250 |
108 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
108 | unsigned char Parameter_KompassWirkung = 64; // Wert : 0-250 |
- | 109 | unsigned char Parameter_Hoehe_GPS_Z = 64; // Wert : 0-250 |
|
109 | unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
110 | unsigned char Parameter_Gyro_D = 8; // Wert : 0-250 |
110 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
111 | unsigned char Parameter_Gyro_P = 150; // Wert : 10-250 |
111 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
112 | unsigned char Parameter_Gyro_I = 150; // Wert : 0-250 |
112 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
113 | unsigned char Parameter_Gier_P = 2; // Wert : 1-20 |
113 | unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
114 | unsigned char Parameter_I_Faktor = 10; // Wert : 1-20 |
Line 494... | Line 495... | ||
494 | #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; } |
495 | #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; } |
495 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
496 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255); |
496 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
497 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
497 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
498 | CHK_POTI_MM(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100); |
498 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
499 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255); |
- | 500 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z,0,255); |
|
499 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
501 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255); |
500 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
502 | CHK_POTI_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255); |
501 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
503 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255); |
502 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
504 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,0,255); |
503 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
505 | CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255); |
Line 1175... | Line 1177... | ||
1175 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
1177 | DebugOut.Analog[0] = IntegralNick / (EE_Parameter.GyroAccFaktor * 4); |
1176 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
1178 | DebugOut.Analog[1] = IntegralRoll / (EE_Parameter.GyroAccFaktor * 4); |
1177 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
1179 | DebugOut.Analog[2] = Mittelwert_AccNick / 4; |
1178 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
1180 | DebugOut.Analog[3] = Mittelwert_AccRoll / 4; |
1179 | DebugOut.Analog[4] = MesswertGier; |
1181 | DebugOut.Analog[4] = MesswertGier; |
1180 | DebugOut.Analog[5] = HoehenWert; |
1182 | DebugOut.Analog[5] = HoehenWert/5; |
1181 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1183 | DebugOut.Analog[6] = Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
1182 | DebugOut.Analog[8] = KompassValue; |
1184 | DebugOut.Analog[8] = KompassValue; |
1183 | DebugOut.Analog[9] = UBat; |
1185 | DebugOut.Analog[9] = UBat; |
1184 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
- | |
1185 | DebugOut.Analog[10] = SenderOkay; |
1186 | DebugOut.Analog[10] = SenderOkay; |
- | 1187 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
|
1186 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1188 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1187 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1189 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1188 | DebugOut.Analog[18] = VarioMeter; |
1190 | DebugOut.Analog[18] = VarioMeter; |
1189 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1191 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1190 | DebugOut.Analog[20] = ServoValue; |
1192 | DebugOut.Analog[20] = ServoValue; |
- | 1193 | DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
|
1191 | // DebugOut.Analog[24] = MesswertNick/2; |
1194 | // DebugOut.Analog[24] = MesswertNick/2; |
1192 | // DebugOut.Analog[25] = MesswertRoll/2; |
1195 | // DebugOut.Analog[25] = MesswertRoll/2; |
1193 | DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1196 | DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1194 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1197 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1195 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1198 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
Line 1269... | Line 1272... | ||
1269 | 1272 | ||
1270 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1273 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1271 | { |
1274 | { |
1272 | int tmp_int; |
1275 | int tmp_int; |
1273 | long tmp_long_h; |
1276 | long tmp_long_h; |
1274 | static int hoover = 400, maxhoover,minhoover; |
1277 | static int hoover = 400, maxhoover,minhoover,gas_old; |
1275 | static char delay = 100; |
1278 | static char delay = 100; |
1276 | unsigned char angle_gas = 0; |
1279 | unsigned char angle_gas = 0; |
1277 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1280 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1278 | { |
1281 | { |
Line 1325... | Line 1328... | ||
1325 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1328 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1326 | h = GasMischanteil - h; // vom Gas abziehen |
1329 | h = GasMischanteil - h; // vom Gas abziehen |
1327 | if(!delay--) |
1330 | if(!delay--) |
1328 | { |
1331 | { |
1329 | delay = 2; |
1332 | delay = 2; |
1330 | hoover = (15*hoover + h) / 16; |
1333 | hoover = (15*hoover + h) / 16 + (GasMischanteil - gas_old); |
- | 1334 | gas_old = GasMischanteil; |
|
1331 | if(hoover < h) hoover++; else hoover--; |
1335 | if(hoover < h) hoover++; else hoover--; |
- | 1336 | if(EE_Parameter.Hoehe_HoverBand) |
|
- | 1337 | { |
|
- | 1338 | unsigned int band; |
|
- | 1339 | band = hoover / EE_Parameter.Hoehe_HoverBand; |
|
1332 | maxhoover = hoover + hoover/6; // max. 16% |
1340 | maxhoover = hoover + band; // max. 16% |
1333 | minhoover = hoover - hoover/6; // max. 16% |
1341 | minhoover = hoover - band; // max. 16% |
- | 1342 | } |
|
- | 1343 | else |
|
- | 1344 | { |
|
- | 1345 | maxhoover = 1023; |
|
- | 1346 | minhoover = 0; |
|
- | 1347 | } |
|
1334 | } |
1348 | } |
1335 | if(maxhoover < 200) maxhoover = 200; |
1349 | if(maxhoover < 200) maxhoover = 200; |
1336 | DebugOut.Analog[25] = hoover; |
1350 | DebugOut.Analog[25] = hoover; |
1337 | DebugOut.Analog[26] = maxhoover; |
1351 | DebugOut.Analog[26] = maxhoover; |
1338 | DebugOut.Analog[28] = minhoover; |
1352 | DebugOut.Analog[28] = minhoover; |
Line 1342... | Line 1356... | ||
1342 | if(vario > 8) vario = 8; |
1356 | if(vario > 8) vario = 8; |
1343 | if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
1357 | if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
1344 | else vario = VarioMeter - (vario * vario) / 4; |
1358 | else vario = VarioMeter - (vario * vario) / 4; |
1345 | vario = (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
1359 | vario = (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
1346 | DebugOut.Analog[24] = -vario; |
1360 | DebugOut.Analog[24] = -vario; |
1347 | if(vario > 63) vario = 63; |
1361 | if(vario > 255) vario = 255; |
1348 | else if(vario < -63) vario = -63; |
1362 | else if(vario < -127) vario = -127; |
1349 | h -= vario; |
1363 | h -= vario; |
Line 1350... | Line 1364... | ||
1350 | 1364 | ||
1351 | // + D2-Part - ACC ++++++++++++++++++++++++++ |
1365 | // + D2-Part - ACC ++++++++++++++++++++++++++ |
1352 | // tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32); |
1366 | // tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32); |
1353 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1367 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1354 | DebugOut.Analog[23] = -tmp_int; |
1368 | DebugOut.Analog[23] = -tmp_int; |
1355 | if(tmp_int > 127) tmp_int = 127; |
1369 | if(tmp_int > 255) tmp_int = 255; |
1356 | else if(tmp_int < -63) tmp_int = -63; |
1370 | else if(tmp_int < -127) tmp_int = -127; |
Line 1357... | Line 1371... | ||
1357 | h -= tmp_int; |
1371 | h -= tmp_int; |
1358 | 1372 | ||
1359 | if(h > maxhoover) h = maxhoover; |
1373 | if(h > maxhoover) h = maxhoover; |
- | 1374 | else |
|
- | 1375 | if(h < minhoover) h = minhoover; |
|
- | 1376 | ||
- | 1377 | // + D3-Part - GPS ++++++++++++++++++++++++++ |
|
- | 1378 | vario = (Parameter_Hoehe_GPS_Z * (long)FromNaviCtrl_Value.GpsZ)/128L; // D-Anteil |
|
- | 1379 | DebugOut.Analog[24] = -vario; |
|
- | 1380 | // if(vario > 127) vario = 127; |
|
- | 1381 | // else |
|
- | 1382 | // if(vario < -63) vario = -63; |
|
- | 1383 | h -= vario; |
|
1360 | else |
1384 | |
1361 | if(h < minhoover) h = minhoover; |
1385 | |
1362 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1386 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1363 | const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
1387 | const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
1364 | tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
1388 | tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
Line 1374... | Line 1398... | ||
1374 | if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
1398 | if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
1375 | } |
1399 | } |
1376 | if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
1400 | if(hoehenregler > GasMischanteil) hoehenregler = GasMischanteil; // nicht mehr als Gas |
1377 | GasMischanteil = hoehenregler; |
1401 | GasMischanteil = hoehenregler; |
1378 | } |
1402 | } |
- | 1403 | else |
|
- | 1404 | { |
|
- | 1405 | hoover = GasMischanteil; |
|
- | 1406 | gas_old = GasMischanteil; |
|
- | 1407 | } |
|
1379 | } |
1408 | } |
1380 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
1409 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
Line 1381... | Line 1410... | ||
1381 | 1410 | ||
1382 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1411 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |