Subversion Repositories FlightCtrl

Rev

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

Rev 188 Rev 297
Line 654... Line 654...
654
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
654
    if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR)
655
        {
655
        {
656
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
656
        IntegralFehlerNick = IntegralNick2 - IntegralNick;
657
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
657
        IntegralFehlerRoll = IntegralRoll2 - IntegralRoll;
658
        ZaehlMessungen = 0;
658
        ZaehlMessungen = 0;
659
        if(IntegralFehlerNick > 500/DRIFT_FAKTOR)   AdNeutralNick++;
659
        if(IntegralFehlerNick > 800/DRIFT_FAKTOR)   AdNeutralNick++;
660
        if(IntegralFehlerNick < -500/DRIFT_FAKTOR)  AdNeutralNick--;
660
        if(IntegralFehlerNick < -800/DRIFT_FAKTOR)  AdNeutralNick--;
661
        if(IntegralFehlerRoll > 500/DRIFT_FAKTOR)   AdNeutralRoll++;
661
        if(IntegralFehlerRoll > 800/DRIFT_FAKTOR)   AdNeutralRoll++;
662
        if(IntegralFehlerRoll < -500/DRIFT_FAKTOR)  AdNeutralRoll--;
662
        if(IntegralFehlerRoll < -800/DRIFT_FAKTOR)  AdNeutralRoll--;
663
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
663
//        if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR)  AdNeutralGier--;   // macht nur mit Referenz (Kompass Sinn)      
664
//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)      
664
//        if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR)  AdNeutralGier++;   // macht nur mit Referenz (Kompass Sinn)      
665
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
665
    ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern
666
        Mess_IntegralNick2 = IntegralNick;
666
        Mess_IntegralNick2 = IntegralNick;
667
        Mess_IntegralRoll2 = IntegralRoll;
667
        Mess_IntegralRoll2 = IntegralRoll;
Line 669... Line 669...
669
    ANALOG_ON;  // ADC einschalten
669
    ANALOG_ON;  // ADC einschalten
670
        }
670
        }
671
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
671
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
672
// Integrale auf ACC-Signal abgleichen
672
// Integrale auf ACC-Signal abgleichen
673
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
673
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
674
  if(IntegralFaktor && !Looping_Nick && !Looping_Roll)
674
  if(!Looping_Nick && !Looping_Roll)
675
  {
675
  {
676
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
676
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
677
    if(labs(Mittelwert_AccNick) < 200) tmp_long /= 8;
-
 
678
    else tmp_long /= 16;
677
    tmp_long /= 32;
679
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
678
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
680
    if(labs(Mittelwert_AccRoll) < 200) tmp_long2 /= 8;
-
 
681
    else tmp_long2 /= 16;
679
    tmp_long2 /= 32;
Line 682... Line 680...
682
 
680
 
683
 #define AUSGLEICH 500
681
 #define AUSGLEICH 100
684
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
682
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
685
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
683
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
686
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
684
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
687
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
685
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
Line 750... Line 748...
750
    DebugOut.Analog[3] = Mittelwert_AccRoll;
748
    DebugOut.Analog[3] = Mittelwert_AccRoll;
751
    DebugOut.Analog[4] = MesswertGier;
749
    DebugOut.Analog[4] = MesswertGier;
752
    DebugOut.Analog[5] = HoehenWert;
750
    DebugOut.Analog[5] = HoehenWert;
753
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
751
    DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);
754
    DebugOut.Analog[8] = KompassValue;
752
    DebugOut.Analog[8] = KompassValue;
-
 
753
    DebugOut.Analog[9] = UBat;
-
 
754
    DebugOut.Analog[10] = SenderOkay;
Line 755... Line 755...
755
 
755
 
756
/*    DebugOut.Analog[16] = motor_rx[0];
756
/*    DebugOut.Analog[16] = motor_rx[0];
757
    DebugOut.Analog[17] = motor_rx[1];
757
    DebugOut.Analog[17] = motor_rx[1];
758
    DebugOut.Analog[18] = motor_rx[2];
758
    DebugOut.Analog[18] = motor_rx[2];
Line 865... Line 865...
865
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
865
    DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick));    // Differenz bestimmen
866
    SummeNick += DiffNick;                                   // I-Anteil
866
    SummeNick += DiffNick;                                   // I-Anteil
867
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
867
    if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1;
868
    if(SummeNick >  16000) SummeNick =  16000;
868
    if(SummeNick >  16000) SummeNick =  16000;
869
    if(SummeNick < -16000) SummeNick = -16000;
869
    if(SummeNick < -16000) SummeNick = -16000;
870
    pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick                                   
870
    pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick                                      
871
    // Motor Vorn
871
    // Motor Vorn
872
#define MUL  2
872
#define MUL  2
873
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
873
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
874
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
874
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
Line 890... Line 890...
890
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
890
        DiffRoll = Kp * (MesswertRoll - (StickRoll  - GPS_Roll));       // Differenz bestimmen
891
        SummeRoll += DiffRoll;                                                              // I-Anteil
891
        SummeRoll += DiffRoll;                                                              // I-Anteil
892
    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;
893
    if(SummeRoll >  16000) SummeRoll =  16000;
893
    if(SummeRoll >  16000) SummeRoll =  16000;
894
    if(SummeRoll < -16000) SummeRoll = -16000;
894
    if(SummeRoll < -16000) SummeRoll = -16000;
895
    pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll
895
    pd_ergebnis = DiffRoll + Ki * SummeRoll;    // PI-Regler für Roll
896
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
896
    if(pd_ergebnis >  MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis =  MUL * (GasMischanteil + abs(GierMischanteil));
897
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
897
    if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil));
898
    // Motor Links
898
    // Motor Links
899
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
899
    motorwert = GasMischanteil + pd_ergebnis - GierMischanteil;
900
        if ((motorwert < 0)) motorwert = 0;
900
        if ((motorwert < 0)) motorwert = 0;