Subversion Repositories FlightCtrl

// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++
      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))
         {
            tmpl3 = (MesswertRoll * winkel_nick) / 2048L;
            tmpl3 *= Parameter_AchsKopplung2; //65
            tmpl3 /= 4096L;
            tmpl4 = (MesswertNick * winkel_roll) / 2048L;
            tmpl4 *= Parameter_AchsKopplung2; //65
            tmpl4 /= 4096L;
            KopplungsteilNickRoll = tmpl3;
            KopplungsteilRollNick = tmpl4;
            tmpl4 -= tmpl3;
            ErsatzKompass += tmpl4;
            if(!Parameter_CouplingYawCorrection) Mess_Integral_Gier -= tmpl4/2; // Gier nachhelfen

            tmpl = ((MesswertGier + tmpl4) * winkel_nick) / 2048L;
            tmpl *= Parameter_AchsKopplung1;  // 90
            tmpl /= 4096L;
            tmpl2 = ((MesswertGier + tmpl4) * winkel_roll) / 2048L;
            tmpl2 *= Parameter_AchsKopplung1;
            tmpl2 /= 4096L;
            if(abs(MesswertGier) > 64) if(labs(tmpl) > 128 || labs(tmpl2) > 128) TrichterFlug = 1;
            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;
         }
      else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;

TrimRoll = tmpl - tmpl2 / 100L;
TrimNick = -tmpl2 + tmpl / 100L;


// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++
            Mess_IntegralRoll2 += MesswertRoll + TrimRoll;
            Mess_IntegralRoll +=  MesswertRoll + TrimRoll - LageKorrekturRoll;
            if(Mess_IntegralRoll > Umschlag180Roll)
            {
             Mess_IntegralRoll  = -(Umschlag180Roll - 25000L);
             Mess_IntegralRoll2 = Mess_IntegralRoll;
            }
            if(Mess_IntegralRoll <-Umschlag180Roll)
            {
             Mess_IntegralRoll =  (Umschlag180Roll - 25000L);
             Mess_IntegralRoll2 = Mess_IntegralRoll;
            }
// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++
            Mess_IntegralNick2 += MesswertNick + TrimNick;
            Mess_IntegralNick  += MesswertNick + TrimNick - LageKorrekturNick;
            if(Mess_IntegralNick > Umschlag180Nick)
             {
              Mess_IntegralNick = -(Umschlag180Nick - 25000L);
              Mess_IntegralNick2 = Mess_IntegralNick;
             }
            if(Mess_IntegralNick <-Umschlag180Nick)
            {
             Mess_IntegralNick =  (Umschlag180Nick - 25000L);
             Mess_IntegralNick2 = Mess_IntegralNick;
            }


..

 if(RohMesswertRoll > 0) TrimRoll  += ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
 else                    TrimRoll -= ((long) abs(KopplungsteilNickRoll) * Parameter_CouplingYawCorrection) / 64L;
 if(RohMesswertNick > 0) TrimNick += ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;
 else                    TrimNick -= ((long) abs(KopplungsteilRollNick) * Parameter_CouplingYawCorrection) / 64L;


#define TRIM_MAX 200
 if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else  if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX;
 if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else  if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX;

    MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / CONTROL_SCALING);
    MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / CONTROL_SCALING);
    MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / CONTROL_SCALING) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / CONTROL_SCALING));

messwertNick er nu P-part....