Subversion Repositories FlightCtrl

/*#######################################################################################

Flight Control

#######################################################################################*/

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) Holger Buss, Ingo Busker

// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY

// + www.MikroKopter.com

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), 

// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. 

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt 

// + bzgl. der Nutzungsbedingungen aufzunehmen. 

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,

// + Verkauf von Luftbildaufnahmen, usw.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, 

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt werden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur 

// + mit unserer Zustimmung zulässig

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Redistributions of source code (with or without modifications) must retain the above copyright notice, 

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet 

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted 

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be 

// +     clearly linked as origin 

// +   * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE. 

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include "main.h"

#include "eeprom.c"

unsigned char h,m,s;

volatile unsigned int I2CTimeout = 100;

volatile int MesswertNick,MesswertRoll,MesswertGier;

volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0,StartNeutralRoll = 0,StartNeutralNick = 0;

volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0;

volatile float NeutralAccZ = 0;

unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;

volatile long IntegralNick = 0,IntegralNick2 = 0;

volatile long IntegralRoll = 0,IntegralRoll2 = 0;

volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0;

volatile long Integral_Gier = 0;

volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;

volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;

volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;

volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;

volatile long Mess_Integral_Hoch = 0;

volatile int  KompassValue = 0;

volatile int  KompassStartwert = 0;

volatile int  KompassRichtung = 0;

unsigned char MAX_GAS,MIN_GAS;

unsigned char Notlandung = 0;

unsigned char HoehenReglerAktiv = 0;

long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;

float GyroFaktor;

float IntegralFaktor;

volatile int  DiffNick,DiffRoll;

int  Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0;

volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count;

unsigned char MotorWert[5];

volatile unsigned char SenderOkay = 0;

int StickNick = 0,StickRoll = 0,StickGier = 0;

char MotorenEin = 0;

int HoehenWert = 0;

int SollHoehe = 0;

int I_LageRoll = 0,I_LageNick = 0;

float Kp =  FAKTOR_P;

float Ki =  FAKTOR_I;

unsigned char Looping_Nick = 0,Looping_Roll = 0;

unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0;

unsigned char Parameter_Luftdruck_D  = 48;      // Wert : 0-250

unsigned char Parameter_MaxHoehe     = 251;      // Wert : 0-250

unsigned char Parameter_Hoehe_P      = 16;      // Wert : 0-32

unsigned char Parameter_Hoehe_ACC_Wirkung = 58; // Wert : 0-250

unsigned char Parameter_KompassWirkung = 64;    // Wert : 0-250

unsigned char Parameter_Gyro_P = 150;           // Wert : 10-250

unsigned char Parameter_Gyro_I = 150;           // Wert : 0-250

unsigned char Parameter_Gier_P = 2;             // Wert : 1-20

unsigned char Parameter_I_Faktor = 10;          // Wert : 1-20

unsigned char Parameter_UserParam1 = 0;

unsigned char Parameter_UserParam2 = 0;

unsigned char Parameter_UserParam3 = 0;

unsigned char Parameter_UserParam4 = 0;

unsigned char Parameter_ServoNickControl = 100;

unsigned char Parameter_LoopGasLimit = 70;

unsigned char Parameter_AchsKopplung1 = 0;

unsigned char Parameter_AchsGegenKopplung1 = 0;

struct mk_param_struct EE_Parameter;

void Piep(unsigned char Anzahl)

{

 while(Anzahl--)

 {

  if(MotorenEin) return; //auf keinen Fall im Flug!

  beeptime = 100;

  Delay_ms(250);

 }

}

//############################################################################

//  Nullwerte ermitteln

void SetNeutral(void)

//############################################################################

{

        NeutralAccX = 0;

        NeutralAccY = 0;

        NeutralAccZ = 0;

    AdNeutralNick = 0; 

        AdNeutralRoll = 0;     

        AdNeutralGier = 0;

    Parameter_AchsKopplung1 = 0;

    Parameter_AchsGegenKopplung1 = 0;

    CalibrierMittelwert();     

    Delay_ms_Mess(100);

        CalibrierMittelwert();

    if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?

     {    

      if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();

     }

     AdNeutralNick= AdWertNick;

         AdNeutralRoll= AdWertRoll;    

         AdNeutralGier= AdWertGier;

     StartNeutralRoll = AdNeutralRoll;

     StartNeutralNick = AdNeutralNick;

    NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;

        NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY;

        NeutralAccZ = Aktuell_az;

        Mess_IntegralNick = 0; 

    Mess_IntegralNick2 = 0;

    Mess_IntegralRoll = 0;     

    Mess_IntegralRoll2 = 0;

    Mess_Integral_Gier = 0;    

    MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

    StartLuftdruck = Luftdruck;

    HoeheD = 0;

    Mess_Integral_Hoch = 0;

    KompassStartwert = KompassValue;

    GPS_Neutral();

    beeptime = 50;  

        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L;

}

//############################################################################

// Bearbeitet die Messwerte

void Mittelwert(void)

//############################################################################

{      

    static signed long tmpl,tmpl2;     

    MesswertGier = (signed int) AdNeutralGier - AdWertGier;

    MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll;

    MesswertNick = (signed int) AdWertNick - AdNeutralNick;

// Beschleunigungssensor  ++++++++++++++++++++++++++++++++++++++++++++++++

        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L;

        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L;

        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L;

    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;

    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;

    IntegralAccZ    += Aktuell_az - 704;//NeutralAccZ;

// Gier  ++++++++++++++++++++++++++++++++++++++++++++++++

            Mess_Integral_Gier +=  MesswertGier;

            Mess_Integral_Gier2 += MesswertGier;

// Kopplungsanteil  +++++++++++++++++++++++++++++++++++++

      if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV))

         {

            tmpl = Mess_IntegralNick / 4096L;

            tmpl *= MesswertGier;

            tmpl *= Parameter_AchsKopplung1;  //125

            tmpl /= 2048L;

            tmpl2 = Mess_IntegralRoll / 4096L;

            tmpl2 *= MesswertGier;

            tmpl2 *= Parameter_AchsKopplung1;

            tmpl2 /= 2048L;

         }

      else  tmpl = tmpl2 = 0;

// Roll  ++++++++++++++++++++++++++++++++++++++++++++++++

            MesswertRoll += tmpl;

            MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109

            Mess_IntegralRoll2 += MesswertRoll;

            Mess_IntegralRoll +=  MesswertRoll - I_LageRoll;

            if(Mess_IntegralRoll > Umschlag180Roll)

            {

             Mess_IntegralRoll  = -(Umschlag180Roll - 10000L);

             Mess_IntegralRoll2 = Mess_IntegralRoll;

            }

            if(Mess_IntegralRoll <-Umschlag180Roll)

            {

             Mess_IntegralRoll =  (Umschlag180Roll - 10000L);

             Mess_IntegralRoll2 = Mess_IntegralRoll;

            }  

            if(AdWertRoll < 15)   MesswertRoll = -1000;

            if(AdWertRoll <  7)   MesswertRoll = -2000;

            if(PlatinenVersion == 10)

                         {

              if(AdWertRoll > 1010) MesswertRoll = +1000;

              if(AdWertRoll > 1017) MesswertRoll = +2000;

                         }

                         else

                         {

              if(AdWertRoll > 2020) MesswertRoll = +1000;

              if(AdWertRoll > 2034) MesswertRoll = +2000;

                         }

// Nick  ++++++++++++++++++++++++++++++++++++++++++++++++

            MesswertNick -= tmpl2;

            MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L;

            Mess_IntegralNick2 += MesswertNick;

            Mess_IntegralNick  += MesswertNick - I_LageNick;

            if(Mess_IntegralNick > Umschlag180Nick)

            {

             Mess_IntegralNick = -(Umschlag180Nick - 10000L);

             Mess_IntegralNick2 = Mess_IntegralNick;

            }

            if(Mess_IntegralNick <-Umschlag180Nick)

            {

             Mess_IntegralNick =  (Umschlag180Nick - 10000L);

             Mess_IntegralNick2 = Mess_IntegralNick;

            }

            if(AdWertNick < 15)   MesswertNick = -1000;

            if(AdWertNick <  7)   MesswertNick = -2000;

            if(PlatinenVersion == 10)

                         {

              if(AdWertNick > 1010) MesswertNick = +1000;

              if(AdWertNick > 1017) MesswertNick = +2000;

                         }

                         else

                         {

              if(AdWertNick > 2020) MesswertNick = +1000;

              if(AdWertNick > 2034) MesswertNick = +2000;

                         }

//++++++++++++++++++++++++++++++++++++++++++++++++

// ADC einschalten

    ANALOG_ON; 

//++++++++++++++++++++++++++++++++++++++++++++++++

DebugOut.Analog[11] = MesswertRoll;

    Integral_Gier  = Mess_Integral_Gier;

    IntegralNick = Mess_IntegralNick;

    IntegralRoll = Mess_IntegralRoll;

    IntegralNick2 = Mess_IntegralNick2;

    IntegralRoll2 = Mess_IntegralRoll2;

  if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll)

  {

    if(MesswertNick > 200)       MesswertNick += 4 * (MesswertNick - 200);

    else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200);

    if(MesswertRoll > 200)       MesswertRoll += 4 * (MesswertRoll - 200);

    else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200);

  }

    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;

    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;

    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;

    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;

    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;

    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;

    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;

    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;

}

//############################################################################

// Messwerte beim Ermitteln der Nullage

void CalibrierMittelwert(void)

//############################################################################

{                

    // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern

        ANALOG_OFF;

        MesswertNick = AdWertNick;

        MesswertRoll = AdWertRoll;

        MesswertGier = AdWertGier;

        Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;

        Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;

        Mittelwert_AccHoch = (long)AdWertAccHoch;

   // ADC einschalten

    ANALOG_ON; 

    if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--;

    if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--;

    if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--;

    if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--;

    if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255;

    if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255;

    if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255;

    if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255;

        Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

        Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L;

}

//############################################################################

// Senden der Motorwerte per I2C-Bus

void SendMotorData(void)

//############################################################################

{

    if(MOTOR_OFF || !MotorenEin)

        {

        Motor_Hinten = 0;

        Motor_Vorne = 0;

        Motor_Rechts = 0;

        Motor_Links = 0;

        if(MotorTest[0]) Motor_Vorne = MotorTest[0];

        if(MotorTest[1]) Motor_Hinten = MotorTest[1];

        if(MotorTest[2]) Motor_Links = MotorTest[2];

        if(MotorTest[3]) Motor_Rechts = MotorTest[3];

        }

    DebugOut.Analog[12] = Motor_Vorne;

    DebugOut.Analog[13] = Motor_Hinten;

    DebugOut.Analog[14] = Motor_Links;

    DebugOut.Analog[15] = Motor_Rechts;  

    //Start I2C Interrupt Mode

    twi_state = 0;

    motor = 0;

    i2c_start();

}

//############################################################################

// Trägt ggf. das Poti als Parameter ein

void ParameterZuordnung(void)

//############################################################################

{

 #define CHK_POTI(b,a,min,max) { if(a > 250) { if(a == 251) b = Poti1; else if(a == 252) b = Poti2; else if(a == 253) b = Poti3; else if(a == 254) b = Poti4;} else b = a; if(b <= min) b = min; else if(b >= max) b = max;}

 CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe,0,255);

 CHK_POTI(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100);

 CHK_POTI(Parameter_Hoehe_P,EE_Parameter.Hoehe_P,0,100);

 CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung,0,255);

 CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung,0,255);

 CHK_POTI(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);

 CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I,0,255);

 CHK_POTI(Parameter_I_Faktor,EE_Parameter.I_Faktor,0,255);

 CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255);

 CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255);

 CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255);

 CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255);

 CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255);

 CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255);

 CHK_POTI(Parameter_AchsKopplung1,    EE_Parameter.AchsKopplung1,0,255);

 CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255);

 Ki = (float) Parameter_I_Faktor * 0.0001;

 MAX_GAS = EE_Parameter.Gas_Max;

 MIN_GAS = EE_Parameter.Gas_Min;

}

//############################################################################

//

void MotorRegler(void)

//############################################################################

{

         int motorwert,pd_ergebnis,h,tmp_int;

         int GierMischanteil,GasMischanteil;

     static long SummeNick=0,SummeRoll=0;

     static long sollGier = 0,tmp_long,tmp_long2;

     static long IntegralFehlerNick = 0;

     static long IntegralFehlerRoll = 0;

         static unsigned int RcLostTimer;

         static unsigned char delay_neutral = 0;

         static unsigned char delay_einschalten = 0,delay_ausschalten = 0;

         static unsigned int  modell_fliegt = 0;

     static int hoehenregler = 0;

     static char TimerWerteausgabe = 0;

     static char NeueKompassRichtungMerken = 0;

     static long ausgleichNick, ausgleichRoll;

        Mittelwert();

    GRN_ON;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Gaswert ermitteln

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

        GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120;

    if(GasMischanteil < 0) GasMischanteil = 0;

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Emfang schlecht

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

   if(SenderOkay < 100)

        {

        if(!PcZugriff)

         {

           if(BeepMuster == 0xffff)

            {

             beeptime = 15000;

             BeepMuster = 0x0c00;

            }

         }

        if(RcLostTimer) RcLostTimer--;

        else

         {

          MotorenEin = 0;

          Notlandung = 0;

         }

        ROT_ON;

        if(modell_fliegt > 2000)  // wahrscheinlich in der Luft --> langsam absenken

            {

            GasMischanteil = EE_Parameter.NotGas;

            Notlandung = 1;

            PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] = 0;

            PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] = 0;

            PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] = 0;

            }

         else MotorenEin = 0;

        }

        else

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Emfang gut

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

        if(SenderOkay > 140)

            {

            Notlandung = 0;

            RcLostTimer = EE_Parameter.NotGasZeit * 50;

            if(GasMischanteil > 40)

                {

                if(modell_fliegt < 0xffff) modell_fliegt++;

                }

            if((modell_fliegt < 200) || (GasMischanteil < 40))

                {

                SummeNick = 0;

                SummeRoll = 0;

                Mess_Integral_Gier = 0;

                Mess_Integral_Gier2 = 0;

                }

            if((GasMischanteil > 200) && MotorenEin == 0)

                {

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// auf Nullwerte kalibrieren

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)  // Neutralwerte

                    {

                    unsigned char setting = 2;

                    if(++delay_neutral > 200)  // nicht sofort

                        {

                        GRN_OFF;

                        MotorenEin = 0;

                        delay_neutral = 0;

                        modell_fliegt = 0;

                        if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70 || abs(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]]) > 70)

                        {

                         unsigned char setting=1;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 1;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 2;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < 70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 3;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > 70) setting = 4;

                         if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] <-70 && PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < 70) setting = 5;

                         eeprom_write_byte(&EEPromArray[EEPROM_ADR_ACTIVE_SET], setting);  // aktiven Datensatz merken

                        }

                        if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG))  // Höhenregelung aktiviert?

                          {

                             if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset();

                          }  

                            ReadParameterSet(GetActiveParamSetNumber(), (unsigned char *) &EE_Parameter.Kanalbelegung[0], STRUCT_PARAM_LAENGE);

                        SetNeutral();

                        Piep(GetActiveParamSetNumber());

                        }

                    }

                 else delay_neutral = 0;

                }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Gas ist unten

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

            if(GasMischanteil < 35)

                {

                // Starten

                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75)

                    {

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Einschalten

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

                    if(++delay_einschalten > 200)

                        {

                        delay_einschalten = 200;

                        modell_fliegt = 1;

                        MotorenEin = 1;

                        sollGier = 0;

                        Mess_Integral_Gier = 0;

                        Mess_Integral_Gier2 = 0;

                        Mess_IntegralNick = 0;

                        Mess_IntegralRoll = 0;

                        Mess_IntegralNick2 = IntegralNick;

                        Mess_IntegralRoll2 = IntegralRoll;

                        SummeNick = 0;

                        SummeRoll = 0;

                        }          

                    }  

                    else delay_einschalten = 0;

                //Auf Neutralwerte setzen

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Auschalten

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

                if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75)

                    {

                    if(++delay_ausschalten > 200)  // nicht sofort

                        {

                        MotorenEin = 0;

                        delay_ausschalten = 200;

                        modell_fliegt = 0;

                        }

                    }

                else delay_ausschalten = 0;

                }

            }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// neue Werte von der Funke

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

 if(!NewPpmData-- || Notlandung)  

  {

    ParameterZuordnung();

    StickNick = PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P;

    StickNick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D;

    StickRoll = PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P;

    StickRoll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D;

    StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]];

    GyroFaktor     = ((float)Parameter_Gyro_P + 10.0) / 256.0;

    IntegralFaktor = ((float) Parameter_Gyro_I) / 44000;

    if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor =  0;

    if(GyroFaktor < 0) GyroFaktor = 0;

    if(IntegralFaktor < 0) IntegralFaktor = 0;

    // greift in den Stick ein, um ungewolltes überschlagen zu verhindern

    if(!(EE_Parameter.LoopConfig & CFG_LOOP_LINKS) && !(EE_Parameter.LoopConfig & CFG_LOOP_RECHTS))

     {

      if(IntegralNick >  60000)  

      {

       StickNick -=  8 * EE_Parameter.Stick_P;      

       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      

      }

      else

      if(IntegralNick < -60000)  

      {

       StickNick += 8 * EE_Parameter.Stick_P;

       if(IntegralNick >  80000) StickNick -= 16 * EE_Parameter.Stick_P;      

      }

      if(IntegralRoll >  60000)  

      {

       StickRoll -=  8 * EE_Parameter.Stick_P;      

       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      

      }

      else

      if(IntegralRoll < -60000)  

      {

       StickRoll += 8 * EE_Parameter.Stick_P;

       if(IntegralRoll >  80000) StickRoll -= 16 * EE_Parameter.Stick_P;      

      }

     }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Looping?

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;

  else

   {

     {

      if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  

     }  

   }

  if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;

   else

   {

   if(Looping_Rechts) // Hysterese

     {

      if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;

     }

   }

  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;

  else

   {

    if(Looping_Oben)  // Hysterese

     {

      if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  

     }  

   }

  if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;

   else

   {

    if(Looping_Unten) // Hysterese

     {

      if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0;

     }

   }

   if(Looping_Links || Looping_Rechts)   Looping_Roll = 1; else Looping_Roll = 0;

   if(Looping_Oben  || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0;

  } // Ende neue Funken-Werte

  if(Looping_Roll) beeptime = 100;

  if(Looping_Roll || Looping_Nick)

   {

    if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit;

   }

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Bei Empfangsausfall im Flug 

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

   if(Notlandung)

    {

     StickGier = 0;

     StickNick = 0;

     StickRoll = 0;

     GyroFaktor  = 0.1;

     IntegralFaktor = 0.005;

     Looping_Roll = 0;

     Looping_Nick = 0;

    }  

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

// Integrale auf ACC-Signal abgleichen

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                

#define ABGLEICH_ANZAHL 256L

 MittelIntegralNick  += IntegralNick;    // Für die Mittelwertbildung aufsummieren

 MittelIntegralRoll  += IntegralRoll;

 MittelIntegralNick2 += IntegralNick2;

 MittelIntegralRoll2 += IntegralRoll2;

 if(Looping_Nick || Looping_Roll)

  {

    IntegralAccNick = 0;

    IntegralAccRoll = 0;

    MittelIntegralNick = 0;

    MittelIntegralRoll = 0;

    MittelIntegralNick2 = 0;

    MittelIntegralRoll2 = 0;

    Mess_IntegralNick2 = Mess_IntegralNick;

    Mess_IntegralRoll2 = Mess_IntegralRoll;

    ZaehlMessungen = 0;

    I_LageNick = 0;

    I_LageRoll = 0;

  }

 if(ZaehlMessungen >= ABGLEICH_ANZAHL)

 {

  static int cnt = 0;

  static char last_n_p,last_n_n,last_r_p,last_r_n;

  static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt;

  if(!Looping_Nick && !Looping_Roll)

  {

    MittelIntegralNick  /= ABGLEICH_ANZAHL;

    MittelIntegralRoll  /= ABGLEICH_ANZAHL;

        IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL;

        IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL;

    IntegralAccZ    = IntegralAccZ / ABGLEICH_ANZAHL;

#define MAX_I 32

// Nick ++++++++++++++++++++++++++++++++++++++++++++++++

    IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick);

    ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich;

    if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000)

     {

       tmp_long = IntegralFehlerNick / 700L;

       if(tmp_long > MAX_I) tmp_long = MAX_I;

       if(tmp_long <-MAX_I) tmp_long =-MAX_I;

       I_LageNick = tmp_long;    

     }

     else

     {

      I_LageNick /= 2;