Subversion Repositories FlightCtrl

Rev

Rev 1680 | Rev 1683 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1680 Rev 1682
Line 177... Line 177...
177 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
 177 
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az;
178 
    DebugOut.Analog[8] = KompassValue;
 178 
    DebugOut.Analog[8] = KompassValue;
179 
    DebugOut.Analog[9] = UBat;
 179 
    DebugOut.Analog[9] = UBat;
180 
    DebugOut.Analog[10] = SenderOkay;
 180 
    DebugOut.Analog[10] = SenderOkay;
181 
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
 181 
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;
182 
        DebugOut.Analog[16] = Capacity.MinOfMaxPWM;
 - 
 
183 
    DebugOut.Analog[20] = ServoNickValue;
 182 
    DebugOut.Analog[20] = ServoNickValue;
184 
    DebugOut.Analog[22] = Capacity.ActualCurrent;
 183 
    DebugOut.Analog[22] = Capacity.ActualCurrent;
185 
    DebugOut.Analog[23] = Capacity.UsedCapacity;
 184 
    DebugOut.Analog[23] = Capacity.UsedCapacity;
186 
//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
 185 
//    DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ;
187 
//    DebugOut.Analog[24] = MesswertNick/2;
 186 
//    DebugOut.Analog[24] = MesswertNick/2;
188 
//    DebugOut.Analog[25] = MesswertRoll/2;
 187 
//    DebugOut.Analog[25] = MesswertRoll/2;
189 
//    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
 188 
//    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
190 
//    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
 189 
//    DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion;
191 
//    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
 190 
//    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
192 
    //DebugOut.Analog[28] = I2CError;
 191 
    //DebugOut.Analog[28] = I2CError;
193 
    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
 192 
//    DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay;
- 
 
 193 
        DebugOut.Analog[29] = Capacity.MinOfMaxPWM;
194 
    DebugOut.Analog[30] = GPS_Nick;
 194 
    DebugOut.Analog[30] = GPS_Nick;
195 
    DebugOut.Analog[31] = GPS_Roll;
 195 
    DebugOut.Analog[31] = GPS_Roll;
196 
}
 196 
}
Line 197... Line 197...
197 
 
 197 
 
Line 592... Line 592...
592 
 MIN_GAS = EE_Parameter.Gas_Min;
 592 
 MIN_GAS = EE_Parameter.Gas_Min;
Line 593... Line 593...
593 
 
 593 
 
594 
 tmp = EE_Parameter.OrientationModeControl;
 594 
 tmp = EE_Parameter.OrientationModeControl;
595 
 if(tmp > 50 && NaviDataOkay > 200)
 595 
 if(tmp > 50 && NaviDataOkay > 200)
- 
 
 596 
   {
- 
 
 597 
#ifdef SWITCH_LEARNS_CAREFREE
- 
 
 598 
    if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2;
596 
   {
 599 
#endif
597 
    CareFree = 1;
 600 
        CareFree = 1;
598 
    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
 601 
    if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0;
599 
   }
 602 
   }
Line 600... Line 603...
600 
   else CareFree = 0;
 603 
   else CareFree = 0;
Line 667... Line 670...
667 
                }
 670 
                }
668 
            if((modell_fliegt < 256))
 671 
            if((modell_fliegt < 256))
669 
                {
 672 
                {
670 
                SummeNick = 0;
 673 
                SummeNick = 0;
671 
                SummeRoll = 0;
 674 
                SummeRoll = 0;
- 
 
 675 
                sollGier = 0;
- 
 
 676 
                Mess_Integral_Gier = 0;
672 
                if(modell_fliegt == 250)
 677 
                if(modell_fliegt == 250)
673 
                 {
 678 
                 {
674 
                  NeueKompassRichtungMerken = 1;
 679 
                  NeueKompassRichtungMerken = 1;
675 
                  sollGier = 0;
 - 
 
676 
                  Mess_Integral_Gier = 0;
 - 
 
677 
//                  Mess_Integral_Gier2 = 0;
 680 
//                  Mess_Integral_Gier2 = 0;
678 
                 }
 681 
                 }
679 
                } else FCFlags |= FCFLAG_FLY;
 682 
                } else FCFlags |= FCFLAG_FLY;
Line 680... Line 683...
680 
 
 683 
 
Line 1617... Line 1620...
1617 
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
 1620 
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
Line 1618... Line 1621...
1618 
 
 1621 
 
1619 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1622 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1620 
// Nick-Achse
 1623 
// Nick-Achse
- 
 
 1624 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1621 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1625 
#define ABSCHWAECHEN 2
1622 
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
 1626 
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1623 
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
 1627 
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1624 
    else  SummeNick += DiffNick; // I-Anteil bei HH
 1628 
    else  SummeNick += DiffNick; // I-Anteil bei HH
1625 
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
 1629 
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
- 
 
 1630 
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
- 
 
 1631 
if(Parameter_UserParam5 > 50)
- 
 
 1632 
        pd_ergebnis_nick = DiffNick/ABSCHWAECHEN + SummeNick / Ki; // PI-Regler für Nick
1626 
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
 1633 
else
- 
 
 1634 
        pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
1627 
    pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
 1635 
 
1628 
    // Motor Vorn
 1636 
    // Motor Vorn
1629 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
 1637 
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1630 
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
 1638 
    if(pd_ergebnis_nick >  tmp_int) pd_ergebnis_nick =  tmp_int;
Line 1636... Line 1644...
1636 
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
 1644 
        DiffRoll = MesswertRoll - StickRoll;    // Differenz bestimmen
1637 
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
 1645 
    if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung
1638 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
 1646 
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1639 
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
 1647 
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1640 
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
 1648 
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
- 
 
 1649 
 
- 
 
 1650 
if(Parameter_UserParam5 > 50)
- 
 
 1651 
    pd_ergebnis_roll = DiffRoll/ABSCHWAECHEN + SummeRoll / Ki;  // PI-Regler für Roll
- 
 
 1652 
else
1641 
    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
 1653 
    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
- 
 
 1654 
 
1642 
    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;
1643 
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
 1656 
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
1644 
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
 1657 
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
Line -... Line 1658...
- 
 
 1658 
 
- 
 
 1659 
DebugOut.Analog[17] = SummeRoll / 4;
- 
 
 1660 
DebugOut.Analog[16] = SummeNick / 4;
- 
 
 1661 
DebugOut.Analog[18] = Mess_Integral_Gier/2;
1645 
 
 1662 
 
1646 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1663 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1647 
// Universal Mixer
 1664 
// Universal Mixer
1648 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1665 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1649 
        for(i=0; i<MAX_MOTORS; i++)
 1666 
        for(i=0; i<MAX_MOTORS; i++)