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 |