Rev 1278 | Rev 1283 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1278 | Rev 1279 | ||
---|---|---|---|
Line 1269... | Line 1269... | ||
1269 | 1269 | ||
1270 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1270 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung |
1271 | { |
1271 | { |
1272 | int tmp_int; |
1272 | int tmp_int; |
- | 1273 | long tmp_long_h; |
|
1273 | long tmp_long_h; |
1274 | static int hoover = 400, maxhoover,minhoover; |
1274 | static char delay = 100; |
1275 | static char delay = 100; |
1275 | unsigned char angle_gas = 0; |
1276 | unsigned char angle_gas = 0; |
1276 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1277 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1277 | { |
1278 | { |
Line 1317... | Line 1318... | ||
1317 | if(MikroKopterFlags & FLAG_NOTLANDUNG) SollHoehe = 0; |
1318 | if(MikroKopterFlags & FLAG_NOTLANDUNG) SollHoehe = 0; |
1318 | h = HoehenWert; |
1319 | h = HoehenWert; |
1319 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1320 | if((h > SollHoehe) && HoehenReglerAktiv) // zu hoch --> drosseln |
1320 | { |
1321 | { |
1321 | int vario; |
1322 | int vario; |
- | 1323 | static char delay = 1; |
|
- | 1324 | // + P-Part ++++++++++++++++++++++++++ |
|
- | 1325 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
|
- | 1326 | h = GasMischanteil - h; // vom Gas abziehen |
|
- | 1327 | if(!delay--) |
|
- | 1328 | { |
|
- | 1329 | delay = 2; |
|
- | 1330 | hoover = (15*hoover + h) / 16; |
|
- | 1331 | if(hoover < h) hoover++; else hoover--; |
|
- | 1332 | maxhoover = hoover + hoover/6; // max. 16% |
|
- | 1333 | minhoover = hoover - hoover/6; // max. 16% |
|
- | 1334 | } |
|
- | 1335 | if(maxhoover < 200) maxhoover = 200; |
|
- | 1336 | DebugOut.Analog[25] = hoover; |
|
- | 1337 | DebugOut.Analog[26] = maxhoover; |
|
- | 1338 | DebugOut.Analog[28] = minhoover; |
|
- | 1339 | ||
- | 1340 | // + D1-Part - Vario ++++++++++++++++++++++++++ |
|
1322 | vario = VarioMeter / 8; |
1341 | vario = VarioMeter / 8; |
1323 | if(vario > 8) vario = 8; |
1342 | if(vario > 8) vario = 8; |
1324 | if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
1343 | if(vario > 0) vario = VarioMeter + (vario * vario) / 4; |
1325 | else vario = VarioMeter - (vario * vario) / 4; |
1344 | else vario = VarioMeter - (vario * vario) / 4; |
1326 | h = ((h - SollHoehe) * (int) Parameter_Hoehe_P) / 16; // Differenz bestimmen --> P-Anteil |
1345 | vario = (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
- | 1346 | DebugOut.Analog[24] = -vario; |
|
- | 1347 | if(vario > 63) vario = 63; |
|
- | 1348 | else if(vario < -63) vario = -63; |
|
- | 1349 | h -= vario; |
|
- | 1350 | ||
1327 | h = GasMischanteil - h; // vom Gas abziehen |
1351 | // + D2-Part - ACC ++++++++++++++++++++++++++ |
1328 | h -= (Parameter_Luftdruck_D * (long)vario)/128L; // D-Anteil |
1352 | // tmp_int = (((Mess_Integral_Hoch / 128)/*+AdWertAccHoch*/) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (32); |
1329 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1353 | tmp_int = ((Mess_Integral_Hoch / 128) * (signed long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
- | 1354 | DebugOut.Analog[23] = -tmp_int; |
|
1330 | if(tmp_int > 70*STICK_GAIN) tmp_int = 70*STICK_GAIN; |
1355 | if(tmp_int > 127) tmp_int = 127; |
1331 | else if(tmp_int < -(70*STICK_GAIN)) tmp_int = -(70*STICK_GAIN); |
1356 | else if(tmp_int < -63) tmp_int = -63; |
1332 | h -= tmp_int; |
1357 | h -= tmp_int; |
Line -... | Line 1358... | ||
- | 1358 | ||
- | 1359 | if(h > maxhoover) h = maxhoover; |
|
- | 1360 | else |
|
- | 1361 | if(h < minhoover) h = minhoover; |
|
1333 | 1362 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
|
1334 | 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}; |
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}; |
1335 | tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
1364 | tmp_long_h = labs(IntegralNick) + labs(IntegralRoll); |
1336 | tmp_long_h /= 1500;//1024 * 2; |
1365 | tmp_long_h /= 1500;//1024 * 2; |
1337 | if(tmp_long_h > 29) tmp_long_h = 29; |
1366 | if(tmp_long_h > 29) tmp_long_h = 29; |
- | 1367 | h = ((long) h * GAS_TAB[tmp_long_h]) / 128L; |
|
1338 | h = ((long) h * GAS_TAB[tmp_long_h]) / 128L; |
1368 | |
1339 | //DebugOut.Analog[21] = (int) tmp_long_h; |
- | |
1340 | //DebugOut.Analog[22] = (int) GAS_TAB[tmp_long_h]; |
- | |
1341 | //DebugOut.Analog[23] = vario; |
1369 | // + Begrenzung ++++++++++++++++++++++++++ |
1342 | hoehenregler = (hoehenregler*7 + h) / 8; |
1370 | hoehenregler = (hoehenregler*3 + h) / 4; |
1343 | if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
1371 | if(hoehenregler < EE_Parameter.Hoehe_MinGas * STICK_GAIN) // nicht unter MIN |
1344 | { |
1372 | { |
1345 | if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
1373 | if(GasMischanteil >= EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = EE_Parameter.Hoehe_MinGas * STICK_GAIN; |
1346 | if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |
1374 | if(GasMischanteil < EE_Parameter.Hoehe_MinGas * STICK_GAIN) hoehenregler = GasMischanteil; |