Rev 1692 | Rev 1695 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1692 | Rev 1693 | ||
---|---|---|---|
Line 1479... | Line 1479... | ||
1479 | // ------------------------- P-Part ---------------------------- |
1479 | // ------------------------- P-Part ---------------------------- |
1480 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1480 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1481 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1481 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1482 | HeightDeviation = (int)(tmp_long); // positive when too high |
1482 | HeightDeviation = (int)(tmp_long); // positive when too high |
1483 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
1483 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 16L; // p-part |
1484 | LIMIT_MIN_MAX(tmp_long, -255 * STICK_GAIN, 255 * STICK_GAIN); // more than 2 times the full range makes no sense |
1484 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 255 * STICK_GAIN); // more than the full range makes no sense |
1485 | HCGas -= tmp_long; |
1485 | HCGas -= tmp_long; |
1486 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1486 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1487 | tmp_int = VarioMeter / 8; |
1487 | tmp_int = VarioMeter / 8; |
1488 | LIMIT_MIN_MAX(tmp_int, -181, 181); // avoid overflow when squared (181^2 = 32761) |
1488 | LIMIT_MIN_MAX(tmp_int, -181, 181); // avoid overflow when squared (181^2 = 32761) |
1489 | tmp_int2 = tmp_int; |
1489 | tmp_int2 = tmp_int; |
Line 1500... | Line 1500... | ||
1500 | HCGas -= tmp_long; |
1500 | HCGas -= tmp_long; |
1501 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1501 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
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; |
1505 | - | ||
1506 | // limit deviation from hoover point within the target region |
1505 | // limit deviation from hoover point within the target region |
1507 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1506 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1508 | { |
1507 | { |
1509 | unsigned int tmp,min,max; |
1508 | signed int tmp,min,max; |
1510 | if(abs(HeightDeviation) < 60) |
1509 | if(abs(HeightDeviation) < 60) |
1511 | { |
1510 | { |
1512 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1511 | LIMIT_MIN_MAX(HCGas, HoverGasMin, HoverGasMax); // limit gas around the hoover point |
1513 | } |
1512 | } |
1514 | else |
1513 | else |
1515 | { |
1514 | { |
1516 | tmp = (abs(HeightDeviation) - 60)/32; |
1515 | tmp = (abs(HeightDeviation) - 60)/32; |
1517 | if(tmp > 15) tmp = 15; |
1516 | if(tmp > 15) tmp = 15; |
1518 | max = (HoverGasMax * (tmp + 16)) / 16; |
1517 | if(HeightDeviation > 0) |
- | 1518 | { |
|
1519 | min = (HoverGasMin * (16 - tmp)) / 16; |
1519 | min = (HoverGasMin * (16 - tmp)) / 16; |
1520 | LIMIT_MIN_MAX(HCGas, min, max); // limit gas around the hoover point |
1520 | LIMIT_MIN_MAX(HCGas, min, HoverGasMax); // limit gas around the hoover point |
- | 1521 | } |
|
1521 | DebugOut.Analog[16] = min; |
1522 | else |
- | 1523 | { |
|
1522 | DebugOut.Analog[17] = max; |
1524 | max = (HoverGasMax * (tmp + 16)) / 16; |
1523 | DebugOut.Analog[18] = SollHoehe; |
1525 | LIMIT_MIN_MAX(HCGas, HoverGasMin, max); // limit gas around the hoover point |
- | 1526 | } |
|
1524 | } |
1527 | } |
1525 | } |
1528 | } |
1526 | // strech control output by inverse attitude projection 1/cos |
1529 | // strech control output by inverse attitude projection 1/cos |
1527 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1530 | // + 1/cos(angle) ++++++++++++++++++++++++++ |
1528 | tmp_long2 = (int32_t)HCGas; |
1531 | tmp_long2 = (int32_t)HCGas; |
Line 1645... | Line 1648... | ||
1645 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1648 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1646 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1649 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1647 | else SummeNick += DiffNick; // I-Anteil bei HH |
1650 | else SummeNick += DiffNick; // I-Anteil bei HH |
1648 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1651 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1649 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1652 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1650 | - | ||
1651 | //if(Parameter_UserParam5 > 50) |
- | |
1652 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
1653 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
1653 | //else |
- | |
1654 | // pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
- | |
1655 | - | ||
1656 | // Motor Vorn |
1654 | // Motor Vorn |
1657 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1655 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1658 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1656 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1659 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1657 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
Line 1664... | Line 1662... | ||
1664 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1662 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1665 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1663 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1666 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1664 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1667 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1665 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1668 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1666 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1669 | - | ||
1670 | //if(Parameter_UserParam5 > 50) |
- | |
1671 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
1667 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
1672 | //else |
- | |
1673 | // pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
- | |
1674 | - | ||
1675 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1668 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1676 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1669 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1677 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1670 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
Line 1678... | Line 1671... | ||
1678 | 1671 | ||
Line 1702... | Line 1695... | ||
1702 | //if(Parameter_UserParam4 < 50) |
1695 | //if(Parameter_UserParam4 < 50) |
1703 | { |
1696 | { |
1704 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
1697 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
1705 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1698 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1706 | } |
1699 | } |
1707 | 1700 | ||
1708 | /* |
- | |
1709 | if(Parameter_UserParam5 > 50) |
- | |
1710 | { |
- | |
1711 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
- | |
1712 | // else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
- | |
1713 | else tmp_int -= Parameter_UserParam6 * (tmp_motorwert[i] - tmp_int); // MotorSmoothing |
- | |
1714 | } |
- | |
1715 | */ |
- | |
1716 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
1701 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4); |
1717 | Motor[i].SetPoint = tmp_int / 4; |
1702 | Motor[i].SetPoint = tmp_int / 4; |
1718 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |
1703 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |
1719 | tmp_motorwert[i] = tmp_int; |
1704 | tmp_motorwert[i] = tmp_int; |
1720 | } |
1705 | } |