Subversion Repositories FlightCtrl

Rev

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

Rev 709 Rev 720
Line 337... Line 337...
337 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
 337 
    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;
338 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
 338 
    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;
339 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
 339 
    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;
Line 340... Line 340...
340 
 
 340 
 
341 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 341 
        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
342 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;
 342 
        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;
Line 343... Line 343...
343 
}
 343 
}
344 
 
 344 
 
345 
//############################################################################
 345 
//############################################################################
Line 785... Line 785...
785 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 785 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
Line 786... Line 786...
786 
 
 786 
 
787 
   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
 787 
   if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
788 
    {
 788 
    {
789 
     LageKorrekturNick /= 2;
 789 
     LageKorrekturNick /= 2;
790 
     LageKorrekturNick /= 2;
 790 
     LageKorrekturRoll /= 2;
Line 791... Line 791...
791 
    }
 791 
    }
792 
 
 792 
 
793 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
793 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
Line 808... Line 808...
808 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
 808 
//    IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2;
809 
DebugOut.Analog[10] = GierGyroFehler;
 809 
DebugOut.Analog[10] = GierGyroFehler;
810 
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
 810 
    if(GierGyroFehler > ABGLEICH_ANZAHL/2) AdNeutralGier++;
811 
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
 811 
    if(GierGyroFehler <-ABGLEICH_ANZAHL/2) AdNeutralGier--;
Line -... Line 812...
- 
 
 812 
 
- 
 
 813 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
812 
 
 814 
 
- 
 
 815 
    GierGyroFehler = 0;
- 
 
 816 
 
813 
    GierGyroFehler = 0;
 817 
 
814 
DebugOut.Analog[17] = IntegralAccNick / 26;
 818 
/*DebugOut.Analog[17] = IntegralAccNick / 26;
815 
DebugOut.Analog[18] = IntegralAccRoll / 26;
 819 
DebugOut.Analog[18] = IntegralAccRoll / 26;
816 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
 820 
DebugOut.Analog[19] = IntegralFehlerNick;// / 26;
- 
 
 821 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
817 
DebugOut.Analog[20] = IntegralFehlerRoll;// / 26;
 822 
*/
818 
DebugOut.Analog[21] = MittelIntegralNick / 26;
 823 
//DebugOut.Analog[21] = MittelIntegralNick / 26;
819 
DebugOut.Analog[22] = MittelIntegralRoll / 26;
 824 
//MittelIntegralRoll = MittelIntegralRoll;
- 
 
 825 
//DebugOut.Analog[28] = ausgleichNick;
820 
//DebugOut.Analog[28] = ausgleichNick;
 826 
/*
821 
DebugOut.Analog[29] = ausgleichRoll;
 827 
DebugOut.Analog[29] = ausgleichRoll;
- 
 
 828 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
Line 822... Line 829...
822 
DebugOut.Analog[30] = LageKorrekturRoll * 10;
 829 
*/
823 
 
 830 
 
824 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 831 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
825 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 832 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
Line 889... Line 896...
889 
        }
 896 
        }
Line 890... Line 897...
890 
 
 897 
 
891 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 898 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
892 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 899 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
893 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 900 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
894 
DebugOut.Analog[27] = ausgleichRoll;
 901 
/*DebugOut.Analog[27] = ausgleichRoll;
895 
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
 902 
DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick);
- 
 
 903 
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
896 
DebugOut.Analog[24] = 10*(AdNeutralRoll - StartNeutralRoll);
 904 
*/
897 
  }
 905 
  }
898 
  else
 906 
  else
899 
  {
 907 
  {
900 
   LageKorrekturRoll = 0;
 908 
   LageKorrekturRoll = 0;
Line 990... Line 998...
990 
    DebugOut.Analog[9] = UBat;
 998 
    DebugOut.Analog[9] = UBat;
991 
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;  
 999 
    DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR;  
992 
    DebugOut.Analog[10] = SenderOkay;
 1000 
    DebugOut.Analog[10] = SenderOkay;
993 
    DebugOut.Analog[16] = Mittelwert_AccHoch;
 1001 
    DebugOut.Analog[16] = Mittelwert_AccHoch;
Line -... Line 1002...
- 
 
 1002 
 
- 
 
 1003 
    DebugOut.Analog[30] = GPS_Nick;
- 
 
 1004 
    DebugOut.Analog[31] = GPS_Roll;
994 
 
 1005 
 
995 
/*    DebugOut.Analog[16] = motor_rx[0];
 1006 
/*    DebugOut.Analog[16] = motor_rx[0];
996 
    DebugOut.Analog[17] = motor_rx[1];
 1007 
    DebugOut.Analog[17] = motor_rx[1];
997 
    DebugOut.Analog[18] = motor_rx[2];
 1008 
    DebugOut.Analog[18] = motor_rx[2];
998 
    DebugOut.Analog[19] = motor_rx[3];
 1009 
    DebugOut.Analog[19] = motor_rx[3];
Line 1026... Line 1037...
1026 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
 1037 
    else             MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor;
1027 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
 1038 
    if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor;
1028 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
 1039 
    else             MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor;
1029 
    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
 1040 
    MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2;
Line -... Line 1041...
- 
 
 1041 
 
1030 
 
 1042 
/*
1031 
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
 1043 
DebugOut.Analog[25] = IntegralRoll * IntegralFaktor;
1032 
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
 1044 
DebugOut.Analog[31] = StickRoll;// / (26*IntegralFaktor);
1033 
DebugOut.Analog[28] = MesswertRoll;
 1045 
DebugOut.Analog[28] = MesswertRoll;
1034 
 
 1046 
*/
1035 
    // Maximalwerte abfangen
 1047 
    // Maximalwerte abfangen
1036 
    #define MAX_SENSOR  2048
 1048 
    #define MAX_SENSOR  2048
1037 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
 1049 
    if(MesswertNick >  MAX_SENSOR) MesswertNick =  MAX_SENSOR;
1038 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;
 1050 
    if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR;