Subversion Repositories FlightCtrl

Rev

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

Rev 871 Rev 880
Line 77... Line 77...
77 
volatile int  KompassRichtung = 0;
 77 
volatile int  KompassRichtung = 0;
78 
unsigned int  KompassSignalSchlecht = 500;
 78 
unsigned int  KompassSignalSchlecht = 500;
79 
unsigned char  MAX_GAS,MIN_GAS;
 79 
unsigned char  MAX_GAS,MIN_GAS;
80 
unsigned char Notlandung = 0;
 80 
unsigned char Notlandung = 0;
81 
unsigned char HoehenReglerAktiv = 0;
 81 
unsigned char HoehenReglerAktiv = 0;
- 
 
 82 
unsigned char TrichterFlug = 0;
82 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
 83 
long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;
83 
long  ErsatzKompass;
 84 
long  ErsatzKompass;
84 
int   ErsatzKompassInGrad; // Kompasswert in Grad
 85 
int   ErsatzKompassInGrad; // Kompasswert in Grad
85 
int   GierGyroFehler = 0;
 86 
int   GierGyroFehler = 0;
86 
float GyroFaktor;
 87 
float GyroFaktor;
Line 226... Line 227...
226 
                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
 227 
                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag
227 
                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
 228 
                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;
228 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
 229 
// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
229 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
 230 
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
230 
         {
 231 
         {
231 
            tmpl = Mess_IntegralNick / 4096L;
 232 
            tmpl = (MesswertGier * Mess_IntegralNick) / 2048L;
232 
            tmpl *= MesswertGier;
 - 
 
233 
            tmpl *= Parameter_AchsKopplung1;  //125
 233 
            tmpl *= Parameter_AchsKopplung1;  //125
234 
            tmpl /= 2048L;
 234 
            tmpl /= 4096L;
235 
            tmpl2 = Mess_IntegralRoll / 4096L;
 235 
            tmpl2 = (MesswertGier * Mess_IntegralRoll) / 2048L;
236 
            tmpl2 *= MesswertGier;
 - 
 
237 
            tmpl2 *= Parameter_AchsKopplung1;
 236 
            tmpl2 *= Parameter_AchsKopplung1;
238 
            tmpl2 /= 2048L;
 237 
            tmpl2 /= 4096L;
- 
 
 238 
            if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
239 
         }
 239 
         }
240 
      else  tmpl = tmpl2 = 0;
 240 
      else  tmpl = tmpl2 = 0;
241 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
 241 
// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
242 
            MesswertRoll += tmpl;
 242 
            MesswertRoll += tmpl;
243 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
 243 
            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109
Line 791... Line 791...
791 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
 791 
 if(ZaehlMessungen >= ABGLEICH_ANZAHL)
792 
 {
 792 
 {
793 
  static int cnt = 0;
 793 
  static int cnt = 0;
794 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
 794 
  static char last_n_p,last_n_n,last_r_p,last_r_n;
795 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
 795 
  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;
796 
  if(!Looping_Nick && !Looping_Roll)
 796 
  if(!Looping_Nick && !Looping_Roll && !TrichterFlug)
797 
  {
 797 
  {
798 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
 798 
    MittelIntegralNick  /= ABGLEICH_ANZAHL;
799 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
 799 
    MittelIntegralRoll  /= ABGLEICH_ANZAHL;
800 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
 800 
        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;
801 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
 801 
        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;
Line 937... Line 937...
937 
  }
 937 
  }
938 
  else
 938 
  else
939 
  {
 939 
  {
940 
   LageKorrekturRoll = 0;
 940 
   LageKorrekturRoll = 0;
941 
   LageKorrekturNick = 0;
 941 
   LageKorrekturNick = 0;
- 
 
 942 
   TrichterFlug = 0;
942 
  }
 943 
  }
943 
 
 944 
 
944 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
 945 
  if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH
945 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
946 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
946 
   MittelIntegralNick_Alt = MittelIntegralNick;      
 947 
   MittelIntegralNick_Alt = MittelIntegralNick;      
947 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
 948 
   MittelIntegralRoll_Alt = MittelIntegralRoll;      
948 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++    
949 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++