Rev 2389 | Rev 2402 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2389 | Rev 2390 | ||
|---|---|---|---|
| Line 93... | Line 93... | ||
| 93 | int ErsatzKompassInGrad; // Kompasswert in Grad |
93 | int ErsatzKompassInGrad; // Kompasswert in Grad |
| 94 | int GierGyroFehler = 0; |
94 | int GierGyroFehler = 0; |
| 95 | char GyroFaktor,GyroFaktorGier; |
95 | char GyroFaktor,GyroFaktorGier; |
| 96 | char IntegralFaktor,IntegralFaktorGier; |
96 | char IntegralFaktor,IntegralFaktorGier; |
| 97 | int DiffNick,DiffRoll; |
97 | int DiffNick,DiffRoll; |
| 98 | int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
98 | int StickGasHover = 127, HoverGasMin = 0, HoverGasMax = 1023; |
| 99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
99 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
| 100 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
100 | //int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0, Poti5 = 0, Poti6 = 0, Poti7 = 0, Poti8 = 0; |
| 101 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
101 | unsigned char Poti[9] = {0,0,0,0,0,0,0,0}; |
| 102 | volatile unsigned char SenderOkay = 0; |
102 | volatile unsigned char SenderOkay = 0; |
| 103 | char MotorenEin = 0,StartTrigger = 0; |
103 | char MotorenEin = 0,StartTrigger = 0; |
| Line 180... | Line 180... | ||
| 180 | unsigned int HooverGasEmergencyPercent = 0; // The gas value for Emergency landing |
180 | unsigned int HooverGasEmergencyPercent = 0; // The gas value for Emergency landing |
| 181 | unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while |
181 | unsigned int GasIsZeroCnt = 0; // to detect that the gas-stick is down for a while |
| 182 | signed int Variance = 0; |
182 | signed int Variance = 0; |
| 183 | signed int CosAttitude; // for projection of hoover gas |
183 | signed int CosAttitude; // for projection of hoover gas |
| 184 | unsigned char ACC_AltitudeControl = 0; |
184 | unsigned char ACC_AltitudeControl = 0; |
| - | 185 | unsigned char LowVoltageLandingActive = 0; |
|
| Line 185... | Line 186... | ||
| 185 | 186 | ||
| 186 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
187 | #if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__)) |
| 187 | #define OPA_OFFSET_STEP 5 |
188 | #define OPA_OFFSET_STEP 5 |
| 188 | #else |
189 | #else |
| Line 1586... | Line 1587... | ||
| 1586 | if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR; |
1587 | if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR; |
| 1587 | if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; |
1588 | if(MesswertGier > MAX_SENSOR) MesswertGier = MAX_SENSOR; |
| 1588 | if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; |
1589 | if(MesswertGier < -MAX_SENSOR) MesswertGier = -MAX_SENSOR; |
| Line 1589... | Line 1590... | ||
| 1589 | 1590 | ||
| - | 1591 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1592 | // Undervoltage |
|
| - | 1593 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1594 | if(!(FC_StatusFlags & FC_STATUS_LOWBAT)) |
|
| - | 1595 | { |
|
| - | 1596 | GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
|
| - | 1597 | } |
|
| - | 1598 | ||
| - | 1599 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1600 | // Auto-Landing |
|
| - | 1601 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 1602 | static unsigned char slower; |
|
| - | 1603 | if(!slower--) |
|
| - | 1604 | { |
|
| - | 1605 | slower = 50; // 10Hz |
|
| - | 1606 | if(UBat < EE_Parameter.AutoLandingVoltage) |
|
| - | 1607 | { |
|
| - | 1608 | LowVoltageLandingActive = 20; // 2 sek |
|
| - | 1609 | } |
|
| - | 1610 | else if(UBat > EE_Parameter.AutoLandingVoltage + 2 && LowVoltageLandingActive) LowVoltageLandingActive--; |
|
| - | 1611 | } |
|
| - | 1612 | if(LowVoltageLandingActive && FromNC_AltitudeSetpoint >= 0) |
|
| - | 1613 | { |
|
| - | 1614 | FromNC_AltitudeSpeed = EE_Parameter.LandingSpeed; |
|
| - | 1615 | FromNC_AltitudeSetpoint = -20000; |
|
| - | 1616 | } |
|
| - | 1617 | ||
| - | 1618 | //DebugOut.Analog[16] = StickGasMiddle; |
|
| 1590 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1619 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1591 | // Höhenregelung |
1620 | // Höhenregelung |
| 1592 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 1593 | if(UBat > BattLowVoltageWarning) GasMischanteil = ((unsigned int)GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1621 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 1622 | GasMischanteil *= STICK_GAIN; |
|
| 1594 | GasMischanteil *= STICK_GAIN; |
1623 | |
| 1595 | // if height control is activated |
1624 | // if height control is activated |
| 1596 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung |
1625 | if((Parameter_GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick) && !(VersionInfo.HardwareError[0] & 0x7F)) // Höhenregelung |
| 1597 | { |
1626 | { |
| 1598 | #define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
1627 | #define HOVER_GAS_AVERAGE 16384L // 16384 * 2ms = 32s averaging |
| Line 1697... | Line 1726... | ||
| 1697 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1726 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| 1698 | VarioCharacter = ' '; |
1727 | VarioCharacter = ' '; |
| 1699 | AltitudeSetpointTrimming = 0; |
1728 | AltitudeSetpointTrimming = 0; |
| 1700 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
1729 | if(HoehenReglerAktiv && !(FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
| 1701 | { |
1730 | { |
| 1702 | #define HEIGHT_CONTROL_STICKTHRESHOLD 15 |
- | |
| 1703 | // Holger original version |
1731 | // Holger original version |
| 1704 | // start of height control algorithm |
1732 | // start of height control algorithm |
| 1705 | // the height control is only an attenuation of the actual gas stick. |
1733 | // the height control is only an attenuation of the actual gas stick. |
| 1706 | // I.e. it will work only if the gas stick is higher than the hover gas |
1734 | // I.e. it will work only if the gas stick is higher than the hover gas |
| 1707 | // and the hover height will be allways larger than height setpoint. |
1735 | // and the hover height will be allways larger than height setpoint. |
| Line 1718... | Line 1746... | ||
| 1718 | { |
1746 | { |
| 1719 | // alternative height control |
1747 | // alternative height control |
| 1720 | // PD-Control with respect to hoover point |
1748 | // PD-Control with respect to hoover point |
| 1721 | // the thrust loss out of horizontal attitude is compensated |
1749 | // the thrust loss out of horizontal attitude is compensated |
| 1722 | // the setpoint will be fine adjusted with the gas stick position |
1750 | // the setpoint will be fine adjusted with the gas stick position |
| 1723 | if(FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
1751 | if(/*1 || */FC_StatusFlags & FC_STATUS_FLY) // trim setpoint only when flying |
| 1724 | { // gas stick is above hoover point |
1752 | { // gas stick is above hoover point |
| 1725 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
1753 | if(StickGas > (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtUpperLimit) |
| 1726 | { |
1754 | { |
| 1727 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
1755 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) |
| 1728 | { |
1756 | { |
| Line 1738... | Line 1766... | ||
| 1738 | #endif |
1766 | #endif |
| 1739 | VarioCharacter = '='; |
1767 | VarioCharacter = '='; |
| 1740 | } |
1768 | } |
| 1741 | else |
1769 | else |
| 1742 | { |
1770 | { |
| 1743 | // SollHoehe = (long) Parameter_MaximumAltitude * 100L; |
- | |
| 1744 | // HeightTrimming += abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
- | |
| 1745 | if(HeightDeviation > 20) SollHoehe = HoehenWertF; // update setpoint to current heigth |
1771 | if(HeightDeviation > 20) SollHoehe = HoehenWertF; // update setpoint to current heigth |
| 1746 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1772 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
| 1747 | AltitudeSetpointTrimming = abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1773 | AltitudeSetpointTrimming = abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
| - | 1774 | if(LowVoltageLandingActive) AltitudeSetpointTrimming /= 3; // only 33% rising |
|
| 1748 | VarioCharacter = '+'; |
1775 | VarioCharacter = '+'; |
| 1749 | } |
1776 | } |
| 1750 | WaypointTrimming = 0; |
1777 | WaypointTrimming = 0; |
| 1751 | } // gas stick is below hoover point |
1778 | } // gas stick is below hoover point |
| 1752 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1779 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) // Minus |
| 1753 | { |
1780 | { |
| 1754 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1781 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
| 1755 | { |
1782 | { |
| 1756 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
1783 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
| 1757 | SollHoehe = HoehenWertF; // update setpoint to current heigth |
1784 | SollHoehe = HoehenWertF; // update setpoint to current heigth |
| 1758 | } |
1785 | } |
| 1759 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1786 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
| 1760 | AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1787 | AltitudeSetpointTrimming = -abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
| 1761 | // HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
- | |
| 1762 | VarioCharacter = '-'; |
1788 | VarioCharacter = '-'; |
| 1763 | WaypointTrimming = 0; |
1789 | WaypointTrimming = 0; |
| 1764 | } |
1790 | } |
| 1765 | else // Gas Stick in Hover Range |
1791 | else // Gas Stick in Hover Range |
| 1766 | { |
1792 | { |