Rev 1691 | Rev 1693 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1691 | Rev 1692 | ||
|---|---|---|---|
| Line 96... | Line 96... | ||
| 96 | volatile unsigned char SenderOkay = 0; |
96 | volatile unsigned char SenderOkay = 0; |
| 97 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
97 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
| 98 | char MotorenEin = 0; |
98 | char MotorenEin = 0; |
| 99 | long HoehenWert = 0; |
99 | long HoehenWert = 0; |
| 100 | long SollHoehe = 0; |
100 | long SollHoehe = 0; |
| 101 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
101 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
| 102 | //float Ki = FAKTOR_I; |
102 | //float Ki = FAKTOR_I; |
| 103 | int Ki = 10300 / 33; |
103 | int Ki = 10300 / 33; |
| 104 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
104 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
| 105 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
105 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
| Line 1293... | Line 1293... | ||
| 1293 | #define OPA_OFFSET_STEP 10 |
1293 | #define OPA_OFFSET_STEP 10 |
| 1294 | #endif |
1294 | #endif |
| 1295 | int HCGas, HeightDeviation = 0; |
1295 | int HCGas, HeightDeviation = 0; |
| 1296 | static int HeightTrimming = 0; // rate for change of height setpoint |
1296 | static int HeightTrimming = 0; // rate for change of height setpoint |
| 1297 | static int FilterHCGas = 0; |
1297 | static int FilterHCGas = 0; |
| 1298 | static int StickGasHover = 120, HoverGas = 0, HoverGasMin = 0, HoverGasMax = 1023; |
1298 | static int StickGasHover = 120, HoverGasMin = 0, HoverGasMax = 1023; |
| 1299 | static unsigned long HoverGasFilter = 0; |
1299 | static unsigned long HoverGasFilter = 0; |
| 1300 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1300 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| 1301 | int CosAttitude; // for projection of hoover gas |
1301 | int CosAttitude; // for projection of hoover gas |
| Line 1302... | Line 1302... | ||
| 1302 | 1302 | ||
| Line 1502... | Line 1502... | ||
| 1502 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1502 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
| 1503 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1503 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
| 1504 | HCGas -= tmp_int; |
1504 | HCGas -= tmp_int; |
| Line 1505... | Line 1505... | ||
| 1505 | 1505 | ||
| 1506 | // limit deviation from hoover point within the target region |
1506 | // limit deviation from hoover point within the target region |
| 1507 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HoverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1507 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
| - | 1508 | { |
|
| - | 1509 | unsigned int tmp,min,max; |
|
| - | 1510 | if(abs(HeightDeviation) < 60) |
|
| 1508 | { |
1511 | { |
| - | 1512 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
|
| - | 1513 | } |
|
| - | 1514 | else |
|
| - | 1515 | { |
|
| - | 1516 | tmp = (abs(HeightDeviation) - 60)/32; |
|
| - | 1517 | if(tmp > 15) tmp = 15; |
|
| - | 1518 | max = (HoverGasMax * (tmp + 16)) / 16; |
|
| - | 1519 | min = (HoverGasMin * (16 - tmp)) / 16; |
|
| - | 1520 | LIMIT_MIN_MAX(HCGas, min, max); // limit gas around the hoover point |
|
| - | 1521 | DebugOut.Analog[16] = min; |
|
| - | 1522 | DebugOut.Analog[17] = max; |
|
| - | 1523 | DebugOut.Analog[18] = SollHoehe; |
|
| 1509 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1524 | } |
| 1510 | } |
- | |
| 1511 | 1525 | } |
|
| 1512 | // strech control output by inverse attitude projection 1/cos |
1526 | // strech control output by inverse attitude projection 1/cos |
| 1513 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1527 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
| 1514 | tmp_long2 = (int32_t)HCGas; |
1528 | tmp_long2 = (int32_t)HCGas; |
| 1515 | tmp_long2 *= 8192L; |
1529 | tmp_long2 *= 8192L; |
| Line 1632... | Line 1646... | ||
| 1632 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1646 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
| 1633 | else SummeNick += DiffNick; // I-Anteil bei HH |
1647 | else SummeNick += DiffNick; // I-Anteil bei HH |
| 1634 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1648 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| 1635 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1649 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| Line 1636... | Line 1650... | ||
| 1636 | 1650 | ||
| 1637 | if(Parameter_UserParam5 > 50) |
1651 | //if(Parameter_UserParam5 > 50) |
| 1638 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
1652 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
| 1639 | else |
1653 | //else |
| Line 1640... | Line 1654... | ||
| 1640 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
1654 | // pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
| 1641 | 1655 | ||
| 1642 | // Motor Vorn |
1656 | // Motor Vorn |
| 1643 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1657 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |