Subversion Repositories FlightCtrl

Rev

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

Rev 1683 Rev 1685
Line 73... Line 73...
73
long Integral_Gier = 0;
73
long Integral_Gier = 0;
74
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
74
long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;
75
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
75
long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;
76
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
76
long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;
77
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
77
long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;
-
 
78
long SummeNick=0,SummeRoll=0;
78
volatile long Mess_Integral_Hoch = 0;
79
volatile long Mess_Integral_Hoch = 0;
79
int  KompassValue = 0;
80
int  KompassValue = 0;
80
int  KompassStartwert = 0;
81
int  KompassStartwert = 0;
81
int  KompassRichtung = 0;
82
int  KompassRichtung = 0;
82
unsigned int  KompassSignalSchlecht = 500;
83
unsigned int  KompassSignalSchlecht = 500;
Line 610... Line 611...
610
void MotorRegler(void)
611
void MotorRegler(void)
611
//############################################################################
612
//############################################################################
612
{
613
{
613
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
614
         int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2;
614
         int GierMischanteil,GasMischanteil;
615
         int GierMischanteil,GasMischanteil;
615
     static long SummeNick=0,SummeRoll=0;
-
 
616
     static long sollGier = 0,tmp_long,tmp_long2;
616
     static long sollGier = 0,tmp_long,tmp_long2;
617
     static long IntegralFehlerNick = 0;
617
     static long IntegralFehlerNick = 0;
618
     static long IntegralFehlerRoll = 0;
618
     static long IntegralFehlerRoll = 0;
619
         static unsigned int RcLostTimer;
619
         static unsigned int RcLostTimer;
620
         static unsigned char delay_neutral = 0;
620
         static unsigned char delay_neutral = 0;
Line 1259... Line 1259...
1259
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1259
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);
1260
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
1260
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN));
Line 1261... Line 1261...
1261
 
1261
 
1262
    // Maximalwerte abfangen
1262
    // Maximalwerte abfangen
1263
//    #define MAX_SENSOR  (4096*STICK_GAIN)
1263
//    #define MAX_SENSOR  (4096*STICK_GAIN)
1264
    #define MAX_SENSOR  (4096*4)
1264
    #define MAX_SENSOR  (4096)
1265
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1265
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1266
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1266
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
1267
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1267
    if(MesswertRoll >  MAX_SENSOR) MesswertRoll =  MAX_SENSOR;
1268
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
1268
    if(MesswertRoll < -MAX_SENSOR) MesswertRoll = -MAX_SENSOR;
Line 1620... Line 1620...
1620
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
1620
    if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil));
Line 1621... Line 1621...
1621
 
1621
 
1622
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1622
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1623
// Nick-Achse
1623
// Nick-Achse
1624
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
1625
#define ABSCHWAECHEN 2
1624
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1626
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1625
    DiffNick = MesswertNick - StickNick;        // Differenz bestimmen
1627
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1626
    if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung
1628
    else  SummeNick += DiffNick; // I-Anteil bei HH
1627
    else  SummeNick += DiffNick; // I-Anteil bei HH
1629
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
1628
    if(SummeNick >  (STICK_GAIN * 16000L)) SummeNick =  (STICK_GAIN * 16000L);
-
 
1629
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
-
 
1630
 
-
 
1631
DebugOut.Analog[16] = MesswertNick;
-
 
1632
DebugOut.Analog[17] = DiffNick;
-
 
1633
 
1630
    if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN);
1634
 
1631
if(Parameter_UserParam5 > 50)
1635
if(Parameter_UserParam5 > 50)
1632
        pd_ergebnis_nick = DiffNick/ABSCHWAECHEN + SummeNick / Ki; // PI-Regler für Nick
1636
        pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick
1633
else
1637
else
Line 1634... Line 1638...
1634
        pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
1638
        pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick
1635
 
1639
 
Line 1646... Line 1650...
1646
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1650
    else                 SummeRoll += DiffRoll;  // I-Anteil bei HH
1647
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1651
    if(SummeRoll >  (STICK_GAIN * 16000L)) SummeRoll =  (STICK_GAIN * 16000L);
1648
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
1652
    if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN);
Line 1649... Line 1653...
1649
 
1653
 
1650
if(Parameter_UserParam5 > 50)
1654
if(Parameter_UserParam5 > 50)
1651
    pd_ergebnis_roll = DiffRoll/ABSCHWAECHEN + SummeRoll / Ki;  // PI-Regler für Roll
1655
    pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki;   // PI-Regler für Roll
1652
else
1656
else
Line 1653... Line 1657...
1653
    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
1657
    pd_ergebnis_roll = DiffRoll + SummeRoll / Ki;       // PI-Regler für Roll
1654
 
1658
 
1655
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
1659
    tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64;
Line 1656... Line -...
1656
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
-
 
1657
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
-
 
1658
 
-
 
1659
DebugOut.Analog[17] = SummeRoll / 4;
-
 
1660
DebugOut.Analog[16] = SummeNick / 4;
1660
    if(pd_ergebnis_roll >  tmp_int) pd_ergebnis_roll =  tmp_int;
1661
DebugOut.Analog[18] = Mess_Integral_Gier/2;
1661
    if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int;
1662
 
1662
 
1663
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1663
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1664
// Universal Mixer
1664
// Universal Mixer