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