Subversion Repositories FlightCtrl

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

Flight Control

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

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

// + Copyright (c) 04.2007 Holger Buss

// + Nur für den privaten Gebrauch

// + 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 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 to systems other than 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;

int MesswertNick,MesswertRoll,MesswertGier;

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

int Mittelwert_AccNick, Mittelwert_AccRoll;

long IntegralNick = 0,IntegralNick2 = 0;

long IntegralRoll = 0,IntegralRoll2 = 0;

long IntegralAccNick = 0,IntegralAccRoll = 0;

long Integral_Gier = 0;

long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0;

long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0;

long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0;

long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2;

volatile long Mess_Integral_Hoch = 0;

int KompassValue = 0;

int KompassStartwert = 0;

int KompassRichtung = 0;

uint8_t updKompass;

unsigned char MAX_GAS,MIN_GAS;

unsigned char Notlandung = 0;

unsigned char HoehenReglerAktiv = 0;

long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;

float GyroFaktor;

float IntegralFaktor;

int DiffNick,DiffRoll;

uint8_t Poti1, Poti2, Poti3, Poti4;

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,StickGas = 0;

char MotorenEin = 0;

int HoehenWert = 0;

int SollHoehe = 0;

int LageKorrekturRoll = 0,LageKorrekturNick = 0;

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_UserParam5 = 0;

unsigned char Parameter_UserParam6 = 0;

unsigned char Parameter_UserParam7 = 0;

unsigned char Parameter_UserParam8 = 0;

unsigned char Parameter_ServoNickControl = 100;

unsigned char Parameter_LoopGasLimit = 70;

unsigned char Parameter_AchsKopplung1 = 0;

unsigned char Parameter_AchsGegenKopplung1 = 0;

unsigned char Parameter_DynamicStability = 100;

uint8_t Cam_Travel;

int8_t nick_tilt, roll_tilt;

signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20;

int MaxStickNick = 0,MaxStickRoll = 0;

struct mk_param_struct EE_Parameter;

struct acc_neutral_struct ee_acc_neutral EEMEM;         // Reservierung im EEPROM

struct acc_neutral_struct acc_neutral;

void Piep(unsigned char Anzahl)

{

 while(Anzahl--)

 {

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

  beeptime = 100;

  Delay_ms(250);

 }

}

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

//  Neutrallage kalibrieren und fest im EEPROM abspeichern

void calib_acc(void)

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

{

        acc_neutral.X = 0;

        acc_neutral.Y = 0;

        acc_neutral.Z = 0;

    CalibrierMittelwert();     

    Delay_ms_Mess(100);

        CalibrierMittelwert();

        acc_neutral.X = abs(Mittelwert_AccNick) / ACC_AMPLIFY;

        acc_neutral.Y = abs(Mittelwert_AccRoll) / ACC_AMPLIFY;

        acc_neutral.Z = Aktuell_az;

/*     

        while (PPM_in_gas > 100)        //Warten, bis Benutzer den Kopter neu ausgerichtet hat

        {

                uint8_t delay=9;

                Delay_ms(10);

                if (!delay--)

                {

                        delay = 9;

                        beeptime = 100;

                }

        }

        Delay_ms_Mess(100);

        acc_neutral.C = Aktuell_az;

*/     

        eeprom_write_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));

}

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

//  Nullwerte ermitteln

void SetNeutral(void)

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

{      

        acc_neutral.X = 0;

        acc_neutral.Y = 0;

        acc_neutral.Z = 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;

        eeprom_read_block(&acc_neutral,&ee_acc_neutral,sizeof(struct acc_neutral_struct));

        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;

    beeptime = 50;  

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

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

    ExternHoehenValue = 0;

        Cam_Travel = EE_Parameter.ServoNickMax - EE_Parameter.ServoNickMin;

}

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

// 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;

//DebugOut.Analog[26] = MesswertNick;

//DebugOut.Analog[28] = MesswertRoll;

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

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

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

    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;

    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;

// 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 - LageKorrekturRoll;

            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;

                        #endif

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

            MesswertNick -= tmpl2;

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

            Mess_IntegralNick2 += MesswertNick;

            Mess_IntegralNick  += MesswertNick - LageKorrekturNick;

            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;                        

                        #endif

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

// ADC einschalten

    ANALOG_ON; 

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

    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]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--;

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

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

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

}

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

// 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;

   // ADC einschalten

    ANALOG_ON; 

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

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

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

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

        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];

        }  

    //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_UserParam5,EE_Parameter.UserParam5,0,255);

 CHK_POTI(Parameter_UserParam6,EE_Parameter.UserParam6,0,255);

 CHK_POTI(Parameter_UserParam7,EE_Parameter.UserParam7,0,255);

 CHK_POTI(Parameter_UserParam8,EE_Parameter.UserParam8,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);

 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,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 = 1;

     static long ausgleichNick, ausgleichRoll;

        Mittelwert();

    GRN_ON;

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

// Gaswert ermitteln

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

        GasMischanteil = StickGas;

    if(GasMischanteil < 0) GasMischanteil = 0;

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

// Datenübernahme von RC

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

        int16_t PPM_in_nick, PPM_in_roll, PPM_in_gier, PPM_diff_nick, PPM_diff_roll, PPM_in_gas;

        uint8_t tmp_sreg = SREG;

    cli();  

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

    PPM_diff_nick = PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]];

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

    PPM_diff_roll = PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]];

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

    PPM_in_gas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]];

    SREG = tmp_sreg;

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

// 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_diff_nick = 0;

                        PPM_diff_roll = 0;

                        PPM_in_nick = 0;

                        PPM_in_roll = 0;

                        PPM_in_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((PPM_in_gas > 80) && MotorenEin == 0)

                {

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

// auf Nullwerte kalibrieren

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

                if(PPM_in_gier > 75)  // Neutralwerte

                    {

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

                        {

                        GRN_OFF;

                        MotorenEin = 0;

                        delay_neutral = 0;

                        modell_fliegt = 0;

                        if(PPM_in_nick > 70 || abs(PPM_in_roll) > 70)

                        {

                         unsigned char setting=1;

                         if(PPM_in_roll > 70 && PPM_in_nick < 70) setting = 1;

                         if(PPM_in_roll > 70 && PPM_in_nick > 70) setting = 2;

                         if(PPM_in_roll < 70 && PPM_in_nick > 70) setting = 3;

                         if(PPM_in_roll <-70 && PPM_in_nick > 70) setting = 4;

                         if(PPM_in_roll <-70 && PPM_in_nick < 70) setting = 5;

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

                        }  

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

                        SetNeutral();

                        Piep(GetActiveParamSetNumber());

                        }

                    }

                 else delay_neutral = 0;

                }

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

// Gas ist unten

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

            if(PPM_in_gas < -85)

                {

                // Starten

                if(PPM_in_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_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)  

  {

    int tmp_int;

    ParameterZuordnung();

    StickNick = (StickNick * 3 + PPM_in_nick * EE_Parameter.Stick_P) / 4;

    StickNick += PPM_diff_nick * EE_Parameter.Stick_D;

    StickRoll = (StickRoll * 3 + PPM_in_roll * EE_Parameter.Stick_P) / 4;

    StickRoll += PPM_diff_roll * EE_Parameter.Stick_D;

    StickGier = -PPM_in_gier;

        StickGas  = PPM_in_gas + 120;

   if(abs(PPM_in_nick) > MaxStickNick)

     MaxStickNick = abs(PPM_in_nick); else MaxStickNick--;

   if(abs(PPM_in_roll) > MaxStickRoll)

     MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--;

   if(Notlandung)  {MaxStickNick = 0; MaxStickRoll = 0;}

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

    IntegralFaktor = ((float)Parameter_Gyro_I) / 44000;

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

//+ Digitale Steuerung per DubWise

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

#define KEY_VALUE (Parameter_UserParam1 * 4)  //(Poti3 * 8)

if(DubWiseKeys[1]) beeptime = 10;

    if(DubWiseKeys[1] & DUB_KEY_UP)    tmp_int = KEY_VALUE;   else

    if(DubWiseKeys[1] & DUB_KEY_DOWN)  tmp_int = -KEY_VALUE;  else   tmp_int = 0;

    ExternStickNick = (ExternStickNick * 7 + tmp_int) / 8;

    if(DubWiseKeys[1] & DUB_KEY_LEFT)  tmp_int = KEY_VALUE; else

    if(DubWiseKeys[1] & DUB_KEY_RIGHT) tmp_int = -KEY_VALUE; else tmp_int = 0;

    ExternStickRoll = (ExternStickRoll * 7 + tmp_int) / 8;

    if(DubWiseKeys[0] & 8)  ExternStickGier = 50;else

    if(DubWiseKeys[0] & 4)  ExternStickGier =-50;else ExternStickGier = 0;

    if(DubWiseKeys[0] & 2)  ExternHoehenValue++;

    if(DubWiseKeys[0] & 16) ExternHoehenValue--;

    StickNick += ExternStickNick / 8;

    StickRoll += ExternStickRoll / 8;

    StickGier += ExternStickGier;

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

//+ Analoge Steuerung per Seriell

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

   if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128)

    {

         StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P;

         StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P;

         StickGier += ExternControl.Gier;

     ExternHoehenValue =  (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung;

     if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas;

    }

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

    if(GyroFaktor < 0) GyroFaktor = 0;

    if(IntegralFaktor < 0) IntegralFaktor = 0;

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

// Looping?

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

  if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS)  Looping_Links = 1;

  else

   {

     {

      if((PPM_in_roll < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0;  

     }  

   }

  if((PPM_in_roll < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1;

   else

   {

   if(Looping_Rechts) // Hysterese

     {

      if(PPM_in_roll > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0;

     }

   }

  if((PPM_in_nick > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1;

  else

   {

    if(Looping_Oben)  // Hysterese

     {

      if((PPM_in_nick < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0;  

     }  

   }

  if((PPM_in_nick < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1;

   else

   {

    if(Looping_Unten) // Hysterese

     {

      if(PPM_in_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.5;

     IntegralFaktor = 0.003;

     Looping_Roll = 0;

     Looping_Nick = 0;

    }  

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

// Integrale auf ACC-Signal abgleichen

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

#define ABGLEICH_ANZAHL 256L

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