Subversion Repositories FlightCtrl

Rev

Rev 188 | Rev 304 | 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;