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 or using the software 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,MesswertGierBias, RohMesswertNick,RohMesswertRoll;

int TrimNick, TrimRoll;

int AdNeutralGierBias;

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

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

int NaviAccNick, NaviAccRoll,NaviCntAcc = 0;

volatile float NeutralAccZ = 0;

unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0;

long IntegralNick = 0,IntegralNick2 = 0;

long IntegralRoll = 0,IntegralRoll2 = 0;

long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 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;

unsigned int  KompassSignalSchlecht = 500;

unsigned char  MAX_GAS,MIN_GAS;

unsigned char Notlandung = 0;

unsigned char HoehenReglerAktiv = 0;

unsigned char TrichterFlug = 0;

long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L;

long  ErsatzKompass;

int   ErsatzKompassInGrad; // Kompasswert in Grad

int   GierGyroFehler = 0;

char GyroFaktor;

char IntegralFaktor;

int  DiffNick,DiffRoll;

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

volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links;

volatile unsigned char Motor1, Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8;

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;

int Ki = 10300 / 33;

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_D = 8;             // 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 = 90;

unsigned char Parameter_AchsKopplung2 = 65;

unsigned char Parameter_CouplingYawCorrection = 64;

//unsigned char Parameter_AchsGegenKopplung1 = 0;

unsigned char Parameter_DynamicStability = 100;

unsigned char Parameter_J16Bitmask;             // for the J16 Output

unsigned char Parameter_J16Timing;              // for the J16 Output

unsigned char Parameter_J17Bitmask;             // for the J17 Output

unsigned char Parameter_J17Timing;              // for the J17 Output

unsigned char Parameter_NaviGpsModeControl;     // Parameters for the Naviboard

unsigned char Parameter_NaviGpsGain;

unsigned char Parameter_NaviGpsP;

unsigned char Parameter_NaviGpsI;

unsigned char Parameter_NaviGpsD;

unsigned char Parameter_NaviGpsACC;

unsigned char Parameter_NaviOperatingRadius;

unsigned char Parameter_NaviWindCorrection;

unsigned char Parameter_NaviSpeedCompensation;

unsigned char Parameter_ExternalControl;

struct mk_param_struct EE_Parameter;

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

int MaxStickNick = 0,MaxStickRoll = 0;

unsigned int  modell_fliegt = 0;

unsigned char MikroKopterFlags = 0;

long GIER_GRAD_FAKTOR = 1291;

signed int KopplungsteilNickRoll,KopplungsteilRollNick;

int MotorSmoothing(int neu, int alt)

{

 int motor;

 if(neu > alt) motor = (1*(int)alt + neu) / 2;

 else   motor = neu - (alt - neu)*1;

//if(Poti2 < 20)  return(neu);

 return(motor);

}

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)

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

{

 unsigned char i;

 unsigned int gier_neutral=0, nick_neutral=0, roll_neutral=0;

    ServoActive = 0; HEF4017R_ON;

        NeutralAccX = 0;

        NeutralAccY = 0;

        NeutralAccZ = 0;

    AdNeutralNick = 0;

        AdNeutralRoll = 0;

        AdNeutralGier = 0;

    AdNeutralGierBias = 0;

    Parameter_AchsKopplung1 = 0;

    Parameter_AchsKopplung2 = 0;

    ExpandBaro = 0;

    CalibrierMittelwert();

    Delay_ms_Mess(100);

        CalibrierMittelwert();

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

     {

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

     }

#define NEUTRAL_FILTER 32

    for(i=0; i<NEUTRAL_FILTER; i++)

         {

          Delay_ms_Mess(10);

          gier_neutral += AdWertGier;

          nick_neutral += AdWertNick;

          roll_neutral += AdWertRoll;

         }

     AdNeutralNick= (nick_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);

         AdNeutralRoll= (roll_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER / 8);

         AdNeutralGier= (gier_neutral+NEUTRAL_FILTER/2) / (NEUTRAL_FILTER);

     AdNeutralGierBias = AdNeutralGier;

     StartNeutralRoll = AdNeutralRoll;

     StartNeutralNick = AdNeutralNick;

    if(eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) > 4)

    {

      NeutralAccY = abs(Mittelwert_AccRoll) / (2*ACC_AMPLIFY);

          NeutralAccX = abs(Mittelwert_AccNick) / (2*ACC_AMPLIFY);

          NeutralAccZ = Aktuell_az;

    }

    else

    {

      NeutralAccX = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_NICK+1]);

          NeutralAccY = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_ROLL+1]);

          NeutralAccZ = (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z]) * 256 + (int)eeprom_read_byte(&EEPromArray[EEPROM_ADR_ACC_Z+1]);

    }

    MesswertNick = 0;

    MesswertRoll = 0;

    MesswertGier = 0;

    Delay_ms_Mess(100);

    Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick;

    Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll;

    IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick;

    IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll;

    Mess_IntegralNick2 = IntegralNick;

    Mess_IntegralRoll2 = IntegralRoll;

    Mess_Integral_Gier = 0;

    StartLuftdruck = Luftdruck;

    HoeheD = 0;

    Mess_Integral_Hoch = 0;

    KompassStartwert = KompassValue;

    GPS_Neutral();

    beeptime = 50;

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

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

    ExternHoehenValue = 0;

    ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR;

    GierGyroFehler = 0;

    SendVersionToNavi = 1;

    LED_Init();

    MikroKopterFlags |= FLAG_CALIBRATE;

    FromNaviCtrl_Value.Kalman_K = -1;

    FromNaviCtrl_Value.Kalman_MaxDrift = 0;

    FromNaviCtrl_Value.Kalman_MaxFusion = 32;

    Poti1 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110;

    Poti2 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110;

    Poti3 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110;

    Poti4 = PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110;

    ServoActive = 1;   

    SenderOkay = 100;

}

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

// Bearbeitet die Messwerte

void Mittelwert(void)

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

{

    static signed long tmpl,tmpl2,tmpl3,tmpl4;

        static signed int oldNick, oldRoll, d2Roll, d2Nick;

        signed long winkel_nick, winkel_roll;

        MesswertGier = (signed int) AdNeutralGier - AdWertGier;

//    MesswertGierBias = (signed int) AdNeutralGierBias - AdWertGier;

    MesswertNick = (signed int) AdWertNickFilter / 8;

    MesswertRoll = (signed int) AdWertRollFilter / 8;

    RohMesswertNick = MesswertNick;

    RohMesswertRoll = MesswertRoll;

//DebugOut.Analog[21] = MesswertNick;

//DebugOut.Analog[22] = MesswertRoll;

//DebugOut.Analog[22] = Mess_Integral_Gier;

//DebugOut.Analog[21] = MesswertNick;  

//DebugOut.Analog[22] = MesswertRoll; 

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

        Mittelwert_AccNick = ((long)Mittelwert_AccNick * 3 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 4L;

        Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 3 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 4L;

        Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 3 + ((long)AdWertAccHoch)) / 4L;

    IntegralAccNick += ACC_AMPLIFY * AdWertAccNick;

    IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll;

    NaviAccNick    += AdWertAccNick;

    NaviAccRoll    += AdWertAccRoll;

    NaviCntAcc++;

    IntegralAccZ  += Aktuell_az - NeutralAccZ;

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

// ADC einschalten

    ANALOG_ON;

        AdReady = 0;

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

    if(Mess_IntegralRoll > 93000L) winkel_roll = 93000L;

        else if(Mess_IntegralRoll <-93000L) winkel_roll = -93000L;

        else winkel_roll = Mess_IntegralRoll;

    if(Mess_IntegralNick > 93000L) winkel_nick = 93000L;

        else if(Mess_IntegralNick <-93000L) winkel_nick = -93000L;

        else winkel_nick = Mess_IntegralNick;

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

   Mess_Integral_Gier += MesswertGier;

   ErsatzKompass += MesswertGier;

// 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(labs(tmpl) > 128 || labs(tmpl2) > 128 /* || abs(KopplungsteilNickRoll) > 128 || abs(KopplungsteilRollNick) > 128)*/) TrichterFlug = 1;

            //MesswertGier += (Parameter_CouplingYawCorrection * tmpl4) / 256;

         }

      else  tmpl = tmpl2 = KopplungsteilNickRoll = KopplungsteilRollNick = 0;

TrimRoll = tmpl - tmpl2 / 100L;

TrimNick = -tmpl2 + tmpl / 100L;

// Kompasswert begrenzen  ++++++++++++++++++++++++++++++++++++++++++++++++

                    if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR;  // 360° Umschlag

                    if(ErsatzKompass < 0)                          ErsatzKompass += 360L * GIER_GRAD_FAKTOR;

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

            }

    Integral_Gier  = Mess_Integral_Gier;

    IntegralNick = Mess_IntegralNick;

    IntegralRoll = Mess_IntegralRoll;

    IntegralNick2 = Mess_IntegralNick2;

    IntegralRoll2 = Mess_IntegralRoll2;

#define D_LIMIT 128

   MesswertNick = HiResNick / 8;

   MesswertRoll = HiResRoll / 8;

   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 > 2000) MesswertNick = +1000;  if(AdWertNick > 2015) MesswertNick = +2000; }

   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 > 2000) MesswertRoll = +1000;  if(AdWertRoll > 2015) MesswertRoll = +2000;  }

  if(Parameter_Gyro_D)

  {

   d2Nick = HiResNick - oldNick;

   oldNick = (oldNick + HiResNick)/2;

   if(d2Nick > D_LIMIT) d2Nick = D_LIMIT;

   else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT;

   MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16;

   d2Roll = HiResRoll - oldRoll;

   oldRoll = (oldRoll + HiResRoll)/2;

   if(d2Roll > D_LIMIT) d2Roll = D_LIMIT;

   else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT;

   MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16;

   HiResNick += (d2Nick * (signed int) Parameter_Gyro_D);

   HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D);

  }

 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;

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

  {

    if(RohMesswertNick > 256)       MesswertNick += 1 * (RohMesswertNick - 256);

    else if(RohMesswertNick < -256) MesswertNick += 1 * (RohMesswertNick + 256);

    if(RohMesswertRoll > 256)       MesswertRoll += 1 * (RohMesswertRoll - 256);

    else if(RohMesswertRoll < -256) MesswertRoll += 1 * (RohMesswertRoll + 256);

  }

    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)

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

{

    if(PlatinenVersion == 13) SucheGyroOffset();

    // 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.WinkelUmschlagRoll * 2500L;

}

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

// Senden der Motorwerte per I2C-Bus

void SendMotorData(void)

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

{

    if(!MotorenEin)

        {

#ifndef QUADRO

                Motor1 = 0;Motor2 = 0;Motor3 = 0;Motor4 = 0;Motor5 = 0;Motor6 = 0;Motor7 = 0;Motor8 = 0;

        if(MotorTest[0]) {Motor1 = MotorTest[0]; Motor2 = MotorTest[0];}

        if(MotorTest[3]) {Motor3 = MotorTest[3]; Motor4 = MotorTest[3];}

        if(MotorTest[1]) {Motor5 = MotorTest[1]; Motor6 = MotorTest[1];}

        if(MotorTest[2]) {Motor7 = MotorTest[2]; Motor8 = MotorTest[2];}

#else 

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

#endif

        MikroKopterFlags &= ~(FLAG_MOTOR_RUN | FLAG_FLY);

        } else MikroKopterFlags |= FLAG_MOTOR_RUN;

    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_MM(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;}

 #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; }

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

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

 CHK_POTI_MM(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_MM(Parameter_Gyro_P,EE_Parameter.Gyro_P,10,255);

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

 CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D,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_AchsKopplung2,    EE_Parameter.AchsKopplung2,0,255);

 CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection,0,255);

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

 CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability,0,255);

 CHK_POTI_MM(Parameter_J16Timing,EE_Parameter.J16Timing,1,255);

 CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255);

 CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl,0,255);

 Ki = 10300 / (Parameter_I_Faktor + 1);

 MAX_GAS = EE_Parameter.Gas_Max;

 MIN_GAS = EE_Parameter.Gas_Min;

}

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

//

void MotorRegler(void)

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

{

         int motorwert,pd_ergebnis_nick,pd_ergebnis_roll,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 int hoehenregler = 0;

         static int motorwert1,motorwert2,motorwert3,motorwert4,motorwert5,motorwert6,motorwert7,motorwert8;

     static char TimerWerteausgabe = 0;

     static char NeueKompassRichtungMerken = 0;

     static long ausgleichNick, ausgleichRoll;

     int IntegralNickMalFaktor,IntegralRollMalFaktor;

        Mittelwert();

    GRN_ON;

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

// Gaswert ermitteln

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

        GasMischanteil = StickGas;

    if(GasMischanteil < MIN_GAS + 10) GasMischanteil = MIN_GAS + 10;

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

// Empfang 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 > 1000)  // wahrscheinlich in der Luft --> langsam absenken

            {

            GasMischanteil = EE_Parameter.NotGas;

            Notlandung = 1;

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

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

            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 && MotorenEin)

                {

                if(modell_fliegt < 0xffff) modell_fliegt++;

                }

            if((modell_fliegt < 256))

                {

                SummeNick = 0;

                SummeRoll = 0;

                if(modell_fliegt == 250)

                 {

                  NeueKompassRichtungMerken = 1;

                  sollGier = 0;

                  Mess_Integral_Gier = 0;

//                  Mess_Integral_Gier2 = 0;

                 }

                } else MikroKopterFlags |= FLAG_FLY;

            if((PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] > 80) && MotorenEin == 0)

                {

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

// auf Nullwerte kalibrieren

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

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

                    {

                    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;