Rev 1912 | Rev 1914 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1912 | Rev 1913 | ||
|---|---|---|---|
| Line 612... | Line 612... | ||
| 612 | 612 | ||
| 613 | tmp = EE_Parameter.OrientationModeControl; |
613 | tmp = EE_Parameter.OrientationModeControl; |
| 614 | if(tmp > 50) |
614 | if(tmp > 50) |
| 615 | { |
615 | { |
| 616 | #ifdef SWITCH_LEARNS_CAREFREE |
616 | #ifdef SWITCH_LEARNS_CAREFREE |
| 617 | if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
617 | // if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
| 618 | #endif |
618 | #endif |
| 619 | CareFree = 1; |
619 | CareFree = 1; |
| 620 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
620 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
| 621 | if(carefree_old != CareFree) |
621 | if(carefree_old != CareFree) |
| Line 805... | Line 805... | ||
| 805 | Mess_IntegralNick2 = IntegralNick; |
805 | Mess_IntegralNick2 = IntegralNick; |
| 806 | Mess_IntegralRoll2 = IntegralRoll; |
806 | Mess_IntegralRoll2 = IntegralRoll; |
| 807 | SummeNick = 0; |
807 | SummeNick = 0; |
| 808 | SummeRoll = 0; |
808 | SummeRoll = 0; |
| 809 | FC_StatusFlags |= FC_STATUS_START; |
809 | FC_StatusFlags |= FC_STATUS_START; |
| 810 | ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
810 | // ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
| 811 | NeueKompassRichtungMerken = 100; // 2 sekunden |
811 | NeueKompassRichtungMerken = 100; // 2 sekunden |
| 812 | } |
812 | } |
| 813 | else |
813 | else |
| 814 | { |
814 | { |
| 815 | beeptime = 1500; // indicate missing calibration |
815 | beeptime = 1500; // indicate missing calibration |
| Line 1485... | Line 1485... | ||
| 1485 | } |
1485 | } |
| 1486 | } |
1486 | } |
| 1487 | else |
1487 | else |
| 1488 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1488 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
| 1489 | { |
1489 | { |
| 1490 | if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height |
1490 | //if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height |
| - | 1491 | //else |
|
| - | 1492 | if(!WaypointTrimming) LIMIT_MIN_MAX(SollHoehe, (HoehenWert-128), (HoehenWert+128)); // max. 1m Unterschied |
|
| 1491 | else WaypointTrimming = 0; |
1493 | else WaypointTrimming = 0; |
| 1492 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1494 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
| 1493 | HeightTrimming = 0; |
1495 | HeightTrimming = 0; |
| 1494 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1496 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
| 1495 | if(!StartTrigger && HoehenWert > 50) |
1497 | if(!StartTrigger && HoehenWert > 50) |
| 1496 | { |
1498 | { |
| Line 1507... | Line 1509... | ||
| 1507 | else SollHoehe += WaypointTrimming; |
1509 | else SollHoehe += WaypointTrimming; |
| 1508 | } |
1510 | } |
| 1509 | else |
1511 | else |
| 1510 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1512 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| 1511 | HeightTrimming = 0; |
1513 | HeightTrimming = 0; |
| 1512 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1514 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // max. 2m Unterschied |
| 1513 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1515 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
| 1514 | //update hoover gas stick value when setpoint is shifted |
1516 | //update hoover gas stick value when setpoint is shifted |
| 1515 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
1517 | if(!EE_Parameter.Hoehe_StickNeutralPoint && FromNC_AltitudeSpeed == 0) |
| 1516 | { |
1518 | { |
| 1517 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
1519 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
| Line 1778... | Line 1780... | ||
| 1778 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1780 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
| Line 1779... | Line 1781... | ||
| 1779 | 1781 | ||
| 1780 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
1782 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
| Line -... | Line 1783... | ||
| - | 1783 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
|
| - | 1784 | ||
| - | 1785 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
|
| - | 1786 | // else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // original MotorSmoothing |
|
| - | 1787 | else |
|
| - | 1788 | { |
|
| - | 1789 | if(EE_Parameter.MotorSmooth == 0) |
|
| - | 1790 | { |
|
| - | 1791 | tmp_int = 2 * tmp_int - tmp_motorwert[i]; |
|
| - | 1792 | } |
|
| - | 1793 | else // 1 means tmp_int = tmp_int; |
|
| - | 1794 | if(EE_Parameter.MotorSmooth > 1) |
|
| - | 1795 | { |
|
| - | 1796 | // If >= 2 then allow >= 50% of the intended step down to rapidly reach the intended value. |
|
| - | 1797 | tmp_int = tmp_int + ((tmp_motorwert[i] - tmp_int)/EE_Parameter.MotorSmooth); |
|
| 1781 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1798 | } |
| 1782 | 1799 | } |
|
| 1783 | LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4); |
1800 | LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4); |
| 1784 | Motor[i].SetPoint = tmp_int / 4; |
1801 | Motor[i].SetPoint = tmp_int / 4; |
| 1785 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |
1802 | Motor[i].SetPointLowerBits = (tmp_int % 4)<<1; // (3 bits total) |