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; |