Rev 1855 | Rev 1861 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1855 | Rev 1857 | ||
|---|---|---|---|
| Line 1344... | Line 1344... | ||
| 1344 | static int HeightTrimming = 0; // rate for change of height setpoint |
1344 | static int HeightTrimming = 0; // rate for change of height setpoint |
| 1345 | static int FilterHCGas = 0; |
1345 | static int FilterHCGas = 0; |
| 1346 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
1346 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
| 1347 | static unsigned long HoverGasFilter = 0; |
1347 | static unsigned long HoverGasFilter = 0; |
| 1348 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1348 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| - | 1349 | static signed char WaypointTrimming = 0; |
|
| 1349 | int CosAttitude; // for projection of hoover gas |
1350 | int CosAttitude; // for projection of hoover gas |
| Line 1350... | Line 1351... | ||
| 1350 | 1351 | ||
| 1351 | // get the current hooverpoint |
1352 | // get the current hooverpoint |
| Line 1476... | Line 1477... | ||
| 1476 | VarioCharacter = '-'; |
1477 | VarioCharacter = '-'; |
| 1477 | } |
1478 | } |
| 1478 | else // Gas Stick in Hover Range |
1479 | else // Gas Stick in Hover Range |
| 1479 | { |
1480 | { |
| 1480 | VarioCharacter = '='; |
1481 | VarioCharacter = '='; |
| 1481 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint - SollHoehe > FromNC_AltitudeSpeed) // von NC gesteuert -> Steigen |
1482 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen |
| 1482 | { |
1483 | { |
| 1483 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1484 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
| 1484 | HeightTrimming += FromNC_AltitudeSpeed; |
1485 | HeightTrimming += FromNC_AltitudeSpeed; |
| - | 1486 | WaypointTrimming = 10; |
|
| 1485 | VarioCharacter = 'u'; |
1487 | VarioCharacter = '^'; |
| 1486 | } |
1488 | } |
| 1487 | else |
1489 | else |
| 1488 | if(FromNC_AltitudeSpeed && SollHoehe - FromNC_AltitudeSetpoint > FromNC_AltitudeSpeed) // von NC gesteuert -> sinken |
1490 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
| 1489 | { |
1491 | { |
| 1490 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1492 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| 1491 | HeightTrimming -= FromNC_AltitudeSpeed; |
1493 | HeightTrimming -= FromNC_AltitudeSpeed; |
| - | 1494 | WaypointTrimming = -10; |
|
| 1492 | VarioCharacter = 'd'; |
1495 | VarioCharacter = 'v'; |
| 1493 | } |
1496 | } |
| 1494 | else |
1497 | else |
| 1495 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1498 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| 1496 | { |
1499 | { |
| - | 1500 | if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height |
|
| - | 1501 | else WaypointTrimming = 0; |
|
| 1497 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1502 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| 1498 | HeightTrimming = 0; |
1503 | HeightTrimming = 0; |
| 1499 | SollHoehe = HoehenWert; // update setpoint to current height |
- | |
| 1500 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1504 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
| 1501 | if(!StartTrigger && HoehenWert > 50) |
1505 | if(!StartTrigger && HoehenWert > 50) |
| 1502 | { |
1506 | { |
| 1503 | StartTrigger = 1; |
1507 | StartTrigger = 1; |
| 1504 | } |
1508 | } |
| 1505 | } |
1509 | } |
| 1506 | } |
1510 | } |
| 1507 | // Trim height set point |
1511 | // Trim height set point |
| 1508 | if(abs(HeightTrimming) > 512) |
1512 | if(abs(HeightTrimming) > 512) |
| 1509 | { |
1513 | { |
| - | 1514 | if(WaypointTrimming) |
|
| - | 1515 | { |
|
| - | 1516 | if(abs(FromNC_AltitudeSetpoint - SollHoehe) < 10) SollHoehe = FromNC_AltitudeSetpoint; |
|
| - | 1517 | else SollHoehe += WaypointTrimming; |
|
| - | 1518 | } |
|
| - | 1519 | else |
|
| 1510 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1520 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| 1511 | HeightTrimming = 0; |
1521 | HeightTrimming = 0; |
| 1512 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1522 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
| 1513 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1523 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
| 1514 | //update hoover gas stick value when setpoint is shifted |
1524 | //update hoover gas stick value when setpoint is shifted |
| 1515 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
1525 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
| 1516 | { |
1526 | { |
| 1517 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
1527 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
| 1518 | StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
1528 | StickGasHover = (StickGasHover * UBat) / BattLowVoltageWarning; |
| 1519 | if(StickGasHover < 70) StickGasHover = 70; |
1529 | if(StickGasHover < 70) StickGasHover = 70; |
| 1520 | else if(StickGasHover > 150) StickGasHover = 150; |
1530 | else if(StickGasHover > 150) StickGasHover = 150; |