Subversion Repositories FlightCtrl

Rev

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

Rev 173 Rev 188
Line 695... Line 695...
695
    Mess_IntegralNick -= tmp_long;
695
    Mess_IntegralNick -= tmp_long;
696
    Mess_IntegralRoll -= tmp_long2;
696
    Mess_IntegralRoll -= tmp_long2;
697
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
697
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
698
//  Gieren
698
//  Gieren
699
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
699
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
700
    if(abs(StickGier) > 35)
700
    if(abs(StickGier) > 20) // war 35 
701
     {
701
     {
702
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
702
      if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1;
703
     }
703
     }
704
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
704
    tmp_int  = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo  y = ax + bx²
705
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
705
    tmp_int += (EE_Parameter.Gier_P * StickGier) / 4;
Line 714... Line 714...
714
//  Kompass
714
//  Kompass
715
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
715
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
716
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
716
    if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV))
717
     {
717
     {
718
       int w,v;
718
       int w,v;
719
       static int SignalSchlecht = 0;
719
       static int SignalSchlecht = 0;
720
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
720
       w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln
721
       v = abs(IntegralRoll /512);
721
       v = abs(IntegralRoll /512);
722
       if(v > w) w = v; // grösste Neigung ermitteln
722
       if(v > w) w = v; // grösste Neigung ermitteln
723
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
723
       if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht)    
724
        {
724
        {
Line 847... Line 847...
847
  DebugOut.Analog[7] = GasMischanteil;
847
  DebugOut.Analog[7] = GasMischanteil;
Line 848... Line 848...
848
 
848
 
849
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
849
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
850
// Gier-Anteil
850
// Gier-Anteil
851
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
851
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
852
#define MUL_G  1.4
852
#define MUL_G  1.0
853
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
853
    GierMischanteil = MesswertGier - sollGier;     // Regler für Gier
854
//GierMischanteil = 0;
854
//GierMischanteil = 0;
855
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
855
    if(GierMischanteil > (MUL_G * GasMischanteil))  GierMischanteil = MUL_G * GasMischanteil;
-
 
856
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
-
 
857
    if(GierMischanteil > 100)  GierMischanteil = 100;
Line 856... Line 858...
856
    if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil);
858
    if(GierMischanteil < -100) GierMischanteil = -100;
857
 
859
 
858
    if(GasMischanteil < 10) GierMischanteil = 0;
860
    if(GasMischanteil < 20) GierMischanteil = 0;
859
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
861
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
Line 860... Line 862...
860
// Nick-Achse
862
// Nick-Achse
Line 866... Line 868...
866
    if(SummeNick >  16000) SummeNick =  16000;
868
    if(SummeNick >  16000) SummeNick =  16000;
867
    if(SummeNick < -16000) SummeNick = -16000;
869
    if(SummeNick < -16000) SummeNick = -16000;
868
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick                                   
870
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick                                   
869
    // Motor Vorn
871
    // Motor Vorn
870
#define MUL  2
872
#define MUL  2
871
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
873
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
872
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
874
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
Line 873... Line 875...
873
 
875
 
874
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
876
    motorwert = GasMischanteil + pd_ergebnis + GierMischanteil;   // Mischer
875
        if ((motorwert < 0)) motorwert = 0;
877
        if ((motorwert < 0)) motorwert = 0;
876
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
878
        else if(motorwert > MAX_GAS)        motorwert = MAX_GAS;
Line 889... Line 891...
889
        SummeRoll += DiffRoll;                                                              // I-Anteil
891
        SummeRoll += DiffRoll;                                                              // I-Anteil
890
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
892
    if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1;
891
    if(SummeRoll >  16000) SummeRoll =  16000;
893
    if(SummeRoll >  16000) SummeRoll =  16000;
892
    if(SummeRoll < -16000) SummeRoll = -16000;
894
    if(SummeRoll < -16000) SummeRoll = -16000;
893
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
895
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
894
    if((pd_ergebnis >  MUL * (GasMischanteil))) pd_ergebnis =  MUL * (GasMischanteil);
896
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
895
    if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil);
897
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
896
    // Motor Links
898
    // Motor Links
897
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
899
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
898
        if ((motorwert < 0)) motorwert = 0;
900
        if ((motorwert < 0)) motorwert = 0;
899
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
901
        else if(motorwert > MAX_GAS)            motorwert = MAX_GAS;
900
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;
902
        if (motorwert < MIN_GAS)            motorwert = MIN_GAS;