Rev 1703 | Rev 1707 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1703 | Rev 1705 | ||
|---|---|---|---|
| Line 1481... | Line 1481... | ||
| 1481 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
1481 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
| 1482 | GasReduction = tmp_long; |
1482 | GasReduction = tmp_long; |
| 1483 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1483 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
| 1484 | tmp_int = VarioMeter / 8; |
1484 | tmp_int = VarioMeter / 8; |
| 1485 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1485 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
| 1486 | if(!HeightTrimmingFlag) |
- | |
| 1487 | { |
- | |
| 1488 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1486 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
| 1489 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1487 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
| 1490 | } |
- | |
| 1491 | else |
- | |
| 1492 | { |
- | |
| 1493 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 16L; // weniger D zum Höhe ändern |
1488 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
| 1494 | LIMIT_MIN_MAX(tmp_int,-16 * STICK_GAIN, 16 * STICK_GAIN); |
- | |
| 1495 | } |
- | |
| 1496 | GasReduction += tmp_int; |
1489 | GasReduction += tmp_int; |
| 1497 | } // EOF no baro range expanding |
1490 | } // EOF no baro range expanding |
| 1498 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1491 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| 1499 | if(Parameter_Hoehe_ACC_Wirkung) |
1492 | if(Parameter_Hoehe_ACC_Wirkung) |
| 1500 | { |
1493 | { |
| Line 1511... | Line 1504... | ||
| 1511 | // ------------------------ ---------------------------------- |
1504 | // ------------------------ ---------------------------------- |
| 1512 | HCGas -= GasReduction; |
1505 | HCGas -= GasReduction; |
| 1513 | // limit deviation from hoover point within the target region |
1506 | // limit deviation from hoover point within the target region |
| 1514 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1507 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
| 1515 | { |
1508 | { |
| 1516 | signed int tmp,min,max; |
1509 | unsigned int tmp; |
| 1517 | if(abs(HeightDeviation) < 60) |
1510 | tmp = abs(HeightDeviation); |
| - | 1511 | if(tmp <= 60) |
|
| 1518 | { |
1512 | { |
| 1519 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1513 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
| 1520 | } |
1514 | } |
| 1521 | else |
1515 | else |
| 1522 | { |
1516 | { |
| 1523 | tmp = (abs(HeightDeviation) - 60)/32; |
1517 | tmp = (tmp - 60) / 32; |
| 1524 | if(tmp > 15) tmp = 15; |
1518 | if(tmp > 15) tmp = 15; |
| 1525 | if(HeightDeviation > 0) |
1519 | if(HeightDeviation > 0) |
| 1526 | { |
1520 | { |
| 1527 | min = (HoverGasMin * (16 - tmp)) / 16; |
1521 | tmp = (HoverGasMin * (16 - tmp)) / 16; |
| 1528 | LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point |
1522 | LIMIT_MIN_MAX(HCGas, tmp, HoverGasMax); // limit gas around the hoover point |
| 1529 | } |
1523 | } |
| 1530 | else |
1524 | else |
| 1531 | { |
1525 | { |
| 1532 | max = (HoverGasMax * (tmp + 16)) / 16; |
1526 | tmp = (HoverGasMax * (tmp + 16)) / 16; |
| 1533 | LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point |
1527 | LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point |
| 1534 | } |
1528 | } |
| 1535 | } |
1529 | } |
| 1536 | } |
1530 | } |
| - | 1531 | ||
| 1537 | // strech control output by inverse attitude projection 1/cos |
1532 | // strech control output by inverse attitude projection 1/cos |
| 1538 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1533 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 1539 | tmp_long2 = (int32_t)HCGas; |
1534 | tmp_long2 = (int32_t)HCGas; |
| 1540 | tmp_long2 *= 8192L; |
1535 | tmp_long2 *= 8192L; |
| 1541 | tmp_long2 /= CosAttitude; |
1536 | tmp_long2 /= CosAttitude; |