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