Rev 1840 | Rev 1857 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1840 | Rev 1855 | ||
|---|---|---|---|
| Line 98... | Line 98... | ||
| 98 | volatile unsigned char SenderOkay = 0; |
98 | volatile unsigned char SenderOkay = 0; |
| 99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
| 100 | char MotorenEin = 0,StartTrigger = 0; |
100 | char MotorenEin = 0,StartTrigger = 0; |
| 101 | long HoehenWert = 0; |
101 | long HoehenWert = 0; |
| 102 | long SollHoehe = 0; |
102 | long SollHoehe = 0; |
| - | 103 | long FromNC_AltitudeSetpoint = 0; |
|
| - | 104 | unsigned char FromNC_AltitudeSpeed = 0; |
|
| - | 105 | ||
| 103 | int CompassGierSetpoint = 0; |
106 | int CompassGierSetpoint = 0; |
| 104 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
107 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
| 105 | //float Ki = FAKTOR_I; |
108 | //float Ki = FAKTOR_I; |
| 106 | int Ki = 10300 / 33; |
109 | int Ki = 10300 / 33; |
| 107 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
110 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
| Line 1472... | Line 1475... | ||
| 1472 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1475 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
| 1473 | VarioCharacter = '-'; |
1476 | VarioCharacter = '-'; |
| 1474 | } |
1477 | } |
| 1475 | else // Gas Stick in Hover Range |
1478 | else // Gas Stick in Hover Range |
| 1476 | { |
1479 | { |
| - | 1480 | VarioCharacter = '='; |
|
| - | 1481 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint - SollHoehe > FromNC_AltitudeSpeed) // von NC gesteuert -> Steigen |
|
| - | 1482 | { |
|
| - | 1483 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
|
| - | 1484 | HeightTrimming += FromNC_AltitudeSpeed; |
|
| - | 1485 | VarioCharacter = 'u'; |
|
| - | 1486 | } |
|
| - | 1487 | else |
|
| - | 1488 | if(FromNC_AltitudeSpeed && SollHoehe - FromNC_AltitudeSetpoint > FromNC_AltitudeSpeed) // von NC gesteuert -> sinken |
|
| - | 1489 | { |
|
| - | 1490 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
|
| - | 1491 | HeightTrimming -= FromNC_AltitudeSpeed; |
|
| - | 1492 | VarioCharacter = 'd'; |
|
| - | 1493 | } |
|
| - | 1494 | else |
|
| 1477 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1495 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| 1478 | { |
1496 | { |
| 1479 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1497 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| 1480 | HeightTrimming = 0; |
1498 | HeightTrimming = 0; |
| 1481 | SollHoehe = HoehenWert; // update setpoint to current height |
1499 | SollHoehe = HoehenWert; // update setpoint to current height |
| Line 1483... | Line 1501... | ||
| 1483 | if(!StartTrigger && HoehenWert > 50) |
1501 | if(!StartTrigger && HoehenWert > 50) |
| 1484 | { |
1502 | { |
| 1485 | StartTrigger = 1; |
1503 | StartTrigger = 1; |
| 1486 | } |
1504 | } |
| 1487 | } |
1505 | } |
| 1488 | VarioCharacter = '='; |
- | |
| 1489 | } |
1506 | } |
| 1490 | // Trim height set point |
1507 | // Trim height set point |
| 1491 | if(abs(HeightTrimming) > 512) |
1508 | if(abs(HeightTrimming) > 512) |
| 1492 | { |
1509 | { |
| 1493 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1510 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |