Rev 1720 | Rev 1744 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1720 | Rev 1722 | ||
|---|---|---|---|
| Line 1482... | Line 1482... | ||
| 1482 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1482 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
| 1483 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1483 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
| 1484 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1484 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
| 1485 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
1485 | if(HeightTrimmingFlag) tmp_int /= 4; // reduce d-part while trimming setpoint |
| 1486 | else |
1486 | else |
| 1487 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 16; // reduce d-part in "Deckel" mode |
1487 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
| 1488 | GasReduction += tmp_int; |
1488 | GasReduction += tmp_int; |
| 1489 | } // EOF no baro range expanding |
1489 | } // EOF no baro range expanding |
| 1490 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1490 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
| 1491 | if(Parameter_Hoehe_ACC_Wirkung) |
1491 | if(Parameter_Hoehe_ACC_Wirkung) |
| 1492 | { |
1492 | { |
| Line 1497... | Line 1497... | ||
| 1497 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1497 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
| 1498 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1498 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
| 1499 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1499 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| 1500 | GasReduction += tmp_int; |
1500 | GasReduction += tmp_int; |
| 1501 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1501 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
| 1502 | - | ||
| 1503 | // ------------------------ ---------------------------------- |
1502 | // ------------------------ ---------------------------------- |
| 1504 | HCGas -= GasReduction; |
1503 | HCGas -= GasReduction; |
| 1505 | // limit deviation from hoover point within the target region |
1504 | // limit deviation from hoover point within the target region |
| 1506 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1505 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
| 1507 | { |
1506 | { |
| Line 1525... | Line 1524... | ||
| 1525 | tmp = (HoverGasMax * (tmp + 16)) / 16; |
1524 | tmp = (HoverGasMax * (tmp + 16)) / 16; |
| 1526 | LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point |
1525 | LIMIT_MIN_MAX(HCGas, HoverGasMin, tmp); // limit gas around the hoover point |
| 1527 | } |
1526 | } |
| 1528 | } |
1527 | } |
| 1529 | } |
1528 | } |
| 1530 | - | ||
| 1531 | // strech control output by inverse attitude projection 1/cos |
1529 | // strech control output by inverse attitude projection 1/cos |
| 1532 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1530 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 1533 | tmp_long2 = (int32_t)HCGas; |
1531 | tmp_long2 = (int32_t)HCGas; |
| 1534 | tmp_long2 *= 8192L; |
1532 | tmp_long2 *= 8192L; |
| 1535 | tmp_long2 /= CosAttitude; |
1533 | tmp_long2 /= CosAttitude; |