Subversion Repositories FlightCtrl

Rev

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

Rev 978 Rev 992
Line 211... Line 211...
211 
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
 211 
    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;
212 
    GierGyroFehler = 0;
 212 
    GierGyroFehler = 0;
213 
    SendVersionToNavi = 1;
 213 
    SendVersionToNavi = 1;
214 
    LED_Init();
 214 
    LED_Init();
215 
    MikroKopterFlags |= FLAG_CALIBRATE;
 215 
    MikroKopterFlags |= FLAG_CALIBRATE;
- 
 
 216 
    FromNaviCtrl_Value.Kalman_K = -1;
- 
 
 217 
    FromNaviCtrl_Value.Kalman_MaxDrift = EE_Parameter.Driftkomp * 16;
- 
 
 218 
    FromNaviCtrl_Value.Kalman_MaxFusion = 32;
216 
}
 219 
}
Line 217... Line 220...
217 
 
 220 
 
218 
//############################################################################
 221 
//############################################################################
219 
// Bearbeitet die Messwerte
 222 
// Bearbeitet die Messwerte
Line 809... Line 812...
809 
 
 812 
 
810 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
813 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
811 
  if(!Looping_Nick && !Looping_Roll)
 814 
  if(!Looping_Nick && !Looping_Roll)
812 
  {
 815 
  {
- 
 
 816 
   long tmp_long, tmp_long2;
- 
 
 817 
   if(FromNaviCtrl_Value.Kalman_K != -1)
813 
   long tmp_long, tmp_long2;
 818 
     {
- 
 
 819 
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
- 
 
 820 
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
- 
 
 821 
      tmp_long  = (tmp_long  * FromNaviCtrl_Value.Kalman_K) / (32 * 16);    
- 
 
 822 
      tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16);    
- 
 
 823 
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
- 
 
 824 
      {
- 
 
 825 
      tmp_long  /= 2;
- 
 
 826 
      tmp_long2 /= 2;
- 
 
 827 
      }
- 
 
 828 
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
- 
 
 829 
      {
- 
 
 830 
      tmp_long  /= 3;
- 
 
 831 
      tmp_long2 /= 3;
- 
 
 832 
      }
- 
 
 833 
      if(tmp_long >  (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
- 
 
 834 
      if(tmp_long <  (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long  = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
- 
 
 835 
      if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion;
- 
 
 836 
      if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion)  tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion;
- 
 
 837 
     }
- 
 
 838 
     else
- 
 
 839 
     {
814 
    tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
 840 
      tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick);
815 
    tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
 841 
      tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll);
816 
    tmp_long /= 16;
 842 
      tmp_long /= 16;
817 
    tmp_long2 /= 16;
 843 
      tmp_long2 /= 16;
818 
   if((MaxStickNick > 64) || (MaxStickRoll > 64))
 844 
     if((MaxStickNick > 64) || (MaxStickRoll > 64))
819 
    {
 845 
      {
820 
    tmp_long  /= 3;
 846 
      tmp_long  /= 3;
821 
    tmp_long2 /= 3;
 847 
      tmp_long2 /= 3;
822 
    }
 848 
      }
823 
   if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
 849 
     if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)
824 
    {
 850 
      {
825 
    tmp_long  /= 3;
 851 
      tmp_long  /= 3;
826 
    tmp_long2 /= 3;
 852 
      tmp_long2 /= 3;
827 
    }
 - 
 
828 
 
 853 
      }
829 
 #define AUSGLEICH 32
 854 
 #define AUSGLEICH 32
830 
    if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
 855 
      if(tmp_long >  AUSGLEICH)  tmp_long  = AUSGLEICH;
831 
    if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
 856 
      if(tmp_long < -AUSGLEICH)  tmp_long  =-AUSGLEICH;
832 
    if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
 857 
      if(tmp_long2 > AUSGLEICH)  tmp_long2 = AUSGLEICH;
- 
 
 858 
      if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
833 
    if(tmp_long2 <-AUSGLEICH)  tmp_long2 =-AUSGLEICH;
 859 
     }
834 
 
 860 
     
835 
    Mess_IntegralNick -= tmp_long;
 861 
    Mess_IntegralNick -= tmp_long;
836 
    Mess_IntegralRoll -= tmp_long2;
 862 
    Mess_IntegralRoll -= tmp_long2;
837 
  }
 863 
  }
838 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
- 
 
839 
 
 864 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
840 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 865 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
841 
 {
 866 
 {
842 
  static int cnt = 0;
 867 
  static int cnt = 0;
843 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
 868 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
Line 858... Line 883...
858 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
 883 
    ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich;
Line 859... Line 884...
859 
 
 884 
 
860 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
 885 
    LageKorrekturNick = ausgleichNick / ABGLEICH_ANZAHL;
Line 861... Line 886...
861 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
 886 
    LageKorrekturRoll = ausgleichRoll / ABGLEICH_ANZAHL;
862 
 
 887 
 
863 
   if((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25))
 888 
   if(((MaxStickNick > 64) || (MaxStickRoll > 64) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) && (FromNaviCtrl_Value.Kalman_K == -1))
864 
    {
 889 
    {
865 
     LageKorrekturNick /= 2;
 890 
     LageKorrekturNick /= 2;
Line 906... Line 931...
906 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
 931 
#define FEHLER_LIMIT  (ABGLEICH_ANZAHL * 4)
907 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
 932 
#define FEHLER_LIMIT2 (ABGLEICH_ANZAHL * 16)
908 
#define BEWEGUNGS_LIMIT 20000
 933 
#define BEWEGUNGS_LIMIT 20000
909 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
 934 
// Nick +++++++++++++++++++++++++++++++++++++++++++++++++
910 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 935 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
911 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT)
 936 
        if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
912 
        {
 937 
        {
913 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
 938 
        if(IntegralFehlerNick >  FEHLER_LIMIT2)
914 
         {
 939 
         {
915 
           if(last_n_p)
 940 
           if(last_n_p)
916 
           {
 941 
           {
Line 937... Line 962...
937 
        {
 962 
        {
938 
         cnt = 0;
 963 
         cnt = 0;
939 
         KompassSignalSchlecht = 1000;
 964 
         KompassSignalSchlecht = 1000;
940 
        }
 965 
        }
941 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 966 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
- 
 
 967 
        if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
942 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
 968 
        if(IntegralFehlerNick >  FEHLER_LIMIT)   AdNeutralNick += cnt;
943 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
 969 
        if(IntegralFehlerNick < -FEHLER_LIMIT)   AdNeutralNick -= cnt;
Line 944... Line 970...
944 
 
 970 
 
945 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
 971 
// Roll +++++++++++++++++++++++++++++++++++++++++++++++++
Line 946... Line 972...
946 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
 972 
        cnt = 1;// + labs(IntegralFehlerNick) / 4096;
947 
 
 973 
 
948 
        ausgleichRoll = 0;
 974 
        ausgleichRoll = 0;
949 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT)
 975 
        if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < BEWEGUNGS_LIMIT || (FromNaviCtrl_Value.Kalman_MaxDrift > 3*16))
950 
        {
 976 
        {
951 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
 977 
        if(IntegralFehlerRoll >  FEHLER_LIMIT2)
952 
         {
 978 
         {
Line 973... Line 999...
973 
        } else
 999 
        } else
974 
        {
 1000 
        {
975 
         cnt = 0;
 1001 
         cnt = 0;
976 
         KompassSignalSchlecht = 1000;
 1002 
         KompassSignalSchlecht = 1000;
977 
        }
 1003 
        }
978 
 
 - 
 
979 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
 1004 
        if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp;
- 
 
 1005 
        if(cnt * 16 > FromNaviCtrl_Value.Kalman_MaxDrift) cnt = FromNaviCtrl_Value.Kalman_MaxDrift/16;
980 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
 1006 
        if(IntegralFehlerRoll >  FEHLER_LIMIT)   AdNeutralRoll += cnt;
981 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
 1007 
        if(IntegralFehlerRoll < -FEHLER_LIMIT)   AdNeutralRoll -= cnt;
982 
  }
 1008 
  }
983 
  else
 1009 
  else
984 
  {
 1010 
  {
Line 1091... Line 1117...
1091 
    //DebugOut.Analog[16] = Mittelwert_AccHoch;
 1117 
    //DebugOut.Analog[16] = Mittelwert_AccHoch;
1092 
    DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
 1118 
    DebugOut.Analog[17] = FromNaviCtrl_Value.Distance;
1093 
    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
 1119 
    DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar;
1094 
    DebugOut.Analog[19] = WinkelOut.CalcState;
 1120 
    DebugOut.Analog[19] = WinkelOut.CalcState;
1095 
    DebugOut.Analog[20] = ServoValue;
 1121 
    DebugOut.Analog[20] = ServoValue;
1096 
 
 - 
 
- 
 
 1122 
    DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift;
- 
 
 1123 
    DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K;
1097 
    DebugOut.Analog[30] = GPS_Nick;
 1124 
    DebugOut.Analog[30] = GPS_Nick;
1098 
    DebugOut.Analog[31] = GPS_Roll;
 1125 
    DebugOut.Analog[31] = GPS_Roll;
Line 1099... Line 1126...
1099 
 
 1126