Rev 1765 | Rev 1769 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1765 | Rev 1767 | ||
|---|---|---|---|
| Line 1383... | Line 1383... | ||
| 1383 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1383 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| 1384 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1384 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| 1385 | VarioCharacter = ' '; |
1385 | VarioCharacter = ' '; |
| 1386 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1386 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| 1387 | { |
1387 | { |
| 1388 | #define HEIGHT_TRIM_UP 0x01 |
- | |
| 1389 | #define HEIGHT_TRIM_DOWN 0x02 |
- | |
| 1390 | static unsigned char HeightTrimmingFlag = 0x00; |
- | |
| 1391 | - | ||
| 1392 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
1388 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
| 1393 | // Holger original version |
1389 | // Holger original version |
| 1394 | // start of height control algorithm |
1390 | // start of height control algorithm |
| 1395 | // the height control is only an attenuation of the actual gas stick. |
1391 | // the height control is only an attenuation of the actual gas stick. |
| 1396 | // I.e. it will work only if the gas stick is higher than the hover gas |
1392 | // I.e. it will work only if the gas stick is higher than the hover gas |
| 1397 | // and the hover height will be allways larger than height setpoint. |
1393 | // and the hover height will be allways larger than height setpoint. |
| 1398 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
1394 | if((EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) || !(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER)) // Regler wird über Schalter gesteuert) |
| 1399 | { // old version |
1395 | { // old version |
| 1400 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
1396 | HCGas = GasMischanteil; // take current stick gas as neutral point for the height control |
| 1401 | HeightTrimming = 0; |
1397 | HeightTrimming = 0; |
| - | 1398 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
|
| 1402 | } |
1399 | } |
| 1403 | else |
1400 | else |
| 1404 | { |
1401 | { |
| 1405 | // alternative height control |
1402 | // alternative height control |
| 1406 | // PD-Control with respect to hoover point |
1403 | // PD-Control with respect to hoover point |
| Line 1408... | Line 1405... | ||
| 1408 | // the setpoint will be fine adjusted with the gas stick position |
1405 | // the setpoint will be fine adjusted with the gas stick position |
| 1409 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1406 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
| 1410 | { // gas stick is above hoover point |
1407 | { // gas stick is above hoover point |
| 1411 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1408 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
| 1412 | { |
1409 | { |
| 1413 | if(HeightTrimmingFlag & HEIGHT_TRIM_DOWN) |
1410 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
| 1414 | { |
1411 | { |
| 1415 | HeightTrimmingFlag &= ~HEIGHT_TRIM_DOWN; |
1412 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
| 1416 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1413 | SollHoehe = HoehenWert; // update setpoint to current heigth |
| 1417 | } |
1414 | } |
| 1418 | HeightTrimmingFlag |= HEIGHT_TRIM_UP; |
1415 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
| 1419 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1416 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
| 1420 | VarioCharacter = '+'; |
1417 | VarioCharacter = '+'; |
| 1421 | } // gas stick is below hoover point |
1418 | } // gas stick is below hoover point |
| 1422 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1419 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
| 1423 | { |
1420 | { |
| 1424 | if(HeightTrimmingFlag & HEIGHT_TRIM_UP) |
1421 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
| 1425 | { |
1422 | { |
| 1426 | HeightTrimmingFlag &= ~HEIGHT_TRIM_UP; |
1423 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
| 1427 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1424 | SollHoehe = HoehenWert; // update setpoint to current heigth |
| 1428 | } |
1425 | } |
| 1429 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
1426 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| 1430 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1427 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
| 1431 | VarioCharacter = '-'; |
1428 | VarioCharacter = '-'; |
| 1432 | } |
1429 | } |
| 1433 | else // Gas Stick in Hover Range |
1430 | else // Gas Stick in Hover Range |
| 1434 | { |
1431 | { |
| 1435 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
1432 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| 1436 | { |
1433 | { |
| 1437 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN); |
1434 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| 1438 | HeightTrimming = 0; |
1435 | HeightTrimming = 0; |
| 1439 | SollHoehe = HoehenWert; // update setpoint to current height |
1436 | SollHoehe = HoehenWert; // update setpoint to current height |
| 1440 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1437 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
| 1441 | if(!StartTrigger && HoehenWert > 50) |
1438 | if(!StartTrigger && HoehenWert > 50) |
| 1442 | { |
1439 | { |
| Line 1492... | Line 1489... | ||
| 1492 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1489 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| 1493 | tmp_int = VarioMeter / 8; |
1490 | tmp_int = VarioMeter / 8; |
| 1494 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1491 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
| 1495 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1492 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
| 1496 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1493 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
| 1497 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
1494 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
| 1498 | else |
1495 | else |
| 1499 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1496 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
| 1500 | GasReduction += tmp_int; |
1497 | GasReduction += tmp_int; |
| 1501 | } // EOF no baro range expanding |
1498 | } // EOF no baro range expanding |
| 1502 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1499 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |